[ { "path": ".gitignore", "content": "build\nFAST-LIO/Log/*.png\nFAST-LIO/Log/*.txt\nFAST-LIO/Log/*.csv\nFAST-LIO/Log/*.pdf\n.vscode/c_cpp_properties.json\n.vscode/settings.json\nPCD/*.pcd\n*.pyc\n" }, { "path": "FAST-LIO/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(fast_lio)\n\nSET(CMAKE_BUILD_TYPE \"Debug\")\n\nADD_COMPILE_OPTIONS(-std=c++14 )\nADD_COMPILE_OPTIONS(-std=c++14 )\nset( CMAKE_CXX_FLAGS \"-std=c++14 -O3\" )\n\nadd_definitions(-DROOT_DIR=\\\"${CMAKE_CURRENT_SOURCE_DIR}/\\\")\n\nset(CMAKE_C_FLAGS \"${CMAKE_C_FLAGS} -fexceptions\" )\nset(CMAKE_CXX_STANDARD 14)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset(CMAKE_CXX_EXTENSIONS OFF)\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions\")\n\nmessage(\"Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"(x86)|(X86)|(amd64)|(AMD64)\" )\n include(ProcessorCount)\n ProcessorCount(N)\n message(\"Processer number: ${N}\")\n if(N GREATER 4)\n add_definitions(-DMP_EN)\n add_definitions(-DMP_PROC_NUM=3)\n message(\"core for MP: 3\")\n elseif(N GREATER 3)\n add_definitions(-DMP_EN)\n add_definitions(-DMP_PROC_NUM=2)\n message(\"core for MP: 2\")\n else()\n add_definitions(-DMP_PROC_NUM=1)\n endif()\nelse()\n add_definitions(-DMP_PROC_NUM=1)\nendif()\n\nfind_package(OpenMP QUIET)\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}\")\nset(CMAKE_C_FLAGS \"${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}\")\n\nfind_package(PythonLibs REQUIRED)\nfind_path(MATPLOTLIB_CPP_INCLUDE_DIRS \"matplotlibcpp.h\")\n\nfind_package(catkin REQUIRED COMPONENTS\n geometry_msgs\n nav_msgs\n sensor_msgs\n roscpp\n rospy\n std_msgs\n pcl_ros\n tf\n livox_ros_driver\n message_generation\n eigen_conversions\n)\n\nfind_package(Eigen3 REQUIRED)\nfind_package(PCL 1.8 REQUIRED)\n\nmessage(Eigen: ${EIGEN3_INCLUDE_DIR})\n\ninclude_directories(\n\t${catkin_INCLUDE_DIRS} \n ${EIGEN3_INCLUDE_DIR}\n ${PCL_INCLUDE_DIRS}\n ${PYTHON_INCLUDE_DIRS}\n include)\n\nadd_message_files(\n FILES\n Pose6D.msg\n)\n\ngenerate_messages(\n DEPENDENCIES\n geometry_msgs\n)\n\ncatkin_package(\n CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime\n DEPENDS EIGEN3 PCL\n INCLUDE_DIRS\n)\n\nadd_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)\ntarget_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})\ntarget_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})" }, { "path": "FAST-LIO/LICENSE", "content": " GNU GENERAL PUBLIC LICENSE\n Version 2, June 1991\n\n Copyright (C) 1989, 1991 Free Software Foundation, Inc.,\n 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n Preamble\n\n The licenses for most software are designed to take away your\nfreedom to share and change it. By contrast, the GNU General Public\nLicense is intended to guarantee your freedom to share and change free\nsoftware--to make sure the software is free for all its users. This\nGeneral Public License applies to most of the Free Software\nFoundation's software and to any other program whose authors commit to\nusing it. (Some other Free Software Foundation software is covered by\nthe GNU Lesser General Public License instead.) You can apply it to\nyour programs, too.\n\n When we speak of free software, we are referring to freedom, not\nprice. Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthis service if you wish), that you receive source code or can get it\nif you want it, that you can change the software or use pieces of it\nin new free programs; and that you know you can do these things.\n\n To protect your rights, we need to make restrictions that forbid\nanyone to deny you these rights or to ask you to surrender the rights.\nThese restrictions translate to certain responsibilities for you if you\ndistribute copies of the software, or if you modify it.\n\n For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must give the recipients all the rights that\nyou have. You must make sure that they, too, receive or can get the\nsource code. And you must show them these terms so they know their\nrights.\n\n We protect your rights with two steps: (1) copyright the software, and\n(2) offer you this license which gives you legal permission to copy,\ndistribute and/or modify the software.\n\n Also, for each author's protection and ours, we want to make certain\nthat everyone understands that there is no warranty for this free\nsoftware. If the software is modified by someone else and passed on, we\nwant its recipients to know that what they have is not the original, so\nthat any problems introduced by others will not reflect on the original\nauthors' reputations.\n\n Finally, any free program is threatened constantly by software\npatents. We wish to avoid the danger that redistributors of a free\nprogram will individually obtain patent licenses, in effect making the\nprogram proprietary. To prevent this, we have made it clear that any\npatent must be licensed for everyone's free use or not licensed at all.\n\n The precise terms and conditions for copying, distribution and\nmodification follow.\n\n GNU GENERAL PUBLIC LICENSE\n TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\n\n 0. This License applies to any program or other work which contains\na notice placed by the copyright holder saying it may be distributed\nunder the terms of this General Public License. The \"Program\", below,\nrefers to any such program or work, and a \"work based on the Program\"\nmeans either the Program or any derivative work under copyright law:\nthat is to say, a work containing the Program or a portion of it,\neither verbatim or with modifications and/or translated into another\nlanguage. (Hereinafter, translation is included without limitation in\nthe term \"modification\".) Each licensee is addressed as \"you\".\n\nActivities other than copying, distribution and modification are not\ncovered by this License; they are outside its scope. The act of\nrunning the Program is not restricted, and the output from the Program\nis covered only if its contents constitute a work based on the\nProgram (independent of having been made by running the Program).\nWhether that is true depends on what the Program does.\n\n 1. You may copy and distribute verbatim copies of the Program's\nsource code as you receive it, in any medium, provided that you\nconspicuously and appropriately publish on each copy an appropriate\ncopyright notice and disclaimer of warranty; keep intact all the\nnotices that refer to this License and to the absence of any warranty;\nand give any other recipients of the Program a copy of this License\nalong with the Program.\n\nYou may charge a fee for the physical act of transferring a copy, and\nyou may at your option offer warranty protection in exchange for a fee.\n\n 2. You may modify your copy or copies of the Program or any portion\nof it, thus forming a work based on the Program, and copy and\ndistribute such modifications or work under the terms of Section 1\nabove, provided that you also meet all of these conditions:\n\n a) You must cause the modified files to carry prominent notices\n stating that you changed the files and the date of any change.\n\n b) You must cause any work that you distribute or publish, that in\n whole or in part contains or is derived from the Program or any\n part thereof, to be licensed as a whole at no charge to all third\n parties under the terms of this License.\n\n c) If the modified program normally reads commands interactively\n when run, you must cause it, when started running for such\n interactive use in the most ordinary way, to print or display an\n announcement including an appropriate copyright notice and a\n notice that there is no warranty (or else, saying that you provide\n a warranty) and that users may redistribute the program under\n these conditions, and telling the user how to view a copy of this\n License. (Exception: if the Program itself is interactive but\n does not normally print such an announcement, your work based on\n the Program is not required to print an announcement.)\n\nThese requirements apply to the modified work as a whole. If\nidentifiable sections of that work are not derived from the Program,\nand can be reasonably considered independent and separate works in\nthemselves, then this License, and its terms, do not apply to those\nsections when you distribute them as separate works. But when you\ndistribute the same sections as part of a whole which is a work based\non the Program, the distribution of the whole must be on the terms of\nthis License, whose permissions for other licensees extend to the\nentire whole, and thus to each and every part regardless of who wrote it.\n\nThus, it is not the intent of this section to claim rights or contest\nyour rights to work written entirely by you; rather, the intent is to\nexercise the right to control the distribution of derivative or\ncollective works based on the Program.\n\nIn addition, mere aggregation of another work not based on the Program\nwith the Program (or with a work based on the Program) on a volume of\na storage or distribution medium does not bring the other work under\nthe scope of this License.\n\n 3. You may copy and distribute the Program (or a work based on it,\nunder Section 2) in object code or executable form under the terms of\nSections 1 and 2 above provided that you also do one of the following:\n\n a) Accompany it with the complete corresponding machine-readable\n source code, which must be distributed under the terms of Sections\n 1 and 2 above on a medium customarily used for software interchange; or,\n\n b) Accompany it with a written offer, valid for at least three\n years, to give any third party, for a charge no more than your\n cost of physically performing source distribution, a complete\n machine-readable copy of the corresponding source code, to be\n distributed under the terms of Sections 1 and 2 above on a medium\n customarily used for software interchange; or,\n\n c) Accompany it with the information you received as to the offer\n to distribute corresponding source code. (This alternative is\n allowed only for noncommercial distribution and only if you\n received the program in object code or executable form with such\n an offer, in accord with Subsection b above.)\n\nThe source code for a work means the preferred form of the work for\nmaking modifications to it. For an executable work, complete source\ncode means all the source code for all modules it contains, plus any\nassociated interface definition files, plus the scripts used to\ncontrol compilation and installation of the executable. However, as a\nspecial exception, the source code distributed need not include\nanything that is normally distributed (in either source or binary\nform) with the major components (compiler, kernel, and so on) of the\noperating system on which the executable runs, unless that component\nitself accompanies the executable.\n\nIf distribution of executable or object code is made by offering\naccess to copy from a designated place, then offering equivalent\naccess to copy the source code from the same place counts as\ndistribution of the source code, even though third parties are not\ncompelled to copy the source along with the object code.\n\n 4. You may not copy, modify, sublicense, or distribute the Program\nexcept as expressly provided under this License. Any attempt\notherwise to copy, modify, sublicense or distribute the Program is\nvoid, and will automatically terminate your rights under this License.\nHowever, parties who have received copies, or rights, from you under\nthis License will not have their licenses terminated so long as such\nparties remain in full compliance.\n\n 5. You are not required to accept this License, since you have not\nsigned it. However, nothing else grants you permission to modify or\ndistribute the Program or its derivative works. These actions are\nprohibited by law if you do not accept this License. Therefore, by\nmodifying or distributing the Program (or any work based on the\nProgram), you indicate your acceptance of this License to do so, and\nall its terms and conditions for copying, distributing or modifying\nthe Program or works based on it.\n\n 6. Each time you redistribute the Program (or any work based on the\nProgram), the recipient automatically receives a license from the\noriginal licensor to copy, distribute or modify the Program subject to\nthese terms and conditions. You may not impose any further\nrestrictions on the recipients' exercise of the rights granted herein.\nYou are not responsible for enforcing compliance by third parties to\nthis License.\n\n 7. If, as a consequence of a court judgment or allegation of patent\ninfringement or for any other reason (not limited to patent issues),\nconditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License. If you cannot\ndistribute so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you\nmay not distribute the Program at all. For example, if a patent\nlicense would not permit royalty-free redistribution of the Program by\nall those who receive copies directly or indirectly through you, then\nthe only way you could satisfy both it and this License would be to\nrefrain entirely from distribution of the Program.\n\nIf any portion of this section is held invalid or unenforceable under\nany particular circumstance, the balance of the section is intended to\napply and the section as a whole is intended to apply in other\ncircumstances.\n\nIt is not the purpose of this section to induce you to infringe any\npatents or other property right claims or to contest validity of any\nsuch claims; this section has the sole purpose of protecting the\nintegrity of the free software distribution system, which is\nimplemented by public license practices. Many people have made\ngenerous contributions to the wide range of software distributed\nthrough that system in reliance on consistent application of that\nsystem; it is up to the author/donor to decide if he or she is willing\nto distribute software through any other system and a licensee cannot\nimpose that choice.\n\nThis section is intended to make thoroughly clear what is believed to\nbe a consequence of the rest of this License.\n\n 8. If the distribution and/or use of the Program is restricted in\ncertain countries either by patents or by copyrighted interfaces, the\noriginal copyright holder who places the Program under this License\nmay add an explicit geographical distribution limitation excluding\nthose countries, so that distribution is permitted only in or among\ncountries not thus excluded. In such case, this License incorporates\nthe limitation as if written in the body of this License.\n\n 9. The Free Software Foundation may publish revised and/or new versions\nof the General Public License from time to time. Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\nEach version is given a distinguishing version number. If the Program\nspecifies a version number of this License which applies to it and \"any\nlater version\", you have the option of following the terms and conditions\neither of that version or of any later version published by the Free\nSoftware Foundation. If the Program does not specify a version number of\nthis License, you may choose any version ever published by the Free Software\nFoundation.\n\n 10. If you wish to incorporate parts of the Program into other free\nprograms whose distribution conditions are different, write to the author\nto ask for permission. For software which is copyrighted by the Free\nSoftware Foundation, write to the Free Software Foundation; we sometimes\nmake exceptions for this. Our decision will be guided by the two goals\nof preserving the free status of all derivatives of our free software and\nof promoting the sharing and reuse of software generally.\n\n NO WARRANTY\n\n 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY\nFOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN\nOTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES\nPROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED\nOR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF\nMERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS\nTO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE\nPROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,\nREPAIR OR CORRECTION.\n\n 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR\nREDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,\nINCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING\nOUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED\nTO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY\nYOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER\nPROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGES.\n\n END OF TERMS AND CONDITIONS\n\n How to Apply These Terms to Your New Programs\n\n If you develop a new program, and you want it to be of the greatest\npossible use to the public, the best way to achieve this is to make it\nfree software which everyone can redistribute and change under these terms.\n\n To do so, attach the following notices to the program. It is safest\nto attach them to the start of each source file to most effectively\nconvey the exclusion of warranty; and each file should have at least\nthe \"copyright\" line and a pointer to where the full notice is found.\n\n \n Copyright (C) \n\n This program is free software; you can redistribute it and/or modify\n it under the terms of the GNU General Public License as published by\n the Free Software Foundation; either version 2 of the License, or\n (at your option) any later version.\n\n This program is distributed in the hope that it will be useful,\n but WITHOUT ANY WARRANTY; without even the implied warranty of\n MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n GNU General Public License for more details.\n\n You should have received a copy of the GNU General Public License along\n with this program; if not, write to the Free Software Foundation, Inc.,\n 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.\n\nAlso add information on how to contact you by electronic and paper mail.\n\nIf the program is interactive, make it output a short notice like this\nwhen it starts in an interactive mode:\n\n Gnomovision version 69, Copyright (C) year name of author\n Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.\n This is free software, and you are welcome to redistribute it\n under certain conditions; type `show c' for details.\n\nThe hypothetical commands `show w' and `show c' should show the appropriate\nparts of the General Public License. Of course, the commands you use may\nbe called something other than `show w' and `show c'; they could even be\nmouse-clicks or menu items--whatever suits your program.\n\nYou should also get your employer (if you work as a programmer) or your\nschool, if any, to sign a \"copyright disclaimer\" for the program, if\nnecessary. Here is a sample; alter the names:\n\n Yoyodyne, Inc., hereby disclaims all copyright interest in the program\n `Gnomovision' (which makes passes at compilers) written by James Hacker.\n\n , 1 April 1989\n Ty Coon, President of Vice\n\nThis General Public License does not permit incorporating your program into\nproprietary programs. If your program is a subroutine library, you may\nconsider it more useful to permit linking proprietary applications with the\nlibrary. If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License.\n" }, { "path": "FAST-LIO/Log/fast_lio_time_log_analysis.m", "content": "clear\nclose all\n\nColor_red = [0.6350 0.0780 0.1840];\nColor_blue = [0 0.4470 0.7410];\nColor_orange = [0.8500 0.3250 0.0980];\nColor_green = [0.4660 0.6740 0.1880];\nColor_lightblue = [0.3010 0.7450 0.9330];\nColor_purple = [0.4940 0.1840 0.5560];\nColor_yellow = [0.9290 0.6940 0.1250];\n\nfast_lio_ikdtree = csvread(\"./fast_lio_time_log.csv\",1,0);\ntimestamp_ikd = fast_lio_ikdtree(:,1);\ntimestamp_ikd = timestamp_ikd - min(timestamp_ikd);\ntotal_time_ikd = fast_lio_ikdtree(:,2)*1e3;\nscan_num = fast_lio_ikdtree(:,3);\nincremental_time_ikd = fast_lio_ikdtree(:,4)*1e3;\nsearch_time_ikd = fast_lio_ikdtree(:,5)*1e3;\ndelete_size_ikd = fast_lio_ikdtree(:,6);\ndelete_time_ikd = fast_lio_ikdtree(:,7) * 1e3;\ntree_size_ikd_st = fast_lio_ikdtree(:,8);\ntree_size_ikd = fast_lio_ikdtree(:,9);\nadd_points = fast_lio_ikdtree(:,10);\n\nfast_lio_forest = csvread(\"fast_lio_time_log.csv\",1,0);\nfov_check_time_forest = fast_lio_forest(:,5)*1e3;\naverage_time_forest = fast_lio_forest(:,2)*1e3;\ntotal_time_forest = fast_lio_forest(:,6)*1e3;\nincremental_time_forest = fast_lio_forest(:,3)*1e3;\nsearch_time_forest = fast_lio_forest(:,4)*1e3;\ntimestamp_forest = fast_lio_forest(:,1);\n\n% Use slide window to calculate average\nL = 1; % Length of slide window\nfor i = 1:length(timestamp_ikd)\n if (i 0);\nsearch_time_ikd = search_time_ikd(index_ikd);\nindex_forest = find(search_time_forest > 0);\nsearch_time_forest = search_time_forest(index_forest);\n\nt = nexttile;\nhold on;\nboxplot_data_ikd = [incremental_time_ikd,total_time_ikd];\nboxplot_data_forest = [incremental_time_forest,total_time_forest];\nColors_ikd = [Color_blue;Color_blue;Color_blue];\nColors_forest = [Color_orange;Color_orange;Color_orange];\n% xticks([3,8,13])\nh_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3);\nh_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3);\nh_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3);\nh_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3);\nax2 = gca;\nax2.YAxis.Scale = 'log';\nxlim([0.5,6.0])\nylim([0.0008,100])\nxticks([1.25 3.25 5.25])\nxticklabels({'Nearest Search',' Incremental Updates','Total Time'});\nyticks([1e-3,1e-2,1e-1,1e0,1e1,1e2])\nax2.YAxis.FontSize = 12;\nax2.XAxis.FontSize = 14.5;\n% ax.XAxis.FontWeight = 'bold';\nylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman')\nbox_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')];\nfor j=1:length(box_vars)\n if (j<=3)\n Color = Color_blue;\n else\n Color = Color_orange;\n end\n patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color);\nend\nLg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman');\ngrid on\nset(gca,'YMinorGrid','off')\nnexttile;\nhold on;\ngrid on;\nbox on;\nset(gca,'FontSize',12,'FontName','Times New Roman') \nplot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2);\nplot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2);\nplot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2);\nlg = legend(\"\\alpha_{bal}\", \"\\alpha_{del}\",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman')\ntitle(\"Re-balancing Criterion\",'FontSize',16,'FontName','Times New Roman')\nxlabel(\"time/s\",'FontSize',16,'FontName','Times New Roman')\nyl = ylabel(\"\\alpha\",'FontSize',15, 'Position',[285.7 0.4250 -1])\nxlim([32,390]);\nylim([0,0.85]);\nax3 = gca;\nax3.YAxis.FontSize = 12;\nax3.XAxis.FontSize = 12;\n% print('./Figures/fastlio_exp_combine','-depsc','-r1200')\n% exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector')\n\n" }, { "path": "FAST-LIO/Log/guide.md", "content": "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.\n" }, { "path": "FAST-LIO/Log/plot.py", "content": "# import matplotlib\n# matplotlib.use('Agg')\nimport numpy as np\nimport matplotlib.pyplot as plt\n\n\n#######for ikfom\nfig, axs = plt.subplots(4,2)\nlab_pre = ['', 'pre-x', 'pre-y', 'pre-z']\nlab_out = ['', 'out-x', 'out-y', 'out-z']\nplot_ind = range(7,10)\na_pre=np.loadtxt('mat_pre.txt')\na_out=np.loadtxt('mat_out.txt')\ntime=a_pre[:,0]\naxs[0,0].set_title('Attitude')\naxs[1,0].set_title('Translation')\naxs[2,0].set_title('Extrins-R')\naxs[3,0].set_title('Extrins-T')\naxs[0,1].set_title('Velocity')\naxs[1,1].set_title('bg')\naxs[2,1].set_title('ba')\naxs[3,1].set_title('Gravity')\nfor i in range(1,4):\n for j in range(8):\n axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i])\n axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])\nfor j in range(8):\n # axs[j].set_xlim(386,389)\n axs[j%4, j/4].grid()\n axs[j%4, j/4].legend()\nplt.grid()\n#######for ikfom#######\n\n\n#### Draw IMU data\nfig, axs = plt.subplots(2)\nimu=np.loadtxt('imu.txt')\ntime=imu[:,0]\naxs[0].set_title('Gyroscope')\naxs[1].set_title('Accelerameter')\nlab_1 = ['gyr-x', 'gyr-y', 'gyr-z']\nlab_2 = ['acc-x', 'acc-y', 'acc-z']\nfor i in range(3):\n # if i==1:\n axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])\n axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])\nfor i in range(2):\n # axs[i].set_xlim(386,389)\n axs[i].grid()\n axs[i].legend()\nplt.grid()\n\n# #### Draw time calculation\n# plt.figure(3)\n# fig = plt.figure()\n# font1 = {'family' : 'Times New Roman',\n# 'weight' : 'normal',\n# 'size' : 12,\n# }\n# c=\"red\"\n# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')\n# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')\n# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')\n# # n = a_out[:,1].size\n# # time_mean = a_out[:,1].mean()\n# # time_se = a_out[:,1].std() / np.sqrt(n)\n# # time_err = a_out[:,1] - time_mean\n# # feat_mean = a_out[:,2].mean()\n# # feat_err = a_out[:,2] - feat_mean\n# # feat_se = a_out[:,2].std() / np.sqrt(n)\n# ax1 = fig.add_subplot(111)\n# ax1.set_ylabel('Effective Feature Numbers',font1)\n# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])\n# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])\n# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])\n# ax1.set_ylim([0, 3000])\n\n# ax2 = ax1.twinx()\n# ax2.spines['right'].set_color('red')\n# ax2.set_ylabel('Compute Time (ms)',font1)\n# ax2.yaxis.label.set_color('red')\n# ax2.tick_params(axis='y', colors='red')\n# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))\n# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))\n# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))\n# ax2.set_xlim([0.5, 3.5])\n# ax2.set_ylim([0, 100])\n\n# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))\n# # # print(time_se)\n# # # print(a_out3[:,2])\n# plt.grid()\n# plt.savefig(\"time.pdf\", dpi=1200)\nplt.show()\n" }, { "path": "FAST-LIO/PCD/1", "content": "1\n" }, { "path": "FAST-LIO/README.md", "content": "## Related Works\n\n1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search.\n2. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter.\n3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning.\n4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.\n5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.\n6. [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.\n\n## FAST-LIO\n**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:\n1. Fast iterated Kalman filter for odometry optimization;\n2. Automaticaly initialized at most steady environments;\n3. Parallel KD-Tree Search to decrease the computation;\n\n## FAST-LIO 2.0 (2021-07-05 Update)\n\n\n
\n\n\n
\n\n**Related video:** [FAST-LIO2](https://youtu.be/2OvjGnxszf8), [FAST-LIO1](https://youtu.be/iYCY6T79oNU), [FAST-LIO1 + Scan-context loop closure](https://youtu.be/Fw9S6D6HozA)\n\n**Pipeline:**\n
\n\n
\n\n**New Features:**\n1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate.\n2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.\n3. 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.\n4. Support external IMU.\n5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM).\n\n**Related papers**: \n\n[FAST-LIO2: Fast Direct LiDAR-inertial Odometry](doc/Fast_LIO_2.pdf)\n\n[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196)\n\n**Contributors**\n\n[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)\n\n\n\n## 1. Prerequisites\n### 1.1 **Ubuntu** and **ROS**\n**Ubuntu >= 16.04**\n\nFor **Ubuntu 18.04 or higher**, the **default** PCL and Eigen is enough for FAST-LIO to work normally.\n\nROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)\n\n### 1.2. **PCL && Eigen**\nPCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).\n\nEigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).\n\n### 1.3. **livox_ros_driver**\nFollow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).\n\n*Remarks:*\n- 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.\n- 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).\n\n\n## 2. Build\nClone the repository and catkin_make:\n\n```\n cd ~/$A_ROS_DIR$/src\n git clone https://github.com/hku-mars/FAST_LIO.git\n cd FAST_LIO\n git submodule update --init\n cd ../..\n catkin_make\n source devel/setup.bash\n```\n- Remember to source the livox_ros_driver before build (follow 1.3 **livox_ros_driver**)\n- If you want to use a custom build of PCL, add the following line to ~/.bashrc\n```export PCL_ROOT={CUSTOM_PCL_PATH}```\n## 3. Directly run\n\nPlease make sure the IMU and LiDAR are **Synchronized**, that's important.\n### 3.1 For Avia\nConnect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then\n```\n cd ~/$FAST_LIO_ROS_DIR$\n source devel/setup.bash\n roslaunch fast_lio mapping_avia.launch\n roslaunch livox_ros_driver livox_lidar_msg.launch\n```\n- 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.\n- 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.\n\n### 3.2 For Livox serials with external IMU\n\nmapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:\n\nEdit ``` config/avia.yaml ``` to set the below parameters:\n\n1. LiDAR point cloud topic name: ``` lid_topic ```\n2. IMU topic name: ``` imu_topic ```\n3. Translational extrinsic: ``` extrinsic_T ```\n4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)\n- 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.\n- 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.\n\n### 3.3 For Velodyne or Ouster (Velodyne as an example)\n\nStep A: Setup before run\n\nEdit ``` config/velodyne.yaml ``` to set the below parameters:\n\n1. LiDAR point cloud topic name: ``` lid_topic ```\n2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)\n3. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ```\n4. Translational extrinsic: ``` extrinsic_T ```\n5. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)\n- 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).\n\nStep B: Run below\n```\n cd ~/$FAST_LIO_ROS_DIR$\n source devel/setup.bash\n roslaunch fast_lio mapping_velodyne.launch\n```\n\nStep C: Run LiDAR's ros driver or play rosbag.\n\n*Remarks:*\n- We will produce some velodyne datasets which is already transfered to Rosbags, please wait for a while.\n\n### 3.4 PCD file save\n\nSet ``` 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.\n\n*Tips for pcl_viewer:*\n- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. \n```\n 1 is all random\n 2 is X values\n 3 is Y values\n 4 is Z values\n 5 is intensity\n```\n\n## 4. Rosbag Example\n### 4.1 Indoor rosbag (Livox Avia LiDAR)\n\n
\n\nDownload [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\n```\nroslaunch fast_lio mapping_avia.launch\nrosbag play YOUR_DOWNLOADED.bag\n\n```\n\n### 4.2 Outdoor rosbag (Livox Avia LiDAR)\n\n
\n\n\n\nDownload [avia_hku_main building_mapping](https://drive.google.com/file/d/1GSb9eLQuwqmgI3VWSB5ApEUhOCFG_Sv5/view?usp=sharing) and then\n```\nroslaunch fast_lio mapping_avia.launch\nrosbag play YOUR_DOWNLOADED.bag\n```\n\n## 5.Implementation on UAV\nIn 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.\n\nThe main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future.\n\n
\n \n \n
\n\n## 6.Acknowledgments\n\nThanks 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).\n" }, { "path": "FAST-LIO/config/avia.yaml", "content": "common:\n lid_topic: \"/livox/lidar\"\n imu_topic: \"/livox/imu\"\n time_sync_en: false # ONLY turn on when external time synchronization is really not possible\n\npreprocess:\n lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, \n scan_line: 6\n blind: 4\n\nmapping:\n acc_cov: 0.1\n gyr_cov: 0.1\n b_acc_cov: 0.0001\n b_gyr_cov: 0.0001\n fov_degree: 70\n det_range: 260.0\n extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]\n # extrinsic_T: [ 0.0, 0.00, -0.0284 ]\n extrinsic_R: [ 1, 0, 0,\n 0, 1, 0,\n 0, 0, 1]\n\npublish: \n scan_publish_en: true # 'false' will close all the point cloud output\n dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame" }, { "path": "FAST-LIO/config/horizon.yaml", "content": "common:\n lid_topic: \"/livox/lidar\"\n imu_topic: \"/livox/imu\"\n time_sync_en: false # ONLY turn on when external time synchronization is really not possible\n \npreprocess:\n lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, \n scan_line: 6\n blind: 4\n\nmapping:\n acc_cov: 0.1\n gyr_cov: 0.1\n b_acc_cov: 0.0001\n b_gyr_cov: 0.0001\n fov_degree: 100\n det_range: 260.0\n extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]\n extrinsic_R: [ 1, 0, 0,\n 0, 1, 0,\n 0, 0, 1]\n\npublish: \n scan_publish_en: true # 'false' will close all the point cloud output\n dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame" }, { "path": "FAST-LIO/config/ouster64.yaml", "content": "common:\n lid_topic: \"/os_cloud_node/points\"\n imu_topic: \"/os_cloud_node/imu\"\n time_sync_en: false # ONLY turn on when external time synchronization is really not possible\n \npreprocess:\n lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, \n scan_line: 64\n blind: 4\n\nmapping:\n acc_cov: 0.1\n gyr_cov: 0.1\n b_acc_cov: 0.0001\n b_gyr_cov: 0.0001\n fov_degree: 180\n det_range: 150.0\n extrinsic_T: [ 0.0, 0.0, 0.0 ]\n extrinsic_R: [1, 0, 0,\n 0, 1, 0,\n 0, 0, 1]\n\npublish: \n scan_publish_en: true # 'false' will close all the point cloud output\n dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame" }, { "path": "FAST-LIO/config/ouster64_mulran.yaml", "content": "common:\n lid_topic: \"/os1_points\"\n imu_topic: \"/imu/data_raw\"\n time_sync_en: false # ONLY turn on when external time synchronization is really not possible\n\npreprocess:\n lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,\n scan_line: 64\n blind: 4\n\nmapping:\n acc_cov: 0.1\n gyr_cov: 0.1\n b_acc_cov: 0.0001\n b_gyr_cov: 0.0001\n fov_degree: 180\n det_range: 150.0\n extrinsic_T: [ 1.77, 0.0, -0.05 ]\n extrinsic_R: [-1, 0, 0,\n 0, -1, 0,\n 0, 0, 1]\n\npublish:\n scan_publish_en: true # 'false' will close all the point cloud output\n dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame\n" }, { "path": "FAST-LIO/config/velodyne.yaml", "content": "common:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/imu/data\"\n time_sync_en: false # ONLY turn on when external time synchronization is really not possible\n \npreprocess:\n lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, \n scan_line: 32\n blind: 4\n\nmapping:\n acc_cov: 0.1\n gyr_cov: 0.1\n b_acc_cov: 0.0001\n b_gyr_cov: 0.0001\n fov_degree: 180\n det_range: 100.0\n extrinsic_T: [ 0, 0, 0.28]\n extrinsic_R: [ 1, 0, 0, \n 0, 1, 0, \n 0, 0, 1]\n\npublish: \n scan_publish_en: true # 'false' will close all the point cloud output\n dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame" }, { "path": "FAST-LIO/include/Exp_mat.h", "content": "#ifndef EXP_MAT_H\n#define EXP_MAT_H\n\n#include \n#include \n#include \n// #include \n\n#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0\n\ntemplate\nEigen::Matrix Exp(const Eigen::Matrix &&ang)\n{\n T ang_norm = ang.norm();\n Eigen::Matrix Eye3 = Eigen::Matrix::Identity();\n if (ang_norm > 0.0000001)\n {\n Eigen::Matrix r_axis = ang / ang_norm;\n Eigen::Matrix K;\n K << SKEW_SYM_MATRX(r_axis);\n /// Roderigous Tranformation\n return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;\n }\n else\n {\n return Eye3;\n }\n}\n\ntemplate\nEigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt)\n{\n T ang_vel_norm = ang_vel.norm();\n Eigen::Matrix Eye3 = Eigen::Matrix::Identity();\n\n if (ang_vel_norm > 0.0000001)\n {\n Eigen::Matrix r_axis = ang_vel / ang_vel_norm;\n Eigen::Matrix K;\n\n K << SKEW_SYM_MATRX(r_axis);\n\n T r_ang = ang_vel_norm * dt;\n\n /// Roderigous Tranformation\n return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;\n }\n else\n {\n return Eye3;\n }\n}\n\ntemplate\nEigen::Matrix Exp(const T &v1, const T &v2, const T &v3)\n{\n T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);\n Eigen::Matrix Eye3 = Eigen::Matrix::Identity();\n if (norm > 0.00001)\n {\n T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};\n Eigen::Matrix K;\n K << SKEW_SYM_MATRX(r_ang);\n\n /// Roderigous Tranformation\n return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;\n }\n else\n {\n return Eye3;\n }\n}\n\n/* Logrithm of a Rotation Matrix */\ntemplate\nEigen::Matrix Log(const Eigen::Matrix &R)\n{\n T &&theta = std::acos(0.5 * (R.trace() - 1));\n Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));\n return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);\n}\n\n// template\n// cv::Mat Exp(const T &v1, const T &v2, const T &v3)\n// {\n \n// T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);\n// cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F);\n// if (norm > 0.0000001)\n// {\n// T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};\n// cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang));\n\n// /// Roderigous Tranformation\n// return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;\n// }\n// else\n// {\n// return Eye3;\n// }\n// }\n\n#endif\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp", "content": "/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Author: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef ESEKFOM_EKF_HPP\n#define ESEKFOM_EKF_HPP\n\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \"../mtk/types/vect.hpp\"\n#include \"../mtk/types/SOn.hpp\"\n#include \"../mtk/types/S2.hpp\"\n#include \"../mtk/startIdx.hpp\"\n#include \"../mtk/build_manifold.hpp\"\n#include \"util.hpp\"\n\n//#define USE_sparse\n\nnamespace esekfom\n{\n\n\tusing namespace Eigen;\n\n\t// used for iterated error state EKF update\n\t// 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.\n\t// applied for measurement as a manifold.\n\ttemplate \n\tstruct share_datastruct\n\t{\n\t\tbool valid;\n\t\tbool converge;\n\t\tM z;\n\t\tEigen::Matrix h_v;\n\t\tEigen::Matrix h_x;\n\t\tEigen::Matrix R;\n\t};\n\n\t// used for iterated error state EKF update\n\t// 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.\n\t// applied for measurement as an Eigen matrix whose dimension is changing\n\ttemplate \n\tstruct dyn_share_datastruct\n\t{\n\t\tbool valid;\n\t\tbool converge;\n\t\tEigen::Matrix z;\n\t\tEigen::Matrix h;\n\t\tEigen::Matrix h_v;\n\t\tEigen::Matrix h_x;\n\t\tEigen::Matrix R;\n\t};\n\n\t// used for iterated error state EKF update\n\t// 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.\n\t// applied for measurement as a dynamic manifold whose dimension or type is changing\n\ttemplate \n\tstruct dyn_runtime_share_datastruct\n\t{\n\t\tbool valid;\n\t\tbool converge;\n\t\t// Z z;\n\t\tEigen::Matrix h_v;\n\t\tEigen::Matrix h_x;\n\t\tEigen::Matrix R;\n\t};\n\t//状态量，噪声维度，输入状态量这三个参数输入\n\ttemplate \n\tclass esekf\n\t{\n\n\t\ttypedef esekf self;\n\t\tenum\n\t\t{\n\t\t\tn = state::DOF,\t\t //状态量自由度，一般代表x的维度\n\t\t\tm = state::DIM,\t\t //状态量自由度，一般代表res的维度\n\t\t\tl = measurement::DOF //测量噪声维度\n\t\t};\n\n\tpublic:\n\t\ttypedef typename state::scalar scalar_type;\n\t\ttypedef Matrix cov;\n\t\ttypedef Matrix cov_;\n\t\ttypedef SparseMatrix spMt;\n\t\ttypedef Matrix vectorized_state;\n\t\ttypedef Matrix flatted_state;\n\t\ttypedef flatted_state processModel(state &, const input &); //声明一个函数指针\n\t\ttypedef Eigen::Matrix processMatrix1(state &, const input &);\n\t\ttypedef Eigen::Matrix processMatrix2(state &, const input &);\n\t\ttypedef Eigen::Matrix processnoisecovariance;\n\t\ttypedef measurement measurementModel(state &, bool &);\n\t\ttypedef measurement measurementModel_share(state &, share_datastruct &);\n\t\ttypedef Eigen::Matrix measurementModel_dyn(state &, bool &);\n\t\t// typedef Eigen::Matrix measurementModel_dyn_share(state &, dyn_share_datastruct &);\n\t\ttypedef void measurementModel_dyn_share(state &, dyn_share_datastruct &);\n\t\ttypedef Eigen::Matrix measurementMatrix1(state &, bool &);\n\t\ttypedef Eigen::Matrix measurementMatrix1_dyn(state &, bool &);\n\t\ttypedef Eigen::Matrix measurementMatrix2(state &, bool &);\n\t\ttypedef Eigen::Matrix measurementMatrix2_dyn(state &, bool &);\n\t\ttypedef Eigen::Matrix measurementnoisecovariance;\n\t\ttypedef Eigen::Matrix measurementnoisecovariance_dyn;\n\n\t\tesekf(const state &x = state(),\n\t\t\t const cov &P = cov::Identity()) : x_(x), P_(P) // esekf初始化，主要是初始化了x_和P_\n\t\t{\n#ifdef USE_sparse\n\t\t\tSparseMatrix ref(n, n);\n\t\t\tref.setIdentity();\n\t\t\tl_ = ref;\n\t\t\tf_x_2 = ref;\n\t\t\tf_x_1 = ref;\n#endif\n\t\t};\n\n\t\t// receive system-specific models and their differentions.\n\t\t// for measurement as a manifold.\n\t\tvoid 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])\n\t\t{\n\t\t\tf = f_in;\n\t\t\tf_x = f_x_in;\n\t\t\tf_w = f_w_in;\n\t\t\th = h_in;\n\t\t\th_x = h_x_in;\n\t\t\th_v = h_v_in;\n\n\t\t\tmaximum_iter = maximum_iteration;\n\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t{\n\t\t\t\tlimit[i] = limit_vector[i];\n\t\t\t}\n\n\t\t\tx_.build_S2_state();\n\t\t\tx_.build_SO3_state();\n\t\t\tx_.build_vect_state();\n\t\t}\n\n\t\t// receive system-specific models and their differentions.\n\t\t// for measurement as an Eigen matrix whose dimention is chaing.\n\t\tvoid 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])\n\t\t{\n\t\t\tf = f_in;\n\t\t\tf_x = f_x_in;\n\t\t\tf_w = f_w_in;\n\t\t\th_dyn = h_in;\n\t\t\th_x_dyn = h_x_in;\n\t\t\th_v_dyn = h_v_in;\n\n\t\t\tmaximum_iter = maximum_iteration;\n\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t{\n\t\t\t\tlimit[i] = limit_vector[i];\n\t\t\t}\n\n\t\t\tx_.build_S2_state();\n\t\t\tx_.build_SO3_state();\n\t\t\tx_.build_vect_state();\n\t\t}\n\n\t\t// receive system-specific models and their differentions.\n\t\t// for measurement as a dynamic manifold whose dimension or type is changing.\n\t\tvoid 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])\n\t\t{\n\t\t\tf = f_in;\n\t\t\tf_x = f_x_in;\n\t\t\tf_w = f_w_in;\n\t\t\th_x_dyn = h_x_in;\n\t\t\th_v_dyn = h_v_in;\n\n\t\t\tmaximum_iter = maximum_iteration;\n\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t{\n\t\t\t\tlimit[i] = limit_vector[i];\n\t\t\t}\n\n\t\t\tx_.build_S2_state();\n\t\t\tx_.build_SO3_state();\n\t\t\tx_.build_vect_state();\n\t\t}\n\n\t\t// receive system-specific models and their differentions\n\t\t// for measurement as a manifold.\n\t\t// 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).\n\t\tvoid 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])\n\t\t{\n\t\t\tf = f_in;\n\t\t\tf_x = f_x_in;\n\t\t\tf_w = f_w_in;\n\t\t\th_share = h_share_in;\n\n\t\t\tmaximum_iter = maximum_iteration;\n\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t{\n\t\t\t\tlimit[i] = limit_vector[i];\n\t\t\t}\n\n\t\t\tx_.build_S2_state();\n\t\t\tx_.build_SO3_state();\n\t\t\tx_.build_vect_state();\n\t\t}\n\n\t\t//接收系统特定的模型及其差异\n\t\t//作为特征矩阵的测量，其维数是变化的。\n\t\t//通过一个函数(h_dyn_share_in)完成了测量(z)，估计测量(h)，偏微分矩阵(h_x, h_v)和噪声协方差(R)的同时计算。\n\t\tvoid 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])\n\t\t{\n\t\t\tf = f_in;\n\t\t\tf_x = f_x_in;\n\t\t\tf_w = f_w_in;\n\t\t\th_dyn_share = h_dyn_share_in;\n\n\t\t\tmaximum_iter = maximum_iteration;\n\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t{\n\t\t\t\tlimit[i] = limit_vector[i];\n\t\t\t}\n\n\t\t\tx_.build_S2_state();\n\t\t\tx_.build_SO3_state();\n\t\t\tx_.build_vect_state();\n\t\t}\n\n\t\t// receive system-specific models and their differentions\n\t\t// for measurement as a dynamic manifold whose dimension or type is changing.\n\t\t// 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).\n\t\t// for any scenarios where it is needed\n\t\tvoid init_dyn_runtime_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, int maximum_iteration, scalar_type limit_vector[n])\n\t\t{\n\t\t\tf = f_in;\n\t\t\tf_x = f_x_in;\n\t\t\tf_w = f_w_in;\n\n\t\t\tmaximum_iter = maximum_iteration;\n\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t{\n\t\t\t\tlimit[i] = limit_vector[i];\n\t\t\t}\n\n\t\t\tx_.build_S2_state();\n\t\t\tx_.build_SO3_state();\n\t\t\tx_.build_vect_state();\n\t\t}\n\n\t\t// 迭代误差状态EKF传播\n\t\tvoid predict(double &dt, processnoisecovariance &Q, const input &i_in) //这里的参数均从use-ikfom中传入\n\t\t{\n\t\t\t// f函数对应use-ikfom.hpp中的 get_f函数，对应fast_lio2论文公式(2)\n\t\t\tflatted_state f_ = f(x_, i_in); // m*1\n\t\t\t// 对应use-ikfom.hpp中的 df_dx函数\n\t\t\t// 对应fast_lio2论文公式(7)\n\t\t\tcov_ f_x_ = f_x(x_, i_in); // m*n\n\t\t\tcov f_x_final;\t\t\t // n*n\n\t\t\t// 对应use-ikfom.hpp中的 df_dw函数\n\t\t\t// 对应fast_lio2论文公式(7)\n\t\t\tMatrix f_w_ = f_w(x_, i_in);\n\t\t\tMatrix f_w_final;\n\t\t\tstate x_before = x_; //保存x_的值，用于后面的更新\n\n\t\t\t// 对应fast_lio2论文公式(2)\n\t\t\tx_.oplus(f_, dt); //这个是公式2的整个函数，用于更新x的\n\n\t\t\tF_x1 = cov::Identity(); //状态转移矩阵\n\t\t\t// 更新f_x和f_w\n\t\t\tfor (std::vector, int>>::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++)\n\t\t\t{\n\t\t\t\tint idx = (*it).first.first; //状态变量的索引\n\t\t\t\tint dim = (*it).first.second; //状态变量的维数\n\t\t\t\tint dof = (*it).second;\t\t //状态变量的自由度\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int j = 0; j < dof; j++)\n\t\t\t\t\t{\n\t\t\t\t\t\tf_x_final(idx + j, i) = f_x_(dim + j, i); //更新f_x_final，形成n*n阵，用于更新\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\tfor (int i = 0; i < process_noise_dof; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int j = 0; j < dof; j++)\n\t\t\t\t\t{\n\t\t\t\t\t\tf_w_final(idx + j, i) = f_w_(dim + j, i); //更新f_w_final，形成n*dof，用于更新\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\t\t\tMatrix res_temp_SO3;\n\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t{\n\t\t\t\tint idx = (*it).first;\t//状态变量的索引\n\t\t\t\tint dim = (*it).second; //状态变量的维数\n\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t{\n\t\t\t\t\tseg_SO3(i) = -1 * f_(dim + i) * dt; //拿到S03更新值\n\t\t\t\t}\n\t\t\t\tMTK::SO3 res;\t\t\t\t\t\t\t\t\t\t\t\t\t//残差计算\n\t\t\t\tres.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(1 / 2)); //残差计算\n#ifdef USE_sparse\n\t\t\t\tres_temp_SO3 = res.toRotationMatrix();\n\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int j = 0; j < 3; j++)\n\t\t\t\t\t{\n\t\t\t\t\t\tf_x_1.coeffRef(idx + i, idx + j) = res_temp_SO3(i, j);\n\t\t\t\t\t}\n\t\t\t\t}\n#else\n\t\t\t\tF_x1.template block<3, 3>(idx, idx) = res.toRotationMatrix(); //更新f_x_1\n#endif\n\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3); //转为矩阵形式\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tf_x_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_.template block<3, 1>(dim, i)); // F矩阵\n\t\t\t\t}\n\t\t\t\tfor (int i = 0; i < process_noise_dof; i++)\n\t\t\t\t{\n\t\t\t\t\tf_w_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_.template block<3, 1>(dim, i));\n\t\t\t\t}\n\t\t\t}\n\n\t\t\tMatrix res_temp_S2;\n\t\t\tMatrix res_temp_S2_;\n\t\t\tMTK::vect<3, scalar_type> seg_S2;\n\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t{\n\t\t\t\tint idx = (*it).first;\n\t\t\t\tint dim = (*it).second;\n\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t{\n\t\t\t\t\tseg_S2(i) = f_(dim + i) * dt;\n\t\t\t\t}\n\t\t\t\tMTK::vect<2, scalar_type> vec = MTK::vect<2, scalar_type>::Zero();\n\t\t\t\tMTK::SO3 res;\n\t\t\t\tres.w() = MTK::exp(res.vec(), seg_S2, scalar_type(1 / 2));\n\t\t\t\tEigen::Matrix Nx;\n\t\t\t\tEigen::Matrix Mx;\n\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\tx_before.S2_Mx(Mx, vec, idx);\n#ifdef USE_sparse\n\t\t\t\tres_temp_S2_ = Nx * res.toRotationMatrix() * Mx;\n\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int j = 0; j < 2; j++)\n\t\t\t\t\t{\n\t\t\t\t\t\tf_x_1.coeffRef(idx + i, idx + j) = res_temp_S2_(i, j);\n\t\t\t\t\t}\n\t\t\t\t}\n#else\n\t\t\t\tF_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx;\n#endif\n\n\t\t\t\tEigen::Matrix x_before_hat;\n\t\t\t\tx_before.S2_hat(x_before_hat, idx);\n\t\t\t\tres_temp_S2 = -Nx * res.toRotationMatrix() * x_before_hat * MTK::A_matrix(seg_S2).transpose();\n\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tf_x_final.template block<2, 1>(idx, i) = res_temp_S2 * (f_x_.template block<3, 1>(dim, i));\n\t\t\t\t}\n\t\t\t\tfor (int i = 0; i < process_noise_dof; i++)\n\t\t\t\t{\n\t\t\t\t\tf_w_final.template block<2, 1>(idx, i) = res_temp_S2 * (f_w_.template block<3, 1>(dim, i));\n\t\t\t\t}\n\t\t\t}\n\n#ifdef USE_sparse\n\t\t\tf_x_1.makeCompressed();\n\t\t\tspMt f_x2 = f_x_final.sparseView();\n\t\t\tspMt f_w1 = f_w_final.sparseView();\n\t\t\tspMt xp = f_x_1 + f_x2 * dt;\n\t\t\tP_ = xp * P_ * xp.transpose() + (f_w1 * dt) * Q * (f_w1 * dt).transpose();\n#else\n\t\t\tF_x1 += f_x_final * dt;\n\t\t\tP_ = (F_x1)*P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose();\n#endif\n\t\t}\n\n\t\t// iterated error state EKF update for measurement as a manifold.\n\t\tvoid update_iterated(measurement &z, measurementnoisecovariance &R)\n\t\t{\n\n\t\t\tif (!(is_same()))\n\t\t\t{\n\t\t\t\tstd::cerr << \"the scalar type of measurment must be the same as the state\" << std::endl;\n\t\t\t\tstd::exit(100);\n\t\t\t}\n\t\t\tint t = 0;\n\t\t\tbool converg = true;\n\t\t\tbool valid = true;\n\t\t\tstate x_propagated = x_;\n\t\t\tcov P_propagated = P_;\n\n\t\t\tfor (int i = -1; i < maximum_iter; i++)\n\t\t\t{\n\t\t\t\tvectorized_state dx, dx_new;\n\t\t\t\tx_.boxminus(dx, x_propagated);\n\t\t\t\tdx_new = dx;\n#ifdef USE_sparse\n\t\t\t\tspMt h_x_ = h_x(x_, valid).sparseView();\n\t\t\t\tspMt h_v_ = h_v(x_, valid).sparseView();\n\t\t\t\tspMt R_ = R.sparseView();\n#else\n\t\t\t\tMatrix h_x_ = h_x(x_, valid);\n\t\t\t\tMatrix h_v_ = h_v(x_, valid);\n#endif\n\t\t\t\tif (!valid)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\n\t\t\t\tP_ = P_propagated;\n\n\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_SO3(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tres_temp_SO3 = A_matrix(seg_SO3).transpose();\n\t\t\t\t\tdx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix res_temp_S2;\n\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_S2(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\tdx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix K_;\n\t\t\t\tif (n > l)\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tMatrix K_temp = h_x_ * P_ * h_x_.transpose();\n\t\t\t\t\tspMt R_temp = h_v_ * R_ * h_v_.transpose();\n\t\t\t\t\tK_temp += R_temp;\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * K_temp.inverse();\n#else\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();\n#endif\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tmeasurementnoisecovariance b = measurementnoisecovariance::Identity();\n\t\t\t\t\tEigen::SparseQR, Eigen::COLAMDOrdering> solver;\n\t\t\t\t\tsolver.compute(R_);\n\t\t\t\t\tmeasurementnoisecovariance R_in_temp = solver.solve(b);\n\t\t\t\t\tspMt R_in = R_in_temp.sparseView();\n\t\t\t\t\tspMt K_temp = h_x_.transpose() * R_in * h_x_;\n\t\t\t\t\tcov P_temp = P_.inverse();\n\t\t\t\t\tP_temp += K_temp;\n\t\t\t\t\tK_ = P_temp.inverse() * h_x_.transpose() * R_in;\n#else\n\t\t\t\t\tmeasurementnoisecovariance R_in = (h_v_ * R * h_v_.transpose()).inverse();\n\t\t\t\t\tK_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;\n#endif\n\t\t\t\t}\n\t\t\t\tMatrix innovation;\n\t\t\t\tz.boxminus(innovation, h(x_, valid));\n\t\t\t\tcov K_x = K_ * h_x_;\n\t\t\t\tMatrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new;\n\t\t\t\tstate x_before = x_;\n\t\t\t\tx_.boxplus(dx_);\n\n\t\t\t\tconverg = true;\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tif (std::fabs(dx_[i]) > limit[i])\n\t\t\t\t\t{\n\t\t\t\t\t\tconverg = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tif (converg)\n\t\t\t\t\tt++;\n\n\t\t\t\tif (t > 1 || i == maximum_iter - 1)\n\t\t\t\t{\n\t\t\t\t\tL_ = P_;\n\n\t\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_SO3(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\t\t\t\t\t\tres_temp_SO3 = A_matrix(seg_SO3).transpose();\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > l)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < l; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\tMatrix res_temp_S2;\n\t\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\n\t\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_S2(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\n\t\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > l)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < l; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t\tif (n > l)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_ * h_x_ * P_;\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\treturn;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\n\t\t// iterated error state EKF update for measurement as a manifold.\n\t\t// 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.\n\t\tvoid update_iterated_share()\n\t\t{\n\n\t\t\tif (!(is_same()))\n\t\t\t{\n\t\t\t\tstd::cerr << \"the scalar type of measurment must be the same as the state\" << std::endl;\n\t\t\t\tstd::exit(100);\n\t\t\t}\n\n\t\t\tint t = 0;\n\t\t\tshare_datastruct _share;\n\t\t\t_share.valid = true;\n\t\t\t_share.converge = true;\n\t\t\tstate x_propagated = x_;\n\t\t\tcov P_propagated = P_;\n\n\t\t\tfor (int i = -1; i < maximum_iter; i++)\n\t\t\t{\n\t\t\t\tvectorized_state dx, dx_new;\n\t\t\t\tx_.boxminus(dx, x_propagated);\n\t\t\t\tdx_new = dx;\n\t\t\t\tmeasurement h = h_share(x_, _share);\n\t\t\t\tmeasurement z = _share.z;\n\t\t\t\tmeasurementnoisecovariance R = _share.R;\n#ifdef USE_sparse\n\t\t\t\tspMt h_x_ = _share.h_x.sparseView();\n\t\t\t\tspMt h_v_ = _share.h_v.sparseView();\n\t\t\t\tspMt R_ = _share.R.sparseView();\n#else\n\t\t\t\tMatrix h_x_ = _share.h_x;\n\t\t\t\tMatrix h_v_ = _share.h_v;\n#endif\n\t\t\t\tif (!_share.valid)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\n\t\t\t\tP_ = P_propagated;\n\n\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_SO3(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tres_temp_SO3 = A_matrix(seg_SO3).transpose();\n\t\t\t\t\tdx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix res_temp_S2;\n\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_S2(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\tdx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix K_;\n\t\t\t\tif (n > l)\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tMatrix K_temp = h_x_ * P_ * h_x_.transpose();\n\t\t\t\t\tspMt R_temp = h_v_ * R_ * h_v_.transpose();\n\t\t\t\t\tK_temp += R_temp;\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * K_temp.inverse();\n#else\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();\n#endif\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tmeasurementnoisecovariance b = measurementnoisecovariance::Identity();\n\t\t\t\t\tEigen::SparseQR, Eigen::COLAMDOrdering> solver;\n\t\t\t\t\tsolver.compute(R_);\n\t\t\t\t\tmeasurementnoisecovariance R_in_temp = solver.solve(b);\n\t\t\t\t\tspMt R_in = R_in_temp.sparseView();\n\t\t\t\t\tspMt K_temp = h_x_.transpose() * R_in * h_x_;\n\t\t\t\t\tcov P_temp = P_.inverse();\n\t\t\t\t\tP_temp += K_temp;\n\t\t\t\t\tK_ = P_temp.inverse() * h_x_.transpose() * R_in;\n#else\n\t\t\t\t\tmeasurementnoisecovariance R_in = (h_v_ * R * h_v_.transpose()).inverse();\n\t\t\t\t\tK_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;\n#endif\n\t\t\t\t}\n\t\t\t\tMatrix innovation;\n\t\t\t\tz.boxminus(innovation, h);\n\t\t\t\tcov K_x = K_ * h_x_;\n\t\t\t\tMatrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new;\n\t\t\t\tstate x_before = x_;\n\t\t\t\tx_.boxplus(dx_);\n\n\t\t\t\t_share.converge = true;\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tif (std::fabs(dx_[i]) > limit[i])\n\t\t\t\t\t{\n\t\t\t\t\t\t_share.converge = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tif (_share.converge)\n\t\t\t\t\tt++;\n\n\t\t\t\tif (t > 1 || i == maximum_iter - 1)\n\t\t\t\t{\n\t\t\t\t\tL_ = P_;\n\n\t\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_SO3(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\t\t\t\t\t\tres_temp_SO3 = A_matrix(seg_SO3).transpose();\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > l)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < l; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\tMatrix res_temp_S2;\n\t\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\n\t\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_S2(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\n\t\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > l)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < l; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t\tif (n > l)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_ * h_x_ * P_;\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\treturn;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\n\t\t// iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing.\n\t\tvoid update_iterated_dyn(Eigen::Matrix z, measurementnoisecovariance_dyn R)\n\t\t{\n\n\t\t\tint t = 0;\n\t\t\tbool valid = true;\n\t\t\tbool converg = true;\n\t\t\tstate x_propagated = x_;\n\t\t\tcov P_propagated = P_;\n\t\t\tint dof_Measurement;\n\t\t\tint dof_Measurement_noise = R.rows();\n\t\t\tfor (int i = -1; i < maximum_iter; i++)\n\t\t\t{\n\t\t\t\tvalid = true;\n#ifdef USE_sparse\n\t\t\t\tspMt h_x_ = h_x_dyn(x_, valid).sparseView();\n\t\t\t\tspMt h_v_ = h_v_dyn(x_, valid).sparseView();\n\t\t\t\tspMt R_ = R.sparseView();\n#else\n\t\t\t\tMatrix h_x_ = h_x_dyn(x_, valid);\n\t\t\t\tMatrix h_v_ = h_v_dyn(x_, valid);\n#endif\n\t\t\t\tMatrix h_ = h_dyn(x_, valid);\n\t\t\t\tdof_Measurement = h_.rows();\n\t\t\t\tvectorized_state dx, dx_new;\n\t\t\t\tx_.boxminus(dx, x_propagated);\n\t\t\t\tdx_new = dx;\n\t\t\t\tif (!valid)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\n\t\t\t\tP_ = P_propagated;\n\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_SO3(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\tdx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix res_temp_S2;\n\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_S2(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\tdx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix K_;\n\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tMatrix K_temp = h_x_ * P_ * h_x_.transpose();\n\t\t\t\t\tspMt R_temp = h_v_ * R_ * h_v_.transpose();\n\t\t\t\t\tK_temp += R_temp;\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * K_temp.inverse();\n#else\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();\n#endif\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tEigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise);\n\t\t\t\t\tEigen::SparseQR, Eigen::COLAMDOrdering> solver;\n\t\t\t\t\tsolver.compute(R_);\n\t\t\t\t\tEigen::Matrix R_in_temp = solver.solve(b);\n\t\t\t\t\tspMt R_in = R_in_temp.sparseView();\n\t\t\t\t\tspMt K_temp = h_x_.transpose() * R_in * h_x_;\n\t\t\t\t\tcov P_temp = P_.inverse();\n\t\t\t\t\tP_temp += K_temp;\n\t\t\t\t\tK_ = P_temp.inverse() * h_x_.transpose() * R_in;\n#else\n\t\t\t\t\tEigen::Matrix R_in = (h_v_ * R * h_v_.transpose()).inverse();\n\t\t\t\t\tK_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;\n#endif\n\t\t\t\t}\n\t\t\t\tcov K_x = K_ * h_x_;\n\t\t\t\tMatrix dx_ = K_ * (z - h_) + (K_x - Matrix::Identity()) * dx_new;\n\t\t\t\tstate x_before = x_;\n\t\t\t\tx_.boxplus(dx_);\n\t\t\t\tconverg = true;\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tif (std::fabs(dx_[i]) > limit[i])\n\t\t\t\t\t{\n\t\t\t\t\t\tconverg = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\tif (converg)\n\t\t\t\t\tt++;\n\t\t\t\tif (t > 1 || i == maximum_iter - 1)\n\t\t\t\t{\n\t\t\t\t\tL_ = P_;\n\t\t\t\t\tstd::cout << \"iteration time:\" << t << \",\" << i << std::endl;\n\n\t\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_SO3(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\t\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\tMatrix res_temp_S2;\n\t\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\n\t\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_S2(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\n\t\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_ * h_x_ * P_;\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\treturn;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\t\t// iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing.\n\t\t// 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.\n\t\tvoid update_iterated_dyn_share()\n\t\t{\n\n\t\t\tint t = 0;\n\t\t\tdyn_share_datastruct dyn_share;\n\t\t\tdyn_share.valid = true;\n\t\t\tdyn_share.converge = true;\n\t\t\tstate x_propagated = x_;\n\t\t\tcov P_propagated = P_;\n\t\t\tint dof_Measurement;\n\t\t\tint dof_Measurement_noise;\n\t\t\tfor (int i = -1; i < maximum_iter; i++)\n\t\t\t{\n\t\t\t\tdyn_share.valid = true;\n\t\t\t\th_dyn_share(x_, dyn_share);\n\t\t\t\t// Matrix h = h_dyn_share (x_, dyn_share);\n\t\t\t\tMatrix z = dyn_share.z;\n\t\t\t\tMatrix h = dyn_share.h;\n#ifdef USE_sparse\n\t\t\t\tspMt h_x = dyn_share.h_x.sparseView();\n\t\t\t\tspMt h_v = dyn_share.h_v.sparseView();\n\t\t\t\tspMt R_ = dyn_share.R.sparseView();\n#else\n\t\t\t\tMatrix R = dyn_share.R;\n\t\t\t\tMatrix h_x = dyn_share.h_x;\n\t\t\t\tMatrix h_v = dyn_share.h_v;\n#endif\n\t\t\t\tdof_Measurement = h_x.rows();\n\t\t\t\tdof_Measurement_noise = dyn_share.R.rows();\n\t\t\t\tvectorized_state dx, dx_new;\n\t\t\t\tx_.boxminus(dx, x_propagated);\n\t\t\t\tdx_new = dx;\n\t\t\t\tif (!(dyn_share.valid))\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\n\t\t\t\tP_ = P_propagated;\n\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_SO3(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\tdx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix res_temp_S2;\n\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_S2(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\tdx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix K_;\n\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tMatrix K_temp = h_x * P_ * h_x.transpose();\n\t\t\t\t\tspMt R_temp = h_v * R_ * h_v.transpose();\n\t\t\t\t\tK_temp += R_temp;\n\t\t\t\t\tK_ = P_ * h_x.transpose() * K_temp.inverse();\n#else\n\t\t\t\t\tK_ = P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();\n#endif\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tEigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise);\n\t\t\t\t\tEigen::SparseQR, Eigen::COLAMDOrdering> solver;\n\t\t\t\t\tsolver.compute(R_);\n\t\t\t\t\tEigen::Matrix R_in_temp = solver.solve(b);\n\t\t\t\t\tspMt R_in = R_in_temp.sparseView();\n\t\t\t\t\tspMt K_temp = h_x.transpose() * R_in * h_x;\n\t\t\t\t\tcov P_temp = P_.inverse();\n\t\t\t\t\tP_temp += K_temp;\n\t\t\t\t\tK_ = P_temp.inverse() * h_x.transpose() * R_in;\n#else\n\t\t\t\t\tEigen::Matrix R_in = (h_v * R * h_v.transpose()).inverse();\n\t\t\t\t\tK_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in;\n#endif\n\t\t\t\t}\n\n\t\t\t\tcov K_x = K_ * h_x;\n\t\t\t\tMatrix dx_ = K_ * (z - h) + (K_x - Matrix::Identity()) * dx_new;\n\t\t\t\tstate x_before = x_;\n\t\t\t\tx_.boxplus(dx_);\n\t\t\t\tdyn_share.converge = true;\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tif (std::fabs(dx_[i]) > limit[i])\n\t\t\t\t\t{\n\t\t\t\t\t\tdyn_share.converge = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\tif (dyn_share.converge)\n\t\t\t\t\tt++;\n\t\t\t\tif (t > 1 || i == maximum_iter - 1)\n\t\t\t\t{\n\t\t\t\t\tL_ = P_;\n\t\t\t\t\tstd::cout << \"iteration time:\" << t << \",\" << i << std::endl;\n\n\t\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_SO3(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\t\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\t\tfor (int i = 0; i < int(n); i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\tMatrix res_temp_S2;\n\t\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\n\t\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_S2(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\n\t\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_ * h_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\treturn;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\n\t\t// iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing.\n\t\t// the measurement and the measurement model are received in a dynamic manner.\n\t\ttemplate \n\t\tvoid update_iterated_dyn_runtime(measurement_runtime z, measurementnoisecovariance_dyn R, measurementModel_runtime h_runtime)\n\t\t{\n\n\t\t\tint t = 0;\n\t\t\tbool valid = true;\n\t\t\tbool converg = true;\n\t\t\tstate x_propagated = x_;\n\t\t\tcov P_propagated = P_;\n\t\t\tint dof_Measurement;\n\t\t\tint dof_Measurement_noise;\n\t\t\tfor (int i = -1; i < maximum_iter; i++)\n\t\t\t{\n\t\t\t\tvalid = true;\n#ifdef USE_sparse\n\t\t\t\tspMt h_x_ = h_x_dyn(x_, valid).sparseView();\n\t\t\t\tspMt h_v_ = h_v_dyn(x_, valid).sparseView();\n\t\t\t\tspMt R_ = R.sparseView();\n#else\n\t\t\t\tMatrix h_x_ = h_x_dyn(x_, valid);\n\t\t\t\tMatrix h_v_ = h_v_dyn(x_, valid);\n#endif\n\t\t\t\tmeasurement_runtime h_ = h_runtime(x_, valid);\n\t\t\t\tdof_Measurement = measurement_runtime::DOF;\n\t\t\t\tdof_Measurement_noise = R.rows();\n\t\t\t\tvectorized_state dx, dx_new;\n\t\t\t\tx_.boxminus(dx, x_propagated);\n\t\t\t\tdx_new = dx;\n\t\t\t\tif (!valid)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\n\t\t\t\tP_ = P_propagated;\n\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_SO3(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\tdx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix res_temp_S2;\n\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_S2(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\tdx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix K_;\n\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tMatrix K_temp = h_x_ * P_ * h_x_.transpose();\n\t\t\t\t\tspMt R_temp = h_v_ * R_ * h_v_.transpose();\n\t\t\t\t\tK_temp += R_temp;\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * K_temp.inverse();\n#else\n\t\t\t\t\tK_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();\n#endif\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tEigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise);\n\t\t\t\t\tEigen::SparseQR, Eigen::COLAMDOrdering> solver;\n\t\t\t\t\tsolver.compute(R_);\n\t\t\t\t\tEigen::Matrix R_in_temp = solver.solve(b);\n\t\t\t\t\tspMt R_in = R_in_temp.sparseView();\n\t\t\t\t\tspMt K_temp = h_x_.transpose() * R_in * h_x_;\n\t\t\t\t\tcov P_temp = P_.inverse();\n\t\t\t\t\tP_temp += K_temp;\n\t\t\t\t\tK_ = P_temp.inverse() * h_x_.transpose() * R_in;\n#else\n\t\t\t\t\tEigen::Matrix R_in = (h_v_ * R * h_v_.transpose()).inverse();\n\t\t\t\t\tK_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;\n#endif\n\t\t\t\t}\n\t\t\t\tcov K_x = K_ * h_x_;\n\t\t\t\tEigen::Matrix innovation;\n\t\t\t\tz.boxminus(innovation, h_);\n\t\t\t\tMatrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new;\n\t\t\t\tstate x_before = x_;\n\t\t\t\tx_.boxplus(dx_);\n\t\t\t\tconverg = true;\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tif (std::fabs(dx_[i]) > limit[i])\n\t\t\t\t\t{\n\t\t\t\t\t\tconverg = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\tif (converg)\n\t\t\t\t\tt++;\n\t\t\t\tif (t > 1 || i == maximum_iter - 1)\n\t\t\t\t{\n\t\t\t\t\tL_ = P_;\n\t\t\t\t\tstd::cout << \"iteration time:\" << t << \",\" << i << std::endl;\n\n\t\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_SO3(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\t\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\tMatrix res_temp_S2;\n\t\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\n\t\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_S2(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\n\t\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_ * h_x_ * P_;\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\treturn;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\n\t\t// iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing.\n\t\t// the measurement and the measurement model are received in a dynamic manner.\n\t\t// 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.\n\t\ttemplate \n\t\tvoid update_iterated_dyn_runtime_share(measurement_runtime z, measurementModel_dyn_runtime_share h)\n\t\t{\n\n\t\t\tint t = 0;\n\t\t\tdyn_runtime_share_datastruct dyn_share;\n\t\t\tdyn_share.valid = true;\n\t\t\tdyn_share.converge = true;\n\t\t\tstate x_propagated = x_;\n\t\t\tcov P_propagated = P_;\n\t\t\tint dof_Measurement;\n\t\t\tint dof_Measurement_noise;\n\t\t\tfor (int i = -1; i < maximum_iter; i++)\n\t\t\t{\n\t\t\t\tdyn_share.valid = true;\n\t\t\t\tmeasurement_runtime h_ = h(x_, dyn_share);\n\t\t\t\t// measurement_runtime z = dyn_share.z;\n#ifdef USE_sparse\n\t\t\t\tspMt h_x = dyn_share.h_x.sparseView();\n\t\t\t\tspMt h_v = dyn_share.h_v.sparseView();\n\t\t\t\tspMt R_ = dyn_share.R.sparseView();\n#else\n\t\t\t\tMatrix R = dyn_share.R;\n\t\t\t\tMatrix h_x = dyn_share.h_x;\n\t\t\t\tMatrix h_v = dyn_share.h_v;\n#endif\n\t\t\t\tdof_Measurement = measurement_runtime::DOF;\n\t\t\t\tdof_Measurement_noise = dyn_share.R.rows();\n\t\t\t\tvectorized_state dx, dx_new;\n\t\t\t\tx_.boxminus(dx, x_propagated);\n\t\t\t\tdx_new = dx;\n\t\t\t\tif (!(dyn_share.valid))\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\n\t\t\t\tP_ = P_propagated;\n\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_SO3(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\tdx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix res_temp_S2;\n\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_S2(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\tdx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix K_;\n\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tMatrix K_temp = h_x * P_ * h_x.transpose();\n\t\t\t\t\tspMt R_temp = h_v * R_ * h_v.transpose();\n\t\t\t\t\tK_temp += R_temp;\n\t\t\t\t\tK_ = P_ * h_x.transpose() * K_temp.inverse();\n#else\n\t\t\t\t\tK_ = P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();\n#endif\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n#ifdef USE_sparse\n\t\t\t\t\tEigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise);\n\t\t\t\t\tEigen::SparseQR, Eigen::COLAMDOrdering> solver;\n\t\t\t\t\tsolver.compute(R_);\n\t\t\t\t\tEigen::Matrix R_in_temp = solver.solve(b);\n\t\t\t\t\tspMt R_in = R_in_temp.sparseView();\n\t\t\t\t\tspMt K_temp = h_x.transpose() * R_in * h_x;\n\t\t\t\t\tcov P_temp = P_.inverse();\n\t\t\t\t\tP_temp += K_temp;\n\t\t\t\t\tK_ = P_temp.inverse() * h_x.transpose() * R_in;\n#else\n\t\t\t\t\tEigen::Matrix R_in = (h_v * R * h_v.transpose()).inverse();\n\t\t\t\t\tK_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in;\n#endif\n\t\t\t\t}\n\t\t\t\tcov K_x = K_ * h_x;\n\t\t\t\tEigen::Matrix innovation;\n\t\t\t\tz.boxminus(innovation, h_);\n\t\t\t\tMatrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new;\n\t\t\t\tstate x_before = x_;\n\t\t\t\tx_.boxplus(dx_);\n\t\t\t\tdyn_share.converge = true;\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tif (std::fabs(dx_[i]) > limit[i])\n\t\t\t\t\t{\n\t\t\t\t\t\tdyn_share.converge = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\tif (dyn_share.converge)\n\t\t\t\t\tt++;\n\t\t\t\tif (t > 1 || i == maximum_iter - 1)\n\t\t\t\t{\n\t\t\t\t\tL_ = P_;\n\t\t\t\t\tstd::cout << \"iteration time:\" << t << \",\" << i << std::endl;\n\n\t\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_SO3(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\t\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\t\tfor (int i = 0; i < int(n); i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\tMatrix res_temp_S2;\n\t\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\n\t\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_S2(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\n\t\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < dof_Measurement; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\telse\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tK_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_ * h_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\telse\n\t\t\t\t\t{\n\t\t\t\t\t\tP_ = L_ - K_x * P_;\n\t\t\t\t\t}\n\t\t\t\t\treturn;\n\t\t\t\t}\n\t\t\t}\n\t\t}\n\n\t\t// 迭代错误状态EKF更新修改为一个特定的系统\n\t\tvoid update_iterated_dyn_share_modified(double R, double &solve_time)\n\t\t{\n\n\t\t\tdyn_share_datastruct dyn_share;\n\t\t\tdyn_share.valid = true;\n\t\t\tdyn_share.converge = true;\n\t\t\tint t = 0;\n\t\t\t//获取上一次的状态和协方差矩阵\n\t\t\tstate x_propagated = x_;\n\t\t\tcov P_propagated = P_;\n\t\t\tint dof_Measurement;\n\n\t\t\tMatrix K_h;\n\t\t\tMatrix K_x;\n\n\t\t\tvectorized_state dx_new = vectorized_state::Zero();\n\t\t\t// 最多进行maximum_iter次迭代优化\n\t\t\tfor (int i = -1; i < maximum_iter; i++)\n\t\t\t{\n\t\t\t\tdyn_share.valid = true;\n\t\t\t\t// 计算测量模型方程的雅克比，也就是点面残差的导数 H(代码里是h_x)\n\t\t\t\th_dyn_share(x_, dyn_share);\n\t\t\t\t// Matrix h = h_dyn_share(x_, dyn_share);\n#ifdef USE_sparse\n\t\t\t\tspMt h_x_ = dyn_share.h_x.sparseView();\n#else\n\t\t\t\t// 获取测量模型的雅克比d(pos, rot, 0, 0)/dx\n\t\t\t\tEigen::Matrix h_x_ = dyn_share.h_x;\n#endif\n\t\t\t\tdouble solve_start = omp_get_wtime();\n\t\t\t\tdof_Measurement = h_x_.rows(); // 观测方程个数m\n\t\t\t\tvectorized_state dx;\t\t // 定义误差状态\n\t\t\t\tx_.boxminus(dx, x_propagated); //获取误差dx\n\t\t\t\tdx_new = dx;\t\t\t\t // 用于迭代的误差状态\n\n\t\t\t\tif (!dyn_share.valid)\n\t\t\t\t{\n\t\t\t\t\tcontinue;\n\t\t\t\t}\n\t\t\t\t// 预测得到的误差状态协方差矩阵\n\t\t\t\t//协方差矩阵在迭代过程中不会代入下一次迭代，直到最后一次退出时更新，在迭代过程中更新的只是先验\n\t\t\t\tP_ = P_propagated;\n\t\t\t\t// 这一大段都在求协方差的先验更新，大致上是P=(J^-1)*P*(J^-T)如论文式16~18\n\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\tfor (std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_SO3(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\tdx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tMatrix res_temp_S2;\n\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\tfor (std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t{\n\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\tint dim = (*it).second;\n\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tseg_S2(i) = dx(idx + i);\n\t\t\t\t\t}\n\n\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\tdx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t}\n\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\t// Matrix K_;\n\t\t\t\t// Matrix K_h;\n\t\t\t\t// Matrix K_x;\n\n\t\t\t\t/*\n\t\t\t\tif(n > dof_Measurement)\n\t\t\t\t{\n\t\t\t\t\tK_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse()/R;\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n\t\t\t\t\tK_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();\n\t\t\t\t}\n\t\t\t\t*/\n\t\t\t\t// 状态维度 n > 测量维度 dof_Measurement\n\t\t\t\t// 如果状态量维度大于观测方程 n > m，不满秩\n\t\t\t\tif (n > dof_Measurement)\n\t\t\t\t{\n\t\t\t\t\t//#ifdef USE_sparse\n\t\t\t\t\t// Matrix K_temp = h_x * P_ * h_x.transpose();\n\t\t\t\t\t// spMt R_temp = h_v * R_ * h_v.transpose();\n\t\t\t\t\t// K_temp += R_temp;\n\t\t\t\t\tEigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n);\n\t\t\t\t\t//每一次迭代将重新计算增益K，即论文式18\n\t\t\t\t\th_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;\n\t\t\t\t\t/*\n\t\t\t\t\th_x_cur.col(0) = h_x_.col(0);\n\t\t\t\t\th_x_cur.col(1) = h_x_.col(1);\n\t\t\t\t\th_x_cur.col(2) = h_x_.col(2);\n\t\t\t\t\th_x_cur.col(3) = h_x_.col(3);\n\t\t\t\t\th_x_cur.col(4) = h_x_.col(4);\n\t\t\t\t\th_x_cur.col(5) = h_x_.col(5);\n\t\t\t\t\th_x_cur.col(6) = h_x_.col(6);\n\t\t\t\t\th_x_cur.col(7) = h_x_.col(7);\n\t\t\t\t\th_x_cur.col(8) = h_x_.col(8);\n\t\t\t\t\th_x_cur.col(9) = h_x_.col(9);\n\t\t\t\t\th_x_cur.col(10) = h_x_.col(10);\n\t\t\t\t\th_x_cur.col(11) = h_x_.col(11);\n\t\t\t\t\t*/\n\t\t\t\t\t// 重新计算增益矩阵K\n\t\t\t\t\tMatrix K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose() / R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse() / R;\n\t\t\t\t\tK_h = K_ * dyn_share.h;\n\t\t\t\t\tK_x = K_ * h_x_cur;\n\t\t\t\t\t//#else\n\t\t\t\t\t//\tK_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();\n\t\t\t\t\t//#endif\n\t\t\t\t}\n\t\t\t\telse\n\t\t\t\t{\n\t\t\t\t\t//避免求逆矩阵，K按稀疏矩阵分解的方法如论文式20\n#ifdef USE_sparse\n\t\t\t\t\t// Eigen::Matrix b = Eigen::Matrix::Identity();\n\t\t\t\t\t// Eigen::SparseQR, Eigen::COLAMDOrdering> solver;\n\t\t\t\t\tspMt A = h_x_.transpose() * h_x_;\n\t\t\t\t\tcov P_temp = (P_ / R).inverse();\n\t\t\t\t\tP_temp.template block<12, 12>(0, 0) += A;\n\t\t\t\t\tP_temp = P_temp.inverse();\n\t\t\t\t\t/*\n\t\t\t\t\tEigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n);\n\t\t\t\t\th_x_cur.col(0) = h_x_.col(0);\n\t\t\t\t\th_x_cur.col(1) = h_x_.col(1);\n\t\t\t\t\th_x_cur.col(2) = h_x_.col(2);\n\t\t\t\t\th_x_cur.col(3) = h_x_.col(3);\n\t\t\t\t\th_x_cur.col(4) = h_x_.col(4);\n\t\t\t\t\th_x_cur.col(5) = h_x_.col(5);\n\t\t\t\t\th_x_cur.col(6) = h_x_.col(6);\n\t\t\t\t\th_x_cur.col(7) = h_x_.col(7);\n\t\t\t\t\th_x_cur.col(8) = h_x_.col(8);\n\t\t\t\t\th_x_cur.col(9) = h_x_.col(9);\n\t\t\t\t\th_x_cur.col(10) = h_x_.col(10);\n\t\t\t\t\th_x_cur.col(11) = h_x_.col(11);\n\t\t\t\t\t*/\n\t\t\t\t\tK_ = P_temp.template block(0, 0) * h_x_.transpose();\n\t\t\t\t\tK_x = cov::Zero();\n\t\t\t\t\tK_x.template block(0, 0) = P_inv.template block(0, 0) * HTH;\n\t\t\t\t\t/*\n\t\t\t\t\tsolver.compute(R_);\n\t\t\t\t\tEigen::Matrix R_in_temp = solver.solve(b);\n\t\t\t\t\tspMt R_in =R_in_temp.sparseView();\n\t\t\t\t\tspMt K_temp = h_x.transpose() * R_in * h_x;\n\t\t\t\t\tcov P_temp = P_.inverse();\n\t\t\t\t\tP_temp += K_temp;\n\t\t\t\t\tK_ = P_temp.inverse() * h_x.transpose() * R_in;\n\t\t\t\t\t*/\n#else\n\t\t\t\t\tcov P_temp = (P_ / R).inverse();\n\t\t\t\t\t// Eigen::Matrix h_T = h_x_.transpose();\n\t\t\t\t\tEigen::Matrix HTH = h_x_.transpose() * h_x_;\n\t\t\t\t\tP_temp.template block<12, 12>(0, 0) += HTH;\n\t\t\t\t\t/*\n\t\t\t\t\tEigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n);\n\t\t\t\t\t//std::cout << \"line 1767\" << std::endl;\n\t\t\t\t\th_x_cur.col(0) = h_x_.col(0);\n\t\t\t\t\th_x_cur.col(1) = h_x_.col(1);\n\t\t\t\t\th_x_cur.col(2) = h_x_.col(2);\n\t\t\t\t\th_x_cur.col(3) = h_x_.col(3);\n\t\t\t\t\th_x_cur.col(4) = h_x_.col(4);\n\t\t\t\t\th_x_cur.col(5) = h_x_.col(5);\n\t\t\t\t\th_x_cur.col(6) = h_x_.col(6);\n\t\t\t\t\th_x_cur.col(7) = h_x_.col(7);\n\t\t\t\t\th_x_cur.col(8) = h_x_.col(8);\n\t\t\t\t\th_x_cur.col(9) = h_x_.col(9);\n\t\t\t\t\th_x_cur.col(10) = h_x_.col(10);\n\t\t\t\t\th_x_cur.col(11) = h_x_.col(11);\n\t\t\t\t\t*/\n\t\t\t\t\tcov P_inv = P_temp.inverse();\n\t\t\t\t\t// std::cout << \"line 1781\" << std::endl;\n\t\t\t\t\tK_h = P_inv.template block(0, 0) * h_x_.transpose() * dyn_share.h; // (H_T_H + P^-1)^-1 * H^T * h(残差) = K * h\n\t\t\t\t\t// std::cout << \"line 1780\" << std::endl;\n\t\t\t\t\t// cov HTH_cur = cov::Zero();\n\t\t\t\t\t// HTH_cur. template block<12, 12>(0, 0) = HTH;\n\t\t\t\t\tK_x.setZero(); // = cov::Zero();\n\n\t\t\t\t\tK_x.template block(0, 0) = P_inv.template block(0, 0) * HTH; //(H_T_H + P^-1)^-1 * H_T_H = KH\n\n\t\t\t\t\t// K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();\n#endif\n\t\t\t\t}\n\n\t\t\t\t// K_x = K_ * h_x_;\n\t\t\t\t//由于是误差迭代KF，得到的是误差的最优估计！\n\t\t\t\tMatrix dx_ = K_h + (K_x - Matrix::Identity()) * dx_new; //误差增量后验 K*h + (K*H - I) dx\n\n\t\t\t\tstate x_before = x_; // 加上校正后的误差状态dx_\n\t\t\t\tx_.boxplus(dx_);\t //根据计算得到的误差增量后验，更新状态量\n\t\t\t\t// 判断迭代是否发散\n\t\t\t\tdyn_share.converge = true;\n\t\t\t\t//判断已收敛的条件是误差的估计值小于阈值\n\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t{\n\t\t\t\t\tif (std::fabs(dx_[i]) > limit[i])\n\t\t\t\t\t{\n\t\t\t\t\t\tdyn_share.converge = false;\n\t\t\t\t\t\tbreak;\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t\tif (dyn_share.converge)\n\t\t\t\t\tt++;\n\n\t\t\t\tif (!t && i == maximum_iter - 2)\n\t\t\t\t{\n\t\t\t\t\tdyn_share.converge = true;\n\t\t\t\t}\n\t\t\t\t// 迭代完成后更新误差状态协方差矩阵\n\t\t\t\t//结束迭代后，更新协方差矩阵的后验值，大致上是P=(I-K*H)*P，如论文式19\n\t\t\t\tif (t > 1 || i == maximum_iter - 1)\n\t\t\t\t{\n\t\t\t\t\tL_ = P_;\n\t\t\t\t\t// std::cout << \"iteration time\" << t << \",\" << i << std::endl;\n\t\t\t\t\tMatrix res_temp_SO3;\n\t\t\t\t\tMTK::vect<3, scalar_type> seg_SO3;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\t\t\t\t\t\tfor (int i = 0; i < 3; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_SO3(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\t\t\t\t\t\tres_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\t// if(n > dof_Measurement)\n\t\t\t\t\t\t// {\n\t\t\t\t\t\t// \tfor(int i = 0; i < dof_Measurement; i++){\n\t\t\t\t\t\t// \t\tK_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));\n\t\t\t\t\t\t// \t}\n\t\t\t\t\t\t// }\n\t\t\t\t\t\t// else\n\t\t\t\t\t\t// {\n\t\t\t\t\t\tfor (int i = 0; i < 12; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tK_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\t//}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\tMatrix res_temp_S2;\n\t\t\t\t\tMTK::vect<2, scalar_type> seg_S2;\n\t\t\t\t\tfor (typename std::vector>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint idx = (*it).first;\n\n\t\t\t\t\t\tfor (int i = 0; i < 2; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tseg_S2(i) = dx_(i + idx);\n\t\t\t\t\t\t}\n\n\t\t\t\t\t\tEigen::Matrix Nx;\n\t\t\t\t\t\tEigen::Matrix Mx;\n\t\t\t\t\t\tx_.S2_Nx_yy(Nx, idx);\n\t\t\t\t\t\tx_propagated.S2_Mx(Mx, seg_S2, idx);\n\t\t\t\t\t\tres_temp_S2 = Nx * Mx;\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\t// if(n > dof_Measurement)\n\t\t\t\t\t\t// {\n\t\t\t\t\t\t// \tfor(int i = 0; i < dof_Measurement; i++){\n\t\t\t\t\t\t// \t\tK_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));\n\t\t\t\t\t\t// \t}\n\t\t\t\t\t\t// }\n\t\t\t\t\t\t// else\n\t\t\t\t\t\t// {\n\t\t\t\t\t\tfor (int i = 0; i < 12; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tK_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));\n\t\t\t\t\t\t}\n\t\t\t\t\t\t//}\n\t\t\t\t\t\tfor (int i = 0; i < n; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tL_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t\tP_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\n\t\t\t\t\t// if(n > dof_Measurement)\n\t\t\t\t\t// {\n\t\t\t\t\t// \tEigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n);\n\t\t\t\t\t// \th_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;\n\t\t\t\t\t// \t/*\n\t\t\t\t\t// \th_x_cur.col(0) = h_x_.col(0);\n\t\t\t\t\t// \th_x_cur.col(1) = h_x_.col(1);\n\t\t\t\t\t// \th_x_cur.col(2) = h_x_.col(2);\n\t\t\t\t\t// \th_x_cur.col(3) = h_x_.col(3);\n\t\t\t\t\t// \th_x_cur.col(4) = h_x_.col(4);\n\t\t\t\t\t// \th_x_cur.col(5) = h_x_.col(5);\n\t\t\t\t\t// \th_x_cur.col(6) = h_x_.col(6);\n\t\t\t\t\t// \th_x_cur.col(7) = h_x_.col(7);\n\t\t\t\t\t// \th_x_cur.col(8) = h_x_.col(8);\n\t\t\t\t\t// \th_x_cur.col(9) = h_x_.col(9);\n\t\t\t\t\t// \th_x_cur.col(10) = h_x_.col(10);\n\t\t\t\t\t// \th_x_cur.col(11) = h_x_.col(11);\n\t\t\t\t\t// \t*/\n\t\t\t\t\t// \tP_ = L_ - K_*h_x_cur * P_;\n\t\t\t\t\t// }\n\t\t\t\t\t// else\n\t\t\t\t\t//{\n\t\t\t\t\tP_ = L_ - K_x.template block(0, 0) * P_.template block<12, n>(0, 0);\n\t\t\t\t\t//}\n\t\t\t\t\tsolve_time += omp_get_wtime() - solve_start;\n\t\t\t\t\treturn;\n\t\t\t\t}\n\t\t\t\tsolve_time += omp_get_wtime() - solve_start;\n\t\t\t}\n\t\t}\n\n\t\tvoid change_x(state &input_state)\n\t\t{\n\t\t\tx_ = input_state;\n\t\t\tif ((!x_.vect_state.size()) && (!x_.SO3_state.size()) && (!x_.S2_state.size()))\n\t\t\t{\n\t\t\t\tx_.build_S2_state();\n\t\t\t\tx_.build_SO3_state();\n\t\t\t\tx_.build_vect_state();\n\t\t\t}\n\t\t}\n\t\t// 修改P_的状态\n\t\tvoid change_P(cov &input_cov)\n\t\t{\n\t\t\tP_ = input_cov;\n\t\t}\n\t\t// 获得x_的状态\n\t\tconst state &get_x() const\n\t\t{\n\t\t\treturn x_;\n\t\t}\n\t\tconst cov &get_P() const\n\t\t{\n\t\t\treturn P_;\n\t\t}\n\n\tprivate:\n\t\tstate x_;\n\t\tmeasurement m_;\n\t\tcov P_;\n\t\tspMt l_;\n\t\tspMt f_x_1;\n\t\tspMt f_x_2;\n\t\tcov F_x1 = cov::Identity();\n\t\tcov F_x2 = cov::Identity();\n\t\tcov L_ = cov::Identity();\n\n\t\tprocessModel *f;\n\t\tprocessMatrix1 *f_x;\n\t\tprocessMatrix2 *f_w;\n\n\t\tmeasurementModel *h;\n\t\tmeasurementMatrix1 *h_x;\n\t\tmeasurementMatrix2 *h_v;\n\n\t\tmeasurementModel_dyn *h_dyn;\n\t\tmeasurementMatrix1_dyn *h_x_dyn;\n\t\tmeasurementMatrix2_dyn *h_v_dyn;\n\n\t\tmeasurementModel_share *h_share;\n\t\tmeasurementModel_dyn_share *h_dyn_share;\n\n\t\tint maximum_iter = 0;\n\t\tscalar_type limit[n];\n\n\t\ttemplate \n\t\tT check_safe_update(T _temp_vec)\n\t\t{\n\t\t\tT temp_vec = _temp_vec;\n\t\t\tif (std::isnan(temp_vec(0, 0)))\n\t\t\t{\n\t\t\t\ttemp_vec.setZero();\n\t\t\t\treturn temp_vec;\n\t\t\t}\n\t\t\tdouble angular_dis = temp_vec.block(0, 0, 3, 1).norm() * 57.3;\n\t\t\tdouble pos_dis = temp_vec.block(3, 0, 3, 1).norm();\n\t\t\tif (angular_dis >= 20 || pos_dis > 1)\n\t\t\t{\n\t\t\t\tprintf(\"Angular dis = %.2f, pos dis = %.2f\\r\\n\", angular_dis, pos_dis);\n\t\t\t\ttemp_vec.setZero();\n\t\t\t}\n\t\t\treturn temp_vec;\n\t\t}\n\n\tpublic:\n\t\tEIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\t};\n\n} // namespace esekfom\n\n#endif // ESEKFOM_EKF_HPP\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/esekfom/util.hpp", "content": "/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Author: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef __MEKFOM_UTIL_HPP__\n#define __MEKFOM_UTIL_HPP__\n\n#include \n#include \"../mtk/src/mtkmath.hpp\"\nnamespace esekfom {\n\ntemplate \nclass is_same {\npublic:\n operator bool() {\n return false;\n }\n};\ntemplate\nclass is_same {\npublic:\n operator bool() {\n return true;\n }\n};\n\ntemplate \nclass is_double {\npublic:\n operator bool() {\n return false;\n }\n};\n\ntemplate<>\nclass is_double {\npublic:\n operator bool() {\n return true;\n }\n};\n\ntemplate\nstatic T\nid(const T &x)\n{\n\treturn x;\n}\n\n} // namespace esekfom\n\t\n#endif // __MEKFOM_UTIL_HPP__\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp", "content": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.\n// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119\n\n/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Modifier: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n \n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/build_manifold.hpp\n * @brief Macro to automatically construct compound manifolds.\n * \n */\n#ifndef MTK_AUTOCONSTRUCT_HPP_\n#define MTK_AUTOCONSTRUCT_HPP_\n\n#include \n\n#include \n#include \n#include \n\n#include \"src/SubManifold.hpp\"\n#include \"startIdx.hpp\"\n\n#ifndef PARSED_BY_DOXYGEN\n//////// internals //////\n\n#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple\n\n#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))\n\n#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)\n\n#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type()\n#define MTK_CONSTRUCTOR_COPY( type, id) id(id)\n#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale);\n#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale);\n#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id);\n#define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);}\n#define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);}\n#define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);}\n#define MTK_OSTREAM( type, id) << __var.id << \" \"\n#define MTK_ISTREAM( type, id) >> __var.id\n#define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));}\n#define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));}\n#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));}\n\n#define MTK_SUBVARLIST(seq, S2state, SO3state) \\\nBOOST_PP_FOR_1( \\\n\t\t( \\\n\t\t\t\tBOOST_PP_SEQ_SIZE(seq), \\\n\t\t\t\tBOOST_PP_SEQ_HEAD(seq), \\\n\t\t\t\tBOOST_PP_SEQ_TAIL(seq) (~), \\\n\t\t\t\t0,\\\n\t\t\t\t0,\\\n\t\t\t\tS2state,\\\n\t\t\t\tSO3state ),\\\n\t\tMTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT)\n\n#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \\\n\tMTK::SubManifold id; \n#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \\\n\tMTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \\\n\tenum {DOF = type::DOF + dof}; \\\n\tenum {DIM = type::DIM+dim}; \\\n\ttypedef type::scalar scalar; \n\n#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state\n#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \\\n\tMTK_APPLY_MACRO_ON_TUPLE(~, \\\n\t\tBOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \\\n\t\t( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state)) \n\n#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state\n\n//! this used to be BOOST_PP_TUPLE_ELEM_4_0:\n#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a\n\n#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state\n#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \\\n\t\tBOOST_PP_DEC(len), \\\n\t\tBOOST_PP_SEQ_HEAD(seq), \\\n\t\tBOOST_PP_SEQ_TAIL(seq), \\\n\t\tdof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\\\n\t\tdim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\\\n\t\tS2state,\\\n\t\tSO3state)\n\n#endif /* not PARSED_BY_DOXYGEN */\n\n\n/**\n * Construct a manifold.\n * @param name is the class-name of the manifold, \n * @param entries is the list of sub manifolds \n * \n * Entries must be given in a list like this:\n * @code\n * typedef MTK::trafo > Pose;\n * typedef MTK::vect Vec3;\n * MTK_BUILD_MANIFOLD(imu_state,\n * ((Pose, pose))\n * ((Vec3, vel))\n * ((Vec3, acc_bias))\n * )\n * @endcode\n * Whitespace is optional, but the double parentheses are necessary.\n * Construction is done entirely in preprocessor.\n * After construction @a name is also a manifold. Its members can be \n * accessed by names given in @a entries.\n * \n * @note Variable types are not allowed to have commas, thus types like\n * @c vect need to be typedef'ed ahead.\n */\n#define MTK_BUILD_MANIFOLD(name, entries) \\\nstruct name { \\\n\ttypedef name self; \\\n\tstd::vector > S2_state;\\\n\tstd::vector > SO3_state;\\\n\tstd::vector, int> > vect_state;\\\n\tMTK_SUBVARLIST(entries, S2_state, SO3_state) \\\n\tname ( \\\n\t\tMTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \\\n\t\t) : \\\n\t\tMTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\\\n\tint getDOF() const { return DOF; } \\\n\tvoid boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \\\n\t\tMTK_TRANSFORM(MTK_BOXPLUS, entries)\\\n\t} \\\n\tvoid oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \\\n\t\tMTK_TRANSFORM(MTK_OPLUS, entries)\\\n\t} \\\n\tvoid boxminus(MTK::vectview __res, const name& __oth) const { \\\n\t\tMTK_TRANSFORM(MTK_BOXMINUS, entries)\\\n\t} \\\n\tfriend std::ostream& operator<<(std::ostream& __os, const name& __var){ \\\n\t\treturn __os MTK_TRANSFORM(MTK_OSTREAM, entries); \\\n\t} \\\n\tvoid build_S2_state(){\\\n\t\tMTK_TRANSFORM(MTK_S2_state, entries)\\\n\t}\\\n\tvoid build_vect_state(){\\\n\t\tMTK_TRANSFORM(MTK_vect_state, entries)\\\n\t}\\\n\tvoid build_SO3_state(){\\\n\t\tMTK_TRANSFORM(MTK_SO3_state, entries)\\\n\t}\\\n\tvoid S2_hat(Eigen::Matrix &res, int idx) {\\\n\t\tMTK_TRANSFORM(MTK_S2_hat, entries)\\\n\t}\\\n\tvoid S2_Nx_yy(Eigen::Matrix &res, int idx) {\\\n\t\tMTK_TRANSFORM(MTK_S2_Nx_yy, entries)\\\n\t}\\\n\tvoid S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\\\n\t\tMTK_TRANSFORM(MTK_S2_Mx, entries)\\\n\t}\\\n\tfriend std::istream& operator>>(std::istream& __is, name& __var){ \\\n\t\treturn __is MTK_TRANSFORM(MTK_ISTREAM, entries); \\\n\t} \\\n};\n\n\n\n#endif /*MTK_AUTOCONSTRUCT_HPP_*/\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp", "content": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.\n// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119\n\n/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Modifier: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n \n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/src/SubManifold.hpp\n * @brief Defines the SubManifold class\n */\n\n\n#ifndef SUBMANIFOLD_HPP_\n#define SUBMANIFOLD_HPP_\n\n\n#include \"vectview.hpp\"\n\n\nnamespace MTK {\n\n/**\n * @ingroup SubManifolds\n * Helper class for compound manifolds. \n * This class wraps a manifold T and provides an enum IDX refering to the \n * index of the SubManifold within the compound manifold. \n * \n * Memberpointers to a submanifold can be used for @ref SubManifolds \"functions accessing submanifolds\".\n * \n * @tparam T The manifold type of the sub-type\n * @tparam idx The index of the sub-type within the compound manifold\n */\ntemplate\nstruct SubManifold : public T \n{\n\tenum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };\n\t//! manifold type\n\ttypedef T type;\n\t\n\t//! Construct from derived type\n\ttemplate\n\texplicit\n\tSubManifold(const X& t) : T(t) {};\n\t\n\t//! Construct from internal type\n\t//explicit\n\tSubManifold(const T& t) : T(t) {};\n\t\n\t//! inherit assignment operator\n\tusing T::operator=;\n\t\n};\n\n} // namespace MTK\n\n\n#endif /* SUBMANIFOLD_HPP_ */\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp", "content": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.\n// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119\n\n/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Modifier: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n \n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/src/mtkmath.hpp\n * @brief several math utility functions.\n */\n\n#ifndef MTKMATH_H_\n#define MTKMATH_H_\n\n#include \n\n#include \n\n#include \"../types/vect.hpp\"\n\n#ifndef M_PI\n#define M_PI 3.1415926535897932384626433832795\n#endif\n\n\nnamespace MTK {\n\nnamespace internal {\n\ntemplate\nstruct traits {\n\ttypedef typename Manifold::scalar scalar;\n\tenum {DOF = Manifold::DOF};\n\ttypedef vect vectorized_type;\n\ttypedef Eigen::Matrix matrix_type;\n};\n\ntemplate<>\nstruct traits : traits > {};\ntemplate<>\nstruct traits : traits > {};\n\n} // namespace internal\n\n/**\n * \\defgroup MTKMath Mathematical helper functions\n */\n//@{\n\n//! constant @f$ \\pi @f$\nconst double pi = M_PI;\n\ntemplate inline scalar tolerance();\n\ntemplate<> inline float tolerance() { return 1e-5f; }\ntemplate<> inline double tolerance() { return 1e-11; }\n\n\n/**\n * normalize @a x to @f$[-bound, bound] @f$.\n * \n * result for @f$ x = bound + 2\\cdot n\\cdot bound @f$ is arbitrary @f$\\pm bound @f$.\n */\ntemplate\ninline scalar normalize(scalar x, scalar bound){ //not used\n\tif(std::fabs(x) <= bound) return x;\n\tint r = (int)(x *(scalar(1.0)/ bound));\n\treturn x - ((r + (r>>31) + 1) & ~1)*bound; \n}\n\n/**\n * Calculate cosine and sinc of sqrt(x2).\n * @param x2 the squared angle must be non-negative\n * @return a pair containing cos and sinc of sqrt(x2)\n */\ntemplate\nstd::pair cos_sinc_sqrt(const scalar &x2){\n\tusing std::sqrt;\n\tusing std::cos;\n\tusing std::sin;\n\tstatic scalar const taylor_0_bound = boost::math::tools::epsilon();\n\tstatic scalar const taylor_2_bound = sqrt(taylor_0_bound);\n\tstatic scalar const taylor_n_bound = sqrt(taylor_2_bound);\n\t\n\tassert(x2>=0 && \"argument must be non-negative\");\n\t\n\t// FIXME check if bigger bounds are possible\n\tif(x2>=taylor_n_bound) {\n\t\t// slow fall-back solution\n\t\tscalar x = sqrt(x2);\n\t\treturn std::make_pair(cos(x), sin(x)/x); // x is greater than 0.\n\t}\n\t\n\t// 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)\n\t// TODO Find optimal coefficients using Remez algorithm\n\tstatic scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};\n\tscalar cosi = 1., sinc=1;\n\tscalar term = -1/2. * x2;\n\tfor(int i=0; i<3; ++i) {\n\t\tcosi += term;\n\t\tterm *= inv[2*i];\n\t\tsinc += term;\n\t\tterm *= -inv[2*i+1] * x2;\n\t}\n\t\n\treturn std::make_pair(cosi, sinc);\n\t\n}\n\ntemplate\nEigen::Matrix hat(const Base& v) {\n Eigen::Matrix res;\n\tres << 0, -v[2], v[1],\n\t\tv[2], 0, -v[0],\n\t\t-v[1], v[0], 0;\n\treturn res;\n}\n\ntemplate\nEigen::Matrix A_inv_trans(const Base& v){\n Eigen::Matrix res;\n if(v.norm() > MTK::tolerance())\n {\n 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();\n \n }\n else\n {\n res = Eigen::Matrix::Identity();\n }\n \n return res;\n}\n\ntemplate\nEigen::Matrix A_inv(const Base& v){\n Eigen::Matrix res;\n if(v.norm() > MTK::tolerance())\n {\n 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();\n \n }\n else\n {\n res = Eigen::Matrix::Identity();\n }\n \n return res;\n}\n\ntemplate\nEigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length)\n\t{\n \tEigen::Matrix res;\n \tscalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]);\n\t\tif(norm < MTK::tolerance()){\n\t \tres = Eigen::Matrix::Zero();\n\t \tres(0, 1) = 1;\n\t \tres(1, 2) = 1;\n \tres /= length;\n\t\t}\n\t\telse{\n\t\t\tres << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0,\n \t -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm);\n \tres /= length;\n \t}\t\n\t}\n\ntemplate\nEigen::Matrix A_matrix(const Base & v){\n Eigen::Matrix res;\n double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];\n\tdouble norm = std::sqrt(squaredNorm);\n\tif(norm < MTK::tolerance()){\n\t\tres = Eigen::Matrix::Identity();\n\t}\n\telse{\n\t\tres = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);\n\t}\n return res;\n}\n\ntemplate\nscalar exp(vectview result, vectview vec, const scalar& scale = 1) {\n\tscalar norm2 = vec.squaredNorm();\n\tstd::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2);\n\tscalar mult = cos_sinc.second * scale; \n\tresult = mult * vec;\n\treturn cos_sinc.first;\n}\n\n\n/**\n * Inverse function to @c exp.\n * \n * @param result @c vectview to the result\n * @param w scalar part of input\n * @param vec vector part of input\n * @param scale scale result by this value\n * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result \n */\ntemplate\nvoid log(vectview result,\n\t\tconst scalar &w, const vectview vec,\n\t\tconst scalar &scale, bool plus_minus_periodicity)\n{\n\t// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division\n\tscalar nv = vec.norm();\n\tif(nv < tolerance()) {\n\t\tif(!plus_minus_periodicity && w < 0) {\n\t\t\t// find the maximal entry:\n\t\t\tint i;\n\t\t\tnv = vec.cwiseAbs().maxCoeff(&i);\n\t\t\tresult = scale * std::atan2(nv, w) * vect::Unit(i);\n\t\t\treturn;\n\t\t}\n\t\tnv = tolerance();\n\t}\n\tscalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );\n\t\n\tresult = s * vec;\n}\n\n\n} // namespace MTK\n\n\n#endif /* MTKMATH_H_ */\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp", "content": "\n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/src/vectview.hpp\n * @brief Wrapper class around a pointer used as interface for plain vectors.\n */\n\n#ifndef VECTVIEW_HPP_\n#define VECTVIEW_HPP_\n\n#include \n\nnamespace MTK {\n\n/**\n * A view to a vector.\n * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.\n * The dimension of the vector is given as template parameter and type-checked when used in expressions.\n * Data has to be modifiable.\n * \n * @tparam scalar Scalar type of the vector.\n * @tparam dim Dimension of the vector.\n * \n * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct\n */\nnamespace internal {\n\ttemplate\n\tstruct CovBlock {\n\t\ttypedef typename Eigen::Block, T1::DOF, T2::DOF> Type;\n\t\ttypedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType;\n\t};\n\n\ttemplate\n\tstruct CovBlock_ {\n\t\ttypedef typename Eigen::Block, T1::DIM, T2::DIM> Type;\n\t\ttypedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType;\n\t};\n\n\ttemplate\n\tstruct CrossCovBlock {\n\t\ttypedef typename Eigen::Block, T1::DOF, T2::DOF> Type;\n\t\ttypedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType;\n\t};\n\n\ttemplate\n\tstruct CrossCovBlock_ {\n\t\ttypedef typename Eigen::Block, T1::DIM, T2::DIM> Type;\n\t\ttypedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType;\n\t};\n\n\ttemplate\n\tstruct VectviewBase {\n\t\ttypedef Eigen::Matrix matrix_type;\n\t\ttypedef typename matrix_type::MapType Type;\n\t\ttypedef typename matrix_type::ConstMapType ConstType;\n\t};\n\n\ttemplate\n\tstruct UnalignedType {\n\t\ttypedef T type;\n\t};\n}\n\ntemplate\nclass vectview : public internal::VectviewBase::Type {\n\ttypedef internal::VectviewBase VectviewBase;\npublic:\n\t//! plain matrix type\n\ttypedef typename VectviewBase::matrix_type matrix_type;\n\t//! base type\n\ttypedef typename VectviewBase::Type base;\n\t//! construct from pointer\n\texplicit\n\tvectview(scalar* data, int dim_=dim) : base(data, dim_) {}\n\t//! construct from plain matrix\n\tvectview(matrix_type& m) : base(m.data(), m.size()) {}\n\t//! construct from another @c vectview\n\tvectview(const vectview &v) : base(v) {}\n\t//! construct from Eigen::Block:\n\ttemplate\n\tvectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {}\n\ttemplate\n\tvectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {}\n\n\t//! inherit assignment operator\n\tusing base::operator=;\n\t//! data pointer\n\tscalar* data() {return const_cast(base::data());}\n};\n\n/**\n * @c const version of @c vectview.\n * Compared to @c Eigen::Map this implementation is const correct, i.e.,\n * data will not be modifiable using this view.\n * \n * @tparam scalar Scalar type of the vector.\n * @tparam dim Dimension of the vector.\n * \n * @sa vectview\n */\ntemplate\nclass vectview : public internal::VectviewBase::ConstType {\n\ttypedef internal::VectviewBase VectviewBase;\npublic:\n\t//! plain matrix type\n\ttypedef typename VectviewBase::matrix_type matrix_type;\n\t//! base type\n\ttypedef typename VectviewBase::ConstType base;\n\t//! construct from const pointer\n\texplicit\n\tvectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}\n\t//! construct from column vector\n\ttemplate\n\tvectview(const Eigen::Matrix& m) : base(m.data()) {}\n\t//! construct from row vector\n\ttemplate\n\tvectview(const Eigen::Matrix& m) : base(m.data()) {}\n\t//! construct from another @c vectview\n\tvectview(vectview x) : base(x.data()) {}\n\t//! construct from base\n\tvectview(const base &x) : base(x) {}\n\t/**\n\t * Construct from Block\n\t * @todo adapt this, when Block gets const-correct\n\t */\n\ttemplate\n\tvectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {}\n\ttemplate\n\tvectview(Eigen::Block block) : base(&block.coeffRef(0)) {}\n\n};\n\n\n} // namespace MTK\n\n#endif /* VECTVIEW_HPP_ */\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/startIdx.hpp", "content": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.\n// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119\n\n/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Modifier: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n \n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/startIdx.hpp \n * @brief Tools to access sub-elements of compound manifolds.\n */\n#ifndef GET_START_INDEX_H_\n#define GET_START_INDEX_H_\n\n#include \n\n#include \"src/SubManifold.hpp\"\n#include \"src/vectview.hpp\"\n\nnamespace MTK {\n\n\n/** \n * \\defgroup SubManifolds Accessing Submanifolds\n * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers\n * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix.\n * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers\n * @c &pose::orient and @c &pose::trans give all required information and are still\n * valid if the base type gets extended or the actual types of @a orient and @a trans\n * change (e.g. from 2D to 3D).\n * \n * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc.\n */\n//@{\n\n/**\n * Determine the index of a sub-variable within a compound variable.\n */\ntemplate \nint getStartIdx( MTK::SubManifold Base::*)\n{\n\treturn idx;\n}\n\ntemplate \nint getStartIdx_( MTK::SubManifold Base::*)\n{\n\treturn dim;\n}\n\n/**\n * Determine the degrees of freedom of a sub-variable within a compound variable.\n */\ntemplate \nint getDof( MTK::SubManifold Base::*)\n{\n\treturn T::DOF;\n}\ntemplate \nint getDim( MTK::SubManifold Base::*)\n{\n\treturn T::DIM;\n}\n\n/**\n * set the diagonal elements of a covariance matrix corresponding to a sub-variable\n */\ntemplate \nvoid setDiagonal(Eigen::Matrix &cov, \n\t\tMTK::SubManifold Base::*, const typename Base::scalar &val)\n{\n\tcov.diagonal().template segment(idx).setConstant(val);\n}\n\ntemplate \nvoid setDiagonal_(Eigen::Matrix &cov, \n\t\tMTK::SubManifold Base::*, const typename Base::scalar &val)\n{\n\tcov.diagonal().template segment(dim).setConstant(val);\n}\n\n/**\n * Get the subblock of corresponding to two members, i.e.\n * \\code\n * Eigen::Matrix m;\n * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression;\n * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans();\n * \\endcode\n * lets you modify mixed covariance entries in a bigger covariance matrix.\n */\ntemplate\ntypename MTK::internal::CovBlock::Type\nsubblock(Eigen::Matrix &cov, \n\t\tMTK::SubManifold Base::*, MTK::SubManifold Base::*)\n{\n\treturn cov.template block(idx1, idx2);\n}\n\ntemplate\ntypename MTK::internal::CovBlock_::Type\nsubblock_(Eigen::Matrix &cov, \n\t\tMTK::SubManifold Base::*, MTK::SubManifold Base::*)\n{\n\treturn cov.template block(dim1, dim2);\n}\n\ntemplate\ntypename MTK::internal::CrossCovBlock::Type\nsubblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*)\n{\n\treturn cov.template block(idx1, idx2);\n}\n\ntemplate\ntypename MTK::internal::CrossCovBlock_::Type\nsubblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*)\n{\n\treturn cov.template block(dim1, dim2);\n}\n/**\n * Get the subblock of corresponding to a member, i.e.\n * \\code\n * Eigen::Matrix m;\n * MTK::subblock(m, &Pose::orient) = some_expression;\n * \\endcode\n * lets you modify covariance entries in a bigger covariance matrix.\n */\ntemplate\ntypename MTK::internal::CovBlock_::Type\nsubblock_(Eigen::Matrix &cov, \n\t\tMTK::SubManifold Base::*)\n{\n\treturn cov.template block(dim, dim);\n}\n\ntemplate\ntypename MTK::internal::CovBlock::Type\nsubblock(Eigen::Matrix &cov, \n\t\tMTK::SubManifold Base::*)\n{\n\treturn cov.template block(idx, idx);\n}\n\ntemplate\nclass get_cov { \npublic:\n typedef Eigen::Matrix type;\n typedef const Eigen::Matrix const_type;\n};\n\ntemplate\nclass get_cov_ { \npublic:\n typedef Eigen::Matrix type;\n typedef const Eigen::Matrix const_type;\n};\n\ntemplate\nclass get_cross_cov {\npublic:\n typedef Eigen::Matrix type;\n typedef const type const_type;\n};\n\ntemplate\nclass get_cross_cov_ {\npublic:\n typedef Eigen::Matrix type;\n typedef const type const_type;\n};\n\n\ntemplate\nvectview\nsubvector_impl_(vectview vec, SubManifold Base::*)\n{\n\treturn vec.template segment(dim);\n}\n\ntemplate\nvectview\nsubvector_impl(vectview vec, SubManifold Base::*)\n{\n\treturn vec.template segment(idx);\n}\n\n/**\n * Get the subvector corresponding to a sub-manifold from a bigger vector.\n */\n template\nvectview\nsubvector_(vectview vec, SubManifold Base::* ptr)\n{\n\treturn subvector_impl_(vec, ptr);\n}\n\ntemplate\nvectview\nsubvector(vectview vec, SubManifold Base::* ptr)\n{\n\treturn subvector_impl(vec, ptr);\n}\n\n/**\n * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*)\n */\ntemplate\nvectview\nsubvector(Eigen::Matrix& vec, SubManifold Base::* ptr)\n{\n\treturn subvector_impl(vectview(vec), ptr);\n}\n \ntemplate\nvectview\nsubvector_(Eigen::Matrix& vec, SubManifold Base::* ptr)\n{\n\treturn subvector_impl_(vectview(vec), ptr);\n}\n\ntemplate\nvectview\nsubvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr)\n{\n\treturn subvector_impl_(vectview(vec), ptr);\n}\n\ntemplate\nvectview\nsubvector(const Eigen::Matrix& vec, SubManifold Base::* ptr)\n{\n\treturn subvector_impl(vectview(vec), ptr);\n}\n\n\n/**\n * const version of subvector(vectview vec,SubManifold Base::*)\n */\ntemplate\nvectview\nsubvector_impl(const vectview cvec, SubManifold Base::*)\n{\n\treturn cvec.template segment(idx);\n}\n\ntemplate\nvectview\nsubvector_impl_(const vectview cvec, SubManifold Base::*)\n{\n\treturn cvec.template segment(dim);\n}\n\ntemplate\nvectview\nsubvector(const vectview cvec, SubManifold Base::* ptr)\n{\n\treturn subvector_impl(cvec, ptr);\n}\n\n\n} // namespace MTK\n\n#endif // GET_START_INDEX_H_\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/S2.hpp", "content": "// This is a NEW implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.\n// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119\n\n/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Modifier: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n\n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/types/S2.hpp\n * @brief Unit vectors on the sphere, or directions in 3D.\n */\n#ifndef S2_H_\n#define S2_H_\n\n\n#include \"vect.hpp\"\n\n#include \"SOn.hpp\"\n#include \"../src/mtkmath.hpp\"\n\n\n\n\nnamespace MTK {\n\n/**\n * Manifold representation of @f$ S^2 @f$. \n * Used for unit vectors on the sphere or directions in 3D.\n * \n * @todo add conversions from/to polar angles?\n */\ntemplate\nstruct S2 {\n\t\n\ttypedef _scalar scalar;\n\ttypedef vect<3, scalar> vect_type; \n\ttypedef SO3 SO3_type;\n\ttypedef typename vect_type::base vec3; \n\tscalar length = scalar(den)/scalar(num);\n\tenum {DOF=2, TYP = 1, DIM = 3};\n\t\n//private:\n\t/**\n\t * Unit vector on the sphere, or vector pointing in a direction\n\t */\n\tvect_type vec; \n\t\npublic:\n\tS2() {\n\t\tif(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1));\n\t\tif(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0);\n\t\tif(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0);\n\t} \n\tS2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { \n\t\tvec.normalize();\n\t\tvec = vec * length;\n\t}\n\t\n\tS2(const vect_type &_vec) : vec(_vec) {\n\t\tvec.normalize();\n\t\tvec = vec * length;\n\t}\n\n\tvoid oplus(MTK::vectview delta, scalar scale = 1)\n\t{\n\t\tSO3_type res;\n\t\tres.w() = MTK::exp(res.vec(), delta, scalar(scale/2));\n\t\tvec = res.toRotationMatrix() * vec;\n\t}\n\t\n\tvoid boxplus(MTK::vectview delta, scalar scale=1) {\n\t\tEigen::Matrix Bx;\n\t\tS2_Bx(Bx);\n\t\tvect_type Bu = Bx*delta;SO3_type res;\n\t\tres.w() = MTK::exp(res.vec(), Bu, scalar(scale/2));\n\t\tvec = res.toRotationMatrix() * vec;\n\t} \n\t\n\tvoid boxminus(MTK::vectview res, const S2& other) const {\n\t\tscalar v_sin = (MTK::hat(vec)*other.vec).norm();\n\t\tscalar v_cos = vec.transpose() * other.vec;\n\t\tscalar theta = std::atan2(v_sin, v_cos);\n\t\tif(v_sin < MTK::tolerance())\n\t\t{\n\t\t\tif(std::fabs(theta) > MTK::tolerance() )\n\t\t\t{\n\t\t\t\tres[0] = 3.1415926;\n\t\t\t\tres[1] = 0;\n\t\t\t}\n\t\t\telse{\n\t\t\t\tres[0] = 0;\n\t\t\t\tres[1] = 0;\n\t\t\t}\n\t\t}\n\t\telse\n\t\t{\n\t\t\tS2 other_copy = other;\n\t\t\tEigen::MatrixBx;\n\t\t\tother_copy.S2_Bx(Bx);\n\t\t\tres = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec;\n\t\t}\n\t}\n\t\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tEigen::Matrix skew_vec;\n\t\tskew_vec << scalar(0), -vec[2], vec[1],\n\t\t\t\t\t\t\t\tvec[2], scalar(0), -vec[0],\n\t\t\t\t\t\t\t\t-vec[1], vec[0], scalar(0);\n\t\tres = skew_vec;\n\t}\n\n\n\tvoid S2_Bx(Eigen::Matrix &res)\n\t{\n\t\tif(S2_typ == 3)\n\t\t{\n\t\tif(vec[2] + length > tolerance())\n\t\t{\n\t\t\t\n\t\t\tres << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]),\n\t\t\t\t\t -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]),\n\t\t\t\t\t -vec[0], -vec[1];\n\t\t\tres /= length;\n\t\t}\n\t\telse\n\t\t{\n\t\t\tres = Eigen::Matrix::Zero();\n\t\t\tres(1, 1) = -1;\n\t\t\tres(2, 0) = 1;\n\t\t}\n\t\t}\n\t\telse if(S2_typ == 2)\n\t\t{\n\t\tif(vec[1] + length > tolerance())\n\t\t{\n\t\t\t\n\t\t\tres << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]),\n\t\t\t\t\t -vec[0], -vec[2],\n\t\t\t\t\t -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]);\n\t\t\tres /= length;\n\t\t}\n\t\telse\n\t\t{\n\t\t\tres = Eigen::Matrix::Zero();\n\t\t\tres(1, 1) = -1;\n\t\t\tres(2, 0) = 1;\n\t\t}\n\t\t}\n\t\telse\n\t\t{\n\t\tif(vec[0] + length > tolerance())\n\t\t{\n\t\t\t\n\t\t\tres << -vec[1], -vec[2],\n\t\t\t\t\t length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]),\n\t\t\t\t\t -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]);\n\t\t\tres /= length;\n\t\t}\n\t\telse\n\t\t{\n\t\t\tres = Eigen::Matrix::Zero();\n\t\t\tres(1, 1) = -1;\n\t\t\tres(2, 0) = 1;\n\t\t}\n\t\t}\n\t}\n\n\tvoid S2_Nx(Eigen::Matrix &res, S2& subtrahend)\n\t{\n\t\tif((vec+subtrahend.vec).norm() > tolerance())\n\t\t{\n\t\t\tEigen::Matrix Bx;\n\t\t\tS2_Bx(Bx);\n\t\t\tif((vec-subtrahend.vec).norm() > tolerance())\n\t\t\t{\n\t\t\t\tscalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm();\n\t\t\t\tscalar v_cos = vec.transpose() * subtrahend.vec;\n\t\t\t\t\n\t\t\t\tres = 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));\n\t\t\t}\n\t\t\telse\n\t\t\t{\n\t\t\t\tres = 1/length/length*Bx.transpose()*MTK::hat(vec);\n\t\t\t}\n\t\t}\n\t\telse\n\t\t{\n\t\t\tstd::cerr << \"No N(x, y) for x=-y\" << std::endl;\n\t\t\tstd::exit(100);\n\t\t}\n\t}\n\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tEigen::Matrix Bx;\n\t\tS2_Bx(Bx);\n\t\tres = 1/length/length*Bx.transpose()*MTK::hat(vec);\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tEigen::Matrix Bx;\n\t\tS2_Bx(Bx);\n\t\tif(delta.norm() < tolerance())\n\t\t{\n\t\t\tres = -MTK::hat(vec)*Bx;\n\t\t}\n\t\telse{\n\t\t\tvect_type Bu = Bx*delta;\n\t\t\tSO3_type exp_delta;\n\t\t\texp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2));\n\t\t\tres = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx;\n\t\t}\n\t}\n\n\toperator const vect_type&() const{\n\t\treturn vec;\n\t}\n\t\n\tconst vect_type& get_vect() const {\n\t\treturn vec;\n\t}\n\t\n\tfriend S2 operator*(const SO3& rot, const S2& dir)\n\t{\n\t\tS2 ret;\n\t\tret.vec = rot * dir.vec;\n\t\treturn ret;\n\t}\n\t\n\tscalar operator[](int idx) const {return vec[idx]; }\n\t\n\tfriend std::ostream& operator<<(std::ostream &os, const S2& vec){\n\t\treturn os << vec.vec.transpose() << \" \";\n\t}\n\tfriend std::istream& operator>>(std::istream &is, S2& vec){\n\t\tfor(int i=0; i<3; ++i)\n\t\t\tis >> vec.vec[i];\n\t\tvec.vec.normalize();\n\t\tvec.vec = vec.vec * vec.length;\n\t\treturn is;\n\t\n\t}\n};\n\n\n} // namespace MTK\n\n\n#endif /*S2_H_*/\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp", "content": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.\n// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119\n\n/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Modifier: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n \n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/types/SOn.hpp\n * @brief Standard Orthogonal Groups i.e.\\ rotatation groups.\n */\n#ifndef SON_H_\n#define SON_H_\n\n#include \n\n#include \"vect.hpp\"\n#include \"../src/mtkmath.hpp\"\n\n\nnamespace MTK {\n\n\n/**\n * Two-dimensional orientations represented as scalar.\n * There is no guarantee that the representing scalar is within any interval,\n * but the result of boxminus will always have magnitude @f$\\le\\pi @f$.\n */\ntemplate\nstruct SO2 : public Eigen::Rotation2D<_scalar> {\n\tenum {DOF = 1, DIM = 2, TYP = 3};\n\t\n\ttypedef _scalar scalar;\n\ttypedef Eigen::Rotation2D base;\n\ttypedef vect vect_type;\n\t\n\t//! Construct from angle\n\tSO2(const scalar& angle = 0) : base(angle) {\t}\n\t\n\t//! Construct from Eigen::Rotation2D\n\tSO2(const base& src) : base(src) {}\n\t\n\t/**\n\t * Construct from 2D vector.\n\t * Resulting orientation will rotate the first unit vector to point to vec.\n\t */\n\tSO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};\n\t\n\t\n\t//! Calculate @c this->inverse() * @c r\n\tSO2 operator%(const base &r) const {\n\t\treturn base::inverse() * r;\n\t}\n\n\t//! Calculate @c this->inverse() * @c r\n\ttemplate\n\tvect_type operator%(const Eigen::MatrixBase &vec) const {\n\t\treturn base::inverse() * vec;\n\t}\n\t\n\t//! Calculate @c *this * @c r.inverse()\n\tSO2 operator/(const SO2 &r) const {\n\t\treturn *this * r.inverse();\n\t}\n\t\n\t//! Gets the angle as scalar.\n\toperator scalar() const {\n\t\treturn base::angle(); \n\t}\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tres = Eigen::Matrix::Zero();\n\t}\n\t//! @name Manifold requirements\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid oplus(MTK::vectview vec, scalar scale = 1) {\n\t\tbase::angle() += scale * vec[0];\n\t}\n\t\n\tvoid boxplus(MTK::vectview vec, scalar scale = 1) {\n\t\tbase::angle() += scale * vec[0];\n\t}\n\tvoid boxminus(MTK::vectview res, const SO2& other) const {\n\t\tres[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));\n\t}\n\t\n\tfriend std::istream& operator>>(std::istream &is, SO2& ang){\n\t\treturn is >> ang.angle();\n\t}\n\n};\n\n\n/**\n * Three-dimensional orientations represented as Quaternion.\n * It is assumed that the internal Quaternion always stays normalized,\n * should this not be the case, call inherited member function @c normalize().\n */\ntemplate\nstruct SO3 : public Eigen::Quaternion<_scalar, Options> {\n\tenum {DOF = 3, DIM = 3, TYP = 2}; \n\ttypedef _scalar scalar;\n\ttypedef Eigen::Quaternion base;\n\ttypedef Eigen::Quaternion Quaternion;\n\ttypedef vect vect_type;\n\t\n\t//! Calculate @c this->inverse() * @c r\n\ttemplate EIGEN_STRONG_INLINE \n\tQuaternion operator%(const Eigen::QuaternionBase &r) const {\n\t\treturn base::conjugate() * r;\n\t}\n\t\n\t//! Calculate @c this->inverse() * @c r\n\ttemplate\n\tvect_type operator%(const Eigen::MatrixBase &vec) const {\n\t\treturn base::conjugate() * vec;\n\t}\n\t\n\t//! Calculate @c this * @c r.conjugate()\n\ttemplate EIGEN_STRONG_INLINE \n\tQuaternion operator/(const Eigen::QuaternionBase &r) const {\n\t\treturn *this * r.conjugate();\n\t}\n\t\n\t/**\n\t * Construct from real part and three imaginary parts.\n\t * Quaternion is normalized after construction.\n\t */\n\tSO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) {\n\t\tbase::normalize();\n\t}\n\t\n\t/**\n\t * Construct from Eigen::Quaternion.\n\t * @note Non-normalized input may result result in spurious behavior.\n\t */\n\tSO3(const base& src = base::Identity()) : base(src) {}\n\t\n\t/**\n\t * Construct from rotation matrix.\n\t * @note Invalid rotation matrices may lead to spurious behavior.\n\t */\n\ttemplate\n\tSO3(const Eigen::MatrixBase& matrix) : base(matrix) {}\n\t\n\t/**\n\t * Construct from arbitrary rotation type.\n\t * @note Invalid rotation matrices may lead to spurious behavior.\n\t */\n\ttemplate\n\tSO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {}\n\t\n\t//! @name Manifold requirements\n\t\n\tvoid boxplus(MTK::vectview vec, scalar scale=1) {\n\t\tSO3 delta = exp(vec, scale);\n\t\t*this = *this * delta;\n\t}\n\tvoid boxminus(MTK::vectview res, const SO3& other) const {\n\t\tres = SO3::log(other.conjugate() * *this);\n\t}\n\t//}\n\n\tvoid oplus(MTK::vectview vec, scalar scale=1) {\n\t\tSO3 delta = exp(vec, scale);\n\t\t*this = *this * delta;\n\t}\n\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tres = Eigen::Matrix::Zero();\n\t}\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tfriend std::ostream& operator<<(std::ostream &os, const SO3& q){\n\t\treturn os << q.coeffs().transpose() << \" \";\n\t}\n\n\tfriend std::istream& operator>>(std::istream &is, SO3& q){\n\t\tvect<4,scalar> coeffs;\n\t\tis >> coeffs;\n\t\tq.coeffs() = coeffs.normalized();\n\t\treturn is;\n\t}\n\t\n\t//! @name Helper functions\n\t//{\n\t/**\n\t * Calculate the exponential map. In matrix terms this would correspond \n\t * to the Rodrigues formula.\n\t */\n\t// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround\n//\tstatic SO3 exp(MTK::vectview dvec, scalar scale = 1){\n\tstatic SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){\n\t\tSO3 res;\n\t\tres.w() = MTK::exp(res.vec(), dvec, scalar(scale/2));\n\t\treturn res;\n\t}\n\t/**\n\t * Calculate the inverse of @c exp.\n\t * Only guarantees that exp(log(x)) == x \n\t */\n\tstatic typename base::Vector3 log(const SO3 &orient){\n\t\ttypename base::Vector3 res;\n\t\tMTK::log(res, orient.w(), orient.vec(), scalar(2), true);\n\t\treturn res;\n\t}\n};\n\nnamespace internal {\ntemplate\nstruct UnalignedType >{\n\ttypedef SO2 type;\n};\n\ntemplate\nstruct UnalignedType >{\n\ttypedef SO3 type;\n};\n\n} // namespace internal\n\n\n} // namespace MTK\n\n#endif /*SON_H_*/\n\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/vect.hpp", "content": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.\n// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119\n\n/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Modifier: Dongjiao HE \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n\n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the names of its\n * contributors may be used to endorse or promote products derived\n * from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n/**\n * @file mtk/types/vect.hpp\n * @brief Basic vectors interpreted as manifolds.\n * \n * This file also implements a simple wrapper for matrices, for arbitrary scalars\n * and for positive scalars.\n */\n#ifndef VECT_H_\n#define VECT_H_\n\n#include \n#include \n#include \n\n#include \"../src/vectview.hpp\"\n\nnamespace MTK {\n\nstatic const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, \",\", \",\", \"\", \"\", \"[\", \"]\"); \n\n\n/**\n * A simple vector class.\n * Implementation is basically a wrapper around Eigen::Matrix with manifold \n * requirements added.\n */\ntemplate\nstruct vect : public Eigen::Matrix<_scalar, D, 1, _Options> {\n\ttypedef Eigen::Matrix<_scalar, D, 1, _Options> base;\n\tenum {DOF = D, DIM = D, TYP = 0};\n\ttypedef _scalar scalar;\n\t\n\t//using base::operator=;\n\t\n\t/** Standard constructor. Sets all values to zero. */\n\tvect(const base &src = base::Zero()) : base(src) {}\n\n\t/** Constructor copying the value of the expression \\a other */\n\ttemplate\n\tEIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {}\n\t\n\t/** Construct from memory. */\n\tvect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { }\n\n\tvoid boxplus(MTK::vectview vec, scalar scale=1) {\n\t\t*this += scale * vec;\n\t}\n\tvoid boxminus(MTK::vectview res, const vect& other) const {\n\t\tres = *this - other;\n\t}\n\n\tvoid oplus(MTK::vectview vec, scalar scale=1) {\n\t\t*this += scale * vec;\n\t}\n\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tfriend std::ostream& operator<<(std::ostream &os, const vect& v){\n\t\t// Eigen sometimes messes with the streams flags, so output manually:\n\t\tfor(int i=0; i>(std::istream &is, vect& v){\n\t\tchar term=0;\n\t\tis >> std::ws; // skip whitespace\n\t\tswitch(is.peek()) {\n\t\tcase '(': term=')'; is.ignore(1); break;\n\t\tcase '[': term=']'; is.ignore(1); break;\n\t\tcase '{': term='}'; is.ignore(1); break;\n\t\tdefault: break;\n\t\t}\n\t\tif(D==Eigen::Dynamic) { \n\t\t\tassert(term !=0 && \"Dynamic vectors must be embraced\");\n\t\t\tstd::vector temp;\n\t\t\twhile(is.good() && is.peek() != term) {\n\t\t\t\tscalar x;\n\t\t\t\tis >> x;\n\t\t\t\ttemp.push_back(x);\n\t\t\t\tif(is.peek()==',') is.ignore(1);\n\t\t\t}\n\t\t\tv = vect::Map(temp.data(), temp.size());\n\t\t} else\n\t\t\tfor(int i=0; i> v[i];\n\t\t\t\tif(is.peek()==',') { // ignore commas between values\n\t\t\t\t\tis.ignore(1);\n\t\t\t\t}\n\t\t\t}\n\t\tif(term!=0) {\n\t\t\tchar x;\n\t\t\tis >> x;\n\t\t\tif(x!=term) {\n\t\t\t\tis.setstate(is.badbit);\n//\t\t\t\tassert(x==term && \"start and end bracket do not match!\");\n\t\t\t}\n\t\t}\n\t\treturn is;\n\t}\n\t\n\ttemplate\n\tvectview tail(){\n\t\tBOOST_STATIC_ASSERT(0< dim && dim <= DOF);\n\t\treturn base::template tail();\n\t}\n\ttemplate\n\tvectview tail() const{\n\t\tBOOST_STATIC_ASSERT(0< dim && dim <= DOF);\n\t\treturn base::template tail();\n\t}\n\ttemplate\n\tvectview head(){\n\t\tBOOST_STATIC_ASSERT(0< dim && dim <= DOF);\n\t\treturn base::template head();\n\t}\n\ttemplate\n\tvectview head() const{\n\t\tBOOST_STATIC_ASSERT(0< dim && dim <= DOF);\n\t\treturn base::template head();\n\t}\n};\n\n\n/**\n * A simple matrix class.\n * Implementation is basically a wrapper around Eigen::Matrix with manifold \n * requirements added, i.e., matrix is viewed as a plain vector for that.\n */\ntemplate::Options>\nstruct matrix : public Eigen::Matrix<_scalar, M, N, _Options> {\n\ttypedef Eigen::Matrix<_scalar, M, N, _Options> base; \n\tenum {DOF = M * N, TYP = 4, DIM=0};\n\ttypedef _scalar scalar;\n\t\n\tusing base::operator=; \n\t\n\t/** Standard constructor. Sets all values to zero. */\n\tmatrix() {\n\t\tbase::setZero();\n\t}\n\t\n\t/** Constructor copying the value of the expression \\a other */\n\ttemplate\n\tEIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {}\n\t\n\t/** Construct from memory. */\n\tmatrix(const scalar* src) : base(src) { } \n\t\n\tvoid boxplus(MTK::vectview vec, scalar scale = 1) {\n\t\t*this += scale * base::Map(vec.data());\n\t}\n\tvoid boxminus(MTK::vectview res, const matrix& other) const {\n\t\tbase::Map(res.data()) = *this - other;\n\t}\n\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid oplus(MTK::vectview vec, scalar scale = 1) {\n\t\t*this += scale * base::Map(vec.data());\n\t}\n\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tfriend std::ostream& operator<<(std::ostream &os, const matrix& mat){\n\t\tfor(int i=0; i>(std::istream &is, matrix& mat){\n\t\tfor(int i=0; i> mat.data()[i];\n\t\t}\n\t\treturn is;\n\t}\n};// @todo What if M / N = Eigen::Dynamic?\n\n\n\n/**\n * A simple scalar type.\n */\ntemplate\nstruct Scalar {\n\tenum {DOF = 1, TYP = 5, DIM=0};\n\ttypedef _scalar scalar;\n\t\n\tscalar value;\n\t\n\tScalar(const scalar& value = scalar(0)) : value(value) {}\n\toperator const scalar&() const { return value; }\n\toperator scalar&() { return value; }\n\tScalar& operator=(const scalar& val) { value = val; return *this; }\n\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid oplus(MTK::vectview vec, scalar scale=1) {\n\t\tvalue += scale * vec[0];\n\t}\n\n\tvoid boxplus(MTK::vectview vec, scalar scale=1) {\n\t\tvalue += scale * vec[0];\n\t}\n\tvoid boxminus(MTK::vectview res, const Scalar& other) const {\n\t\tres[0] = *this - other;\n\t}\n};\n\n/**\n * Positive scalars.\n * Boxplus is implemented using multiplication by @f$x\\boxplus\\delta = x\\cdot\\exp(\\delta) @f$.\n */\ntemplate\nstruct PositiveScalar {\n\tenum {DOF = 1, TYP = 6, DIM=0};\n\ttypedef _scalar scalar;\n\t\n\tscalar value;\n\t\n\tPositiveScalar(const scalar& value = scalar(1)) : value(value) {\n\t\tassert(value > scalar(0));\n\t}\n\toperator const scalar&() const { return value; }\n\tPositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; }\n\n\tvoid boxplus(MTK::vectview vec, scalar scale = 1) {\n\t\tvalue *= std::exp(scale * vec[0]);\n\t}\n\tvoid boxminus(MTK::vectview res, const PositiveScalar& other) const {\n\t\tres[0] = std::log(*this / other);\n\t}\n\n\tvoid oplus(MTK::vectview vec, scalar scale = 1) {\n\t\tvalue *= std::exp(scale * vec[0]);\n\t}\n\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\n\tfriend std::istream& operator>>(std::istream &is, PositiveScalar& s){\n\t\tis >> s.value;\n\t\tassert(s.value > 0);\n\t\treturn is;\n\t}\n};\n\ntemplate\nstruct Complex : public std::complex<_scalar>{\n\tenum {DOF = 2, TYP = 7, DIM=0};\n\ttypedef _scalar scalar;\n\t\n\ttypedef std::complex Base;\n\t\n\tComplex(const Base& value) : Base(value) {}\n\tComplex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {}\n\tComplex(const MTK::vectview &in) : Base(in[0], in[1]) {}\n\ttemplate\n\tComplex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {}\n\t\n\tvoid boxplus(MTK::vectview vec, scalar scale = 1) {\n\t\tBase::real() += scale * vec[0];\n\t\tBase::imag() += scale * vec[1];\n\t};\n\tvoid boxminus(MTK::vectview res, const Complex& other) const {\n\t\tComplex diff = *this - other;\n\t\tres << diff.real(), diff.imag();\n\t}\n\n\tvoid S2_hat(Eigen::Matrix &res)\n\t{\n\t\tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid oplus(MTK::vectview vec, scalar scale = 1) {\n\t\tBase::real() += scale * vec[0];\n\t\tBase::imag() += scale * vec[1];\n\t};\n\n\tvoid S2_Nx_yy(Eigen::Matrix &res)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\n\tvoid S2_Mx(Eigen::Matrix &res, MTK::vectview delta)\n\t{\n\t\tstd::cerr << \"wrong idx for S2\" << std::endl;\n\t\tstd::exit(100);\t\n \tres = Eigen::Matrix::Zero();\n\t}\n\t\n\tscalar squaredNorm() const {\n\t\treturn std::pow(Base::real(),2) + std::pow(Base::imag(),2);\n\t}\n\t\n\tconst scalar& operator()(int i) const {\n\t\tassert(0<=i && i<2 && \"Index out of range\");\n\t\treturn i==0 ? Base::real() : Base::imag();\n\t}\n\tscalar& operator()(int i){\n\t\tassert(0<=i && i<2 && \"Index out of range\");\n\t\treturn i==0 ? Base::real() : Base::imag();\n\t}\n};\n\n\nnamespace internal {\n\ntemplate\nstruct UnalignedType >{\n\ttypedef vect type;\n};\n\n} // namespace internal\n\n\n} // namespace MTK\n\n\n\n\n#endif /*VECT_H_*/\n" }, { "path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp", "content": "/*\n * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH\n * All rights reserved.\n *\n * Author: Rene Wagner \n * Christoph Hertzberg \n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * * Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * * Redistributions in binary form must reproduce the above\n * copyright notice, this list of conditions and the following\n * disclaimer in the documentation and/or other materials provided\n * with the distribution.\n * * Neither the name of the Universitaet Bremen nor the DFKI GmbH \n * nor the names of its contributors may be used to endorse or \n * promote products derived from this software without specific \n * prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n * \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n * POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef WRAPPED_CV_MAT_HPP_\n#define WRAPPED_CV_MAT_HPP_\n\n#include \n#include \n\nnamespace MTK {\n\ntemplate\nstruct cv_f_type;\n\ntemplate<>\nstruct cv_f_type\n{\n\tenum {value = CV_64F};\n};\n\ntemplate<>\nstruct cv_f_type\n{\n\tenum {value = CV_32F};\n};\n\n/**\n * cv_mat wraps a CvMat around an Eigen Matrix\n */\ntemplate\nclass cv_mat : public matrix\n{\n\ttypedef matrix base_type;\n\tenum {type_ = cv_f_type::value};\n\tCvMat cv_mat_;\n\npublic:\n\tcv_mat()\n\t{\n\t\tcv_mat_ = cvMat(rows, cols, type_, base_type::data());\n\t}\n\n\tcv_mat(const cv_mat& oth) : base_type(oth)\n\t{\n\t\tcv_mat_ = cvMat(rows, cols, type_, base_type::data());\n\t}\n\n\ttemplate\n\tcv_mat(const Eigen::MatrixBase &value) : base_type(value)\n\t{\n\t\tcv_mat_ = cvMat(rows, cols, type_, base_type::data());\n\t}\n\n\ttemplate\n\tcv_mat& operator=(const Eigen::MatrixBase &value)\n\t{\n\t\tbase_type::operator=(value);\n\t\treturn *this;\n\t}\n\n\tcv_mat& operator=(const cv_mat& value)\n\t{\n\t\tbase_type::operator=(value);\n\t\treturn *this;\n\t}\n\t\n\t// FIXME: Maybe overloading operator& is not a good idea ...\n\tCvMat* operator&()\n\t{\n\t\treturn &cv_mat_;\n\t}\n\tconst CvMat* operator&() const\n\t{\n\t\treturn &cv_mat_;\n\t}\n};\n\n} // namespace MTK\n\n#endif /* WRAPPED_CV_MAT_HPP_ */\n" }, { "path": "FAST-LIO/include/common_lib.h", "content": "#ifndef COMMON_LIB_H\n#define COMMON_LIB_H\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nusing namespace std;\nusing namespace Eigen;\n\n#define USE_IKFOM\n\n#define PI_M (3.14159265358)\n#define G_m_s2 (9.81) // 在广东的重力加速度\n#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3)\n#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3)\n#define CUBE_LEN (6.0)\n#define LIDAR_SP_LEN (2)\n#define INIT_COV (1)\n#define NUM_MATCH_POINTS (5)\n#define MAX_MEAS_DIM (10000)\n\n#define VEC_FROM_ARRAY(v) v[0], v[1], v[2]\n#define MAT_FROM_ARRAY(v) v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]\n#define CONSTRAIN(v, min, max) ((v > min) ? ((v < max) ? v : max) : min)\n#define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols()\n#define STD_VEC_FROM_EIGEN(mat) vector(mat.data(), mat.data() + mat.rows() * mat.cols())\n#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + \"Log/\" + name))\n\ntypedef fast_lio::Pose6D Pose6D;\ntypedef pcl::PointXYZINormal PointType;\ntypedef pcl::PointCloud PointCloudXYZI;\ntypedef vector> PointVector;\ntypedef Vector3d V3D;\ntypedef Matrix3d M3D;\ntypedef Vector3f V3F;\ntypedef Matrix3f M3F;\n\n#define MD(a, b) Matrix\n#define VD(a) Matrix\n#define MF(a, b) Matrix\n#define VF(a) Matrix\n\nM3D Eye3d(M3D::Identity());\nM3F Eye3f(M3F::Identity());\nV3D Zero3d(0, 0, 0);\nV3F Zero3f(0, 0, 0);\n\nstruct MeasureGroup // Lidar data and imu dates for the curent process\n{\n MeasureGroup()\n {\n lidar_beg_time = 0.0;\n this->lidar.reset(new PointCloudXYZI());\n };\n double lidar_beg_time;\n PointCloudXYZI::Ptr lidar;\n deque imu;\n};\n\nstruct StatesGroup\n{\n StatesGroup()\n {\n this->rot_end = M3D::Identity();\n this->pos_end = Zero3d;\n this->vel_end = Zero3d;\n this->bias_g = Zero3d;\n this->bias_a = Zero3d;\n this->gravity = Zero3d;\n this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV;\n this->cov.block<9, 9>(9, 9) = MD(9, 9)::Identity() * 0.00001;\n };\n\n StatesGroup(const StatesGroup &b)\n {\n this->rot_end = b.rot_end;\n this->pos_end = b.pos_end;\n this->vel_end = b.vel_end;\n this->bias_g = b.bias_g;\n this->bias_a = b.bias_a;\n this->gravity = b.gravity;\n this->cov = b.cov;\n };\n\n StatesGroup &operator=(const StatesGroup &b)\n {\n this->rot_end = b.rot_end;\n this->pos_end = b.pos_end;\n this->vel_end = b.vel_end;\n this->bias_g = b.bias_g;\n this->bias_a = b.bias_a;\n this->gravity = b.gravity;\n this->cov = b.cov;\n return *this;\n };\n\n StatesGroup operator+(const Matrix &state_add)\n {\n StatesGroup a;\n a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0));\n a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0);\n a.vel_end = this->vel_end + state_add.block<3, 1>(6, 0);\n a.bias_g = this->bias_g + state_add.block<3, 1>(9, 0);\n a.bias_a = this->bias_a + state_add.block<3, 1>(12, 0);\n a.gravity = this->gravity + state_add.block<3, 1>(15, 0);\n a.cov = this->cov;\n return a;\n };\n\n StatesGroup &operator+=(const Matrix &state_add)\n {\n this->rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0));\n this->pos_end += state_add.block<3, 1>(3, 0);\n this->vel_end += state_add.block<3, 1>(6, 0);\n this->bias_g += state_add.block<3, 1>(9, 0);\n this->bias_a += state_add.block<3, 1>(12, 0);\n this->gravity += state_add.block<3, 1>(15, 0);\n return *this;\n };\n\n Matrix operator-(const StatesGroup &b)\n {\n Matrix a;\n M3D rotd(b.rot_end.transpose() * this->rot_end);\n a.block<3, 1>(0, 0) = Log(rotd);\n a.block<3, 1>(3, 0) = this->pos_end - b.pos_end;\n a.block<3, 1>(6, 0) = this->vel_end - b.vel_end;\n a.block<3, 1>(9, 0) = this->bias_g - b.bias_g;\n a.block<3, 1>(12, 0) = this->bias_a - b.bias_a;\n a.block<3, 1>(15, 0) = this->gravity - b.gravity;\n return a;\n };\n\n void resetpose()\n {\n this->rot_end = M3D::Identity();\n this->pos_end = Zero3d;\n this->vel_end = Zero3d;\n }\n\n M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point\n V3D pos_end; // the estimated position at the end lidar point (world frame)\n V3D vel_end; // the estimated velocity at the end lidar point (world frame)\n V3D bias_g; // gyroscope bias\n V3D bias_a; // accelerator bias\n V3D gravity; // the estimated gravity acceleration\n Matrix cov; // states covariance\n};\n\ntemplate \nT rad2deg(T radians)\n{\n return radians * 180.0 / PI_M;\n}\n\ntemplate \nT deg2rad(T degrees)\n{\n return degrees * PI_M / 180.0;\n}\n\ntemplate \nauto set_pose6d(const double t, const Matrix &a, const Matrix &g,\n const Matrix &v, const Matrix &p, const Matrix &R)\n{\n Pose6D rot_kp;\n rot_kp.offset_time = t;\n for (int i = 0; i < 3; i++)\n {\n rot_kp.acc[i] = a(i);\n rot_kp.gyr[i] = g(i);\n rot_kp.vel[i] = v(i);\n rot_kp.pos[i] = p(i);\n for (int j = 0; j < 3; j++)\n rot_kp.rot[i * 3 + j] = R(i, j);\n }\n return move(rot_kp);\n}\n\n/* comment\nplane equation: Ax + By + Cz + D = 0\nconvert to: A/D*x + B/D*y + C/D*z = -1\nsolve: A0*x0 = b0\nwhere A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T\nnormvec: normalized x0\n*/\ntemplate \nbool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num)\n{\n MatrixXf A(point_num, 3);\n MatrixXf b(point_num, 1);\n b.setOnes();\n b *= -1.0f;\n\n for (int j = 0; j < point_num; j++)\n {\n A(j, 0) = point[j].x;\n A(j, 1) = point[j].y;\n A(j, 2) = point[j].z;\n }\n normvec = A.colPivHouseholderQr().solve(b);\n\n for (int j = 0; j < point_num; j++)\n {\n if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)\n {\n return false;\n }\n }\n\n normvec.normalize();\n return true;\n}\n\nfloat calc_dist(PointType p1, PointType p2)\n{\n 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);\n return d;\n}\n\ntemplate \nbool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) //按照点集计算平面方程\n{\n Matrix A; //点集的矩阵\n Matrix b; //点集的值\n A.setZero();\n b.setOnes();\n b *= -1.0f;\n\n for (int j = 0; j < NUM_MATCH_POINTS; j++) //将点集的值和矩阵放入A和b中\n {\n A(j, 0) = point[j].x;\n A(j, 1) = point[j].y;\n A(j, 2) = point[j].z;\n }\n\n Matrix normvec = A.colPivHouseholderQr().solve(b); //求解点集的平面方程\n\n T n = normvec.norm(); //求解点集的平面方程的法向量\n pca_result(0) = normvec(0) / n; //求解点集的平面方程的法向量\n pca_result(1) = normvec(1) / n;\n pca_result(2) = normvec(2) / n;\n pca_result(3) = 1.0 / n;\n\n for (int j = 0; j < NUM_MATCH_POINTS; j++)\n {\n //求解点集的平面方程的法向量与点集的值的积大于阈值，则代表点集不符合平面方程\n if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)\n {\n return false;\n }\n }\n return true;\n}\n\n#endif" }, { "path": "FAST-LIO/include/ikd-Tree/README.md", "content": "# ikd-Tree\nikd-Tree is an incremental k-d tree for robotic applications.\n" }, { "path": "FAST-LIO/include/ikd-Tree/ikd_Tree.cpp", "content": "#include \"ikd_Tree.h\"\n\n/*\nDescription: ikd-Tree: an incremental k-d tree for robotic applications\nAuthor: Yixi Cai\nemail: yixicai@connect.hku.hk\n*/\n\nKD_TREE::KD_TREE(float delete_param, float balance_param, float box_length)\n{\n delete_criterion_param = delete_param;\n balance_criterion_param = balance_param;\n downsample_size = box_length;\n Rebuild_Logger.clear();\n termination_flag = false;\n // 初始化一系列数据互斥锁，并且创建重构树的线程\n start_thread();\n}\n\nKD_TREE::~KD_TREE()\n{\n stop_thread();\n Delete_Storage_Disabled = true;\n delete_tree_nodes(&Root_Node);\n PointVector().swap(PCL_Storage);\n Rebuild_Logger.clear();\n}\n\nvoid KD_TREE::Set_delete_criterion_param(float delete_param) //设置删除标准参数\n{\n delete_criterion_param = delete_param;\n}\n\nvoid KD_TREE::Set_balance_criterion_param(float balance_param) //设置平衡阈值\n{\n balance_criterion_param = balance_param;\n}\n\nvoid KD_TREE::set_downsample_param(float downsample_param) //设置下采样的参数\n{\n downsample_size = downsample_param;\n}\n\nvoid KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) //初始化kd-tree\n{\n Set_delete_criterion_param(delete_param);\n Set_balance_criterion_param(balance_param);\n set_downsample_param(box_length);\n}\n\n// 初始化根节点\nvoid KD_TREE::InitTreeNode(KD_TREE_NODE *root)\n{\n root->point.x = 0.0f; //初始化根节点的坐标\n root->point.y = 0.0f;\n root->point.z = 0.0f;\n root->node_range_x[0] = 0.0f; //初始化根节点的范围\n root->node_range_x[1] = 0.0f;\n root->node_range_y[0] = 0.0f;\n root->node_range_y[1] = 0.0f;\n root->node_range_z[0] = 0.0f;\n root->node_range_z[1] = 0.0f;\n root->division_axis = 0; //初始化根节点的分割轴\n root->father_ptr = nullptr; //初始化根节点的父节点指针\n root->left_son_ptr = nullptr; //初始化根节点的左子指针\n root->right_son_ptr = nullptr; //初始化根节点的右子指针\n root->TreeSize = 0; //初始化根节点的子树大小\n root->invalid_point_num = 0; //初始化根节点的无效点数\n root->down_del_num = 0; //初始化根节点的下采样删除数\n root->point_deleted = false; //初始化根节点的点是否被删除\n root->tree_deleted = false; //初始化根节点的树是否被删除\n root->need_push_down_to_left = false; //初始化根节点的是否需要向左子树下采样\n root->need_push_down_to_right = false; //初始化根节点的是否需要向右子树下采样\n root->point_downsample_deleted = false; //初始化根节点的点是否被下采样删除\n root->working_flag = false; //初始化根节点的工作标志\n // 初始化互斥锁 相当于 pthread_mutex_t root->push_down_mutex_lock = PTHREAD_MUTEX_INITIALIZER\n pthread_mutex_init(&(root->push_down_mutex_lock), NULL);\n}\n\nint KD_TREE::size()\n{\n int s = 0;\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n if (Root_Node != nullptr)\n {\n return Root_Node->TreeSize;\n }\n else\n {\n return 0;\n }\n }\n else\n {\n /*锁操作主要包括加锁pthread_mutex_lock()、解锁pthread_mutex_unlock()和\n 测试加锁 pthread_mutex_trylock()三个，不论哪种类型的锁，都不可能被两个不\n 同的线程同时得到，而必须等待解锁。对于普通锁和适应锁类型，解锁者可以是同进程\n 内任何线程；而检错锁则必须由加锁者解锁才有效，否则返回EPERM；对于嵌套锁，文\n 档和实现要求必须由加锁者解锁，但实验结果表明并没有这种限制，这个不同目前还没\n 有得到解释。在同一进程中的线程，如果加锁后没有解锁，则任何其他线程都无法再获得锁。*/\n // pthread_mutex_trylock()语义与pthread_mutex_lock()类似，不同的是在锁已经被占据时返回EBUSY而不是挂起等待\n if (!pthread_mutex_trylock(&working_flag_mutex))\n {\n // 返回根节点汇总得到的有效节点个数\n s = Root_Node->TreeSize;\n pthread_mutex_unlock(&working_flag_mutex);\n return s;\n }\n else\n {\n return Treesize_tmp;\n }\n }\n}\n\n// 获取一个包围整个树的Box边界\nBoxPointType KD_TREE::tree_range()\n{\n BoxPointType range;\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n if (Root_Node != nullptr)\n {\n range.vertex_min[0] = Root_Node->node_range_x[0];\n range.vertex_min[1] = Root_Node->node_range_y[0];\n range.vertex_min[2] = Root_Node->node_range_z[0];\n range.vertex_max[0] = Root_Node->node_range_x[1];\n range.vertex_max[1] = Root_Node->node_range_y[1];\n range.vertex_max[2] = Root_Node->node_range_z[1];\n }\n else\n {\n memset(&range, 0, sizeof(range));\n }\n }\n else\n {\n if (!pthread_mutex_trylock(&working_flag_mutex))\n {\n range.vertex_min[0] = Root_Node->node_range_x[0];\n range.vertex_min[1] = Root_Node->node_range_y[0];\n range.vertex_min[2] = Root_Node->node_range_z[0];\n range.vertex_max[0] = Root_Node->node_range_x[1];\n range.vertex_max[1] = Root_Node->node_range_y[1];\n range.vertex_max[2] = Root_Node->node_range_z[1];\n pthread_mutex_unlock(&working_flag_mutex);\n }\n else\n {\n memset(&range, 0, sizeof(range));\n }\n }\n return range;\n}\n\nint KD_TREE::validnum()\n{\n int s = 0;\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n if (Root_Node != nullptr)\n return (Root_Node->TreeSize - Root_Node->invalid_point_num);\n else\n return 0;\n }\n else\n {\n if (!pthread_mutex_trylock(&working_flag_mutex))\n {\n s = Root_Node->TreeSize - Root_Node->invalid_point_num;\n pthread_mutex_unlock(&working_flag_mutex);\n return s;\n }\n else\n {\n return -1;\n }\n }\n}\n\nvoid KD_TREE::root_alpha(float &alpha_bal, float &alpha_del)\n{\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n alpha_bal = Root_Node->alpha_bal;\n alpha_del = Root_Node->alpha_del;\n return;\n }\n else\n {\n if (!pthread_mutex_trylock(&working_flag_mutex))\n {\n alpha_bal = Root_Node->alpha_bal;\n alpha_del = Root_Node->alpha_del;\n pthread_mutex_unlock(&working_flag_mutex);\n return;\n }\n else\n {\n alpha_bal = alpha_bal_tmp;\n alpha_del = alpha_del_tmp;\n return;\n }\n }\n}\n\nvoid KD_TREE::start_thread()\n{\n // 初始一系列互斥锁\n pthread_mutex_init(&termination_flag_mutex_lock, NULL);\n pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL);\n pthread_mutex_init(&rebuild_logger_mutex_lock, NULL);\n pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL);\n pthread_mutex_init(&working_flag_mutex, NULL);\n pthread_mutex_init(&search_flag_mutex, NULL);\n // 创建重建树的线程，线程标识符指针：rebuild_thread， 运行函数：multi_thread_ptr\n pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this);\n printf(\"Multi thread started \\n\");\n}\n\n// 停止线程\nvoid KD_TREE::stop_thread()\n{\n pthread_mutex_lock(&termination_flag_mutex_lock);\n termination_flag = true;\n pthread_mutex_unlock(&termination_flag_mutex_lock);\n // 等待重构树的线程结束\n if (rebuild_thread)\n pthread_join(rebuild_thread, NULL);\n /*销毁一个互斥锁即意味着释放它所占用的资源，且要求锁当前处于开放状态。由于在Linux中，\n 互斥锁并不占用任何资源，因此LinuxThreads中的 pthread_mutex_destroy()除了检查\n 锁状态以外（锁定状态则返回EBUSY）没有其他动作。*/\n pthread_mutex_destroy(&termination_flag_mutex_lock);\n pthread_mutex_destroy(&rebuild_logger_mutex_lock);\n pthread_mutex_destroy(&rebuild_ptr_mutex_lock);\n pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock);\n pthread_mutex_destroy(&working_flag_mutex);\n pthread_mutex_destroy(&search_flag_mutex);\n}\n\nvoid *KD_TREE::multi_thread_ptr(void *arg)\n{\n KD_TREE *handle = (KD_TREE *)arg;\n handle->multi_thread_rebuild();\n return nullptr;\n}\n\n// 构建ikd tree后，该线程一直在运行\n// Algorithm 4: Parallelly Rebuild for Re-balancing\nvoid KD_TREE::multi_thread_rebuild()\n{\n bool terminated = false;\n // 新建虚拟头结点和重建的树的新节点\n KD_TREE_NODE *father_ptr, **new_node_ptr;\n pthread_mutex_lock(&termination_flag_mutex_lock);\n // KD树初始化时termination_flag被设为false，所以后面的while循环是一直进行的\n terminated = termination_flag;\n pthread_mutex_unlock(&termination_flag_mutex_lock);\n while (!terminated)\n {\n // The second thread will lock all incremental updates (i.e., points insert, re-insert,\n // and delete) but not queries on this sub-tree\n // 锁定了插入、重插入、删除功能，但不锁定查询功能\n pthread_mutex_lock(&rebuild_ptr_mutex_lock);\n pthread_mutex_lock(&working_flag_mutex);\n // Rebuild_Ptr是一个tree_node类型的指针的指针\n if (Rebuild_Ptr != nullptr)\n {\n /* Traverse and copy */\n // 如果需要重建树的话，会先将树展平放到Rebuild_Logger里，如果这个记录器为空的话，但是又要求重建的话，那会报错\n if (!Rebuild_Logger.empty())\n {\n printf(\"\\n\\n\\n\\n\\n\\n\\n\\n\\n\\n\\n ERROR!!! \\n\\n\\n\\n\\n\\n\\n\\n\\n\");\n }\n rebuild_flag = true;\n // 判断重建的子树的根节点是否为整棵树的根节点\n if (*Rebuild_Ptr == Root_Node)\n {\n Treesize_tmp = Root_Node->TreeSize;\n Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num;\n alpha_bal_tmp = Root_Node->alpha_bal;\n alpha_del_tmp = Root_Node->alpha_del;\n }\n KD_TREE_NODE *old_root_node = (*Rebuild_Ptr);\n father_ptr = (*Rebuild_Ptr)->father_ptr;\n PointVector().swap(Rebuild_PCL_Storage);\n // Lock Search\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter != 0)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter = -1;\n pthread_mutex_unlock(&search_flag_mutex);\n // Lock deleted points cache\n pthread_mutex_lock(&points_deleted_rebuild_mutex_lock);\n // 将树展平\n flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC);\n // Unlock deleted points cache\n pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);\n // Unlock Search\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter = 0;\n pthread_mutex_unlock(&search_flag_mutex);\n pthread_mutex_unlock(&working_flag_mutex);\n /* Rebuild and update missed operations*/\n Operation_Logger_Type Operation;\n KD_TREE_NODE *new_root_node = nullptr;\n // 如果展平的树的节点大于0，则可以开始重建\n if (int(Rebuild_PCL_Storage.size()) > 0)\n {\n // 重建树，树的根节点为new_root_node\n BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage);\n // Rebuild has been done. Updates the blocked operations into the new tree\n pthread_mutex_lock(&working_flag_mutex);\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n int tmp_counter = 0;\n // 重建树完成后，将中途的更新操作（插入、重插入、删除）操作补充\n while (!Rebuild_Logger.empty())\n {\n // 读取操作，是插入？重插入？还是删除？\n Operation = Rebuild_Logger.front();\n max_queue_size = max(max_queue_size, Rebuild_Logger.size());\n Rebuild_Logger.pop();\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n pthread_mutex_unlock(&working_flag_mutex);\n // 在新的树中操作\n run_operation(&new_root_node, Operation);\n tmp_counter++;\n if (tmp_counter % 10 == 0)\n usleep(1);\n pthread_mutex_lock(&working_flag_mutex);\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n /* Replace to original tree*/\n // pthread_mutex_lock(&working_flag_mutex);\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter != 0)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter = -1;\n pthread_mutex_unlock(&search_flag_mutex);\n // 用新树替换原来的树\n if (father_ptr->left_son_ptr == *Rebuild_Ptr)\n {\n father_ptr->left_son_ptr = new_root_node;\n }\n else if (father_ptr->right_son_ptr == *Rebuild_Ptr)\n {\n father_ptr->right_son_ptr = new_root_node;\n }\n else\n {\n throw \"Error: Father ptr incompatible with current node\\n\";\n }\n // 给新的子树重新配置虚拟头结点\n if (new_root_node != nullptr)\n new_root_node->father_ptr = father_ptr;\n // 将新的头结点赋给重建的树\n (*Rebuild_Ptr) = new_root_node;\n // 重新配置树的属性\n int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num;\n int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num;\n if (father_ptr == STATIC_ROOT_NODE)\n Root_Node = STATIC_ROOT_NODE->left_son_ptr;\n KD_TREE_NODE *update_root = *Rebuild_Ptr;\n // 将子树属性上拉到树的根节点\n while (update_root != nullptr && update_root != Root_Node)\n {\n update_root = update_root->father_ptr;\n if (update_root->working_flag)\n break;\n if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left)\n break;\n if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right)\n break;\n Update(update_root);\n }\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter = 0;\n pthread_mutex_unlock(&search_flag_mutex);\n Rebuild_Ptr = nullptr;\n pthread_mutex_unlock(&working_flag_mutex);\n rebuild_flag = false;\n /* Delete discarded tree nodes */\n delete_tree_nodes(&old_root_node);\n }\n else\n {\n pthread_mutex_unlock(&working_flag_mutex);\n }\n pthread_mutex_unlock(&rebuild_ptr_mutex_lock);\n pthread_mutex_lock(&termination_flag_mutex_lock);\n terminated = termination_flag;\n pthread_mutex_unlock(&termination_flag_mutex_lock);\n usleep(100);\n }\n printf(\"Rebuild thread terminated normally\\n\");\n}\n\nvoid KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation)\n{\n switch (operation.op)\n {\n case ADD_POINT:\n Add_by_point(root, operation.point, false, (*root)->division_axis);\n break;\n case ADD_BOX:\n Add_by_range(root, operation.boxpoint, false);\n break;\n case DELETE_POINT:\n Delete_by_point(root, operation.point, false);\n break;\n case DELETE_BOX:\n Delete_by_range(root, operation.boxpoint, false, false);\n break;\n case DOWNSAMPLE_DELETE:\n Delete_by_range(root, operation.boxpoint, false, true);\n break;\n case PUSH_DOWN:\n (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted;\n (*root)->point_downsample_deleted |= operation.tree_downsample_deleted;\n (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted;\n (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted;\n if (operation.tree_downsample_deleted)\n (*root)->down_del_num = (*root)->TreeSize;\n if (operation.tree_deleted)\n (*root)->invalid_point_num = (*root)->TreeSize;\n else\n (*root)->invalid_point_num = (*root)->down_del_num;\n (*root)->need_push_down_to_left = true;\n (*root)->need_push_down_to_right = true;\n break;\n default:\n break;\n }\n}\n\nvoid KD_TREE::Build(PointVector point_cloud)\n{\n if (Root_Node != nullptr)\n {\n delete_tree_nodes(&Root_Node); //如果有root需要删除以前的root\n }\n if (point_cloud.size() == 0)\n return;\n STATIC_ROOT_NODE = new KD_TREE_NODE;\n //初始化当前节点的属性\n InitTreeNode(STATIC_ROOT_NODE);\n //建立kdtree，这里直接从其左子树开始构建\n BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud);\n //更新root的属性\n Update(STATIC_ROOT_NODE);\n STATIC_ROOT_NODE->TreeSize = 0;\n // 将左子树直接替换根节点\n Root_Node = STATIC_ROOT_NODE->left_son_ptr;\n}\n\n// K近邻搜索\nvoid KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, double max_dist)\n{\n // 初始化自定义的大顶堆，容量为2*k_nearest\n MANUAL_HEAP q(2 * k_nearest);\n q.clear();\n // 清空Point_Distance\n vector().swap(Point_Distance);\n // 开始搜索\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n Search(Root_Node, k_nearest, point, q, max_dist);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter == -1)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter += 1;\n pthread_mutex_unlock(&search_flag_mutex);\n Search(Root_Node, k_nearest, point, q, max_dist);\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter -= 1;\n pthread_mutex_unlock(&search_flag_mutex);\n }\n int k_found = min(k_nearest, int(q.size()));\n // 清空Nearest_Points和Point_Distance\n PointVector().swap(Nearest_Points);\n vector().swap(Point_Distance);\n // 存储\n for (int i = 0; i < k_found; i++)\n {\n // 优先取出最近的点\n Nearest_Points.insert(Nearest_Points.begin(), q.top().point);\n Point_Distance.insert(Point_Distance.begin(), q.top().dist);\n q.pop();\n }\n return;\n}\n\n// 向树中插入新的点集\nint KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on)\n{\n // 需要插入的点的个数\n int NewPointSize = PointToAdd.size();\n // 树的节点个数\n int tree_size = size();\n BoxPointType Box_of_Point;\n PointType downsample_result, mid_point;\n // 根据输入参数判断是否需要进行降采样\n bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH;\n float min_dist, tmp_dist;\n int tmp_counter = 0;\n // 遍历点集\n for (int i = 0; i < PointToAdd.size(); i++)\n {\n // 判断是否需要降采样\n if (downsample_switch)\n {\n // 计算该点所属的体素\n Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size;\n Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size;\n Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size;\n Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size;\n Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size;\n Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size;\n // 计算该体素的中心点坐标\n mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0;\n mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0;\n mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0;\n // 清空降采样缓存\n PointVector().swap(Downsample_Storage);\n // 在树中搜索最近邻点，且要求点都在Box中，将点存于Downsample_Storage\n Search_by_range(Root_Node, Box_of_Point, Downsample_Storage);\n // 计算当前点于体素中心的距离\n min_dist = calc_dist(PointToAdd[i], mid_point);\n downsample_result = PointToAdd[i];\n // 遍历体素内的所有点,寻找近邻点中最接近体素中心的点\n for (int index = 0; index < Downsample_Storage.size(); index++)\n {\n // 计算当前近邻点与体素中心的距离\n tmp_dist = calc_dist(Downsample_Storage[index], mid_point);\n // 比较两个距离，判断是否需要添加该点\n if (tmp_dist < min_dist)\n {\n min_dist = tmp_dist;\n downsample_result = Downsample_Storage[index];\n }\n }\n // 如果不需要重建树\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n // 如果近邻点不止1个，或者当前点与原有的点很接近\n if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result))\n {\n // 删除体素内的原有点，然后插入当前点\n if (Downsample_Storage.size() > 0)\n Delete_by_range(&Root_Node, Box_of_Point, true, true);\n Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis);\n tmp_counter++;\n }\n }\n else\n { // 需要重建树\n if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result))\n {\n Operation_Logger_Type operation_delete, operation;\n // 记录待删除的点\n operation_delete.boxpoint = Box_of_Point;\n operation_delete.op = DOWNSAMPLE_DELETE;\n // 记录待插入的新点\n operation.point = downsample_result;\n operation.op = ADD_POINT;\n pthread_mutex_lock(&working_flag_mutex);\n // 删除体素内的点\n if (Downsample_Storage.size() > 0)\n Delete_by_range(&Root_Node, Box_of_Point, false, true);\n // 添加点\n Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis);\n tmp_counter++;\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n if (Downsample_Storage.size() > 0)\n Rebuild_Logger.push(operation_delete);\n Rebuild_Logger.push(operation);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n };\n }\n }\n else\n { // 不需要降采样\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis);\n }\n else\n {\n Operation_Logger_Type operation;\n operation.point = PointToAdd[i];\n operation.op = ADD_POINT;\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(operation);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n }\n return tmp_counter;\n}\n\n// 以体素的形式添加点（re-insertion）\nvoid KD_TREE::Add_Point_Boxes(vector &BoxPoints)\n{\n for (int i = 0; i < BoxPoints.size(); i++)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n Add_by_range(&Root_Node, BoxPoints[i], true);\n }\n else\n {\n Operation_Logger_Type operation;\n operation.boxpoint = BoxPoints[i];\n operation.op = ADD_BOX;\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_range(&Root_Node, BoxPoints[i], false);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(operation);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n return;\n}\n\n// 删除多个点\nvoid KD_TREE::Delete_Points(PointVector &PointToDel)\n{\n for (int i = 0; i < PointToDel.size(); i++)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n Delete_by_point(&Root_Node, PointToDel[i], true);\n }\n else\n {\n Operation_Logger_Type operation;\n operation.point = PointToDel[i];\n operation.op = DELETE_POINT;\n pthread_mutex_lock(&working_flag_mutex);\n Delete_by_point(&Root_Node, PointToDel[i], false);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(operation);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n return;\n}\n\n// 删除多个体素的点\nint KD_TREE::Delete_Point_Boxes(vector &BoxPoints)\n{\n int tmp_counter = 0;\n for (int i = 0; i < BoxPoints.size(); i++)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)\n {\n tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false);\n }\n else\n {\n Operation_Logger_Type operation;\n operation.boxpoint = BoxPoints[i];\n operation.op = DELETE_BOX;\n pthread_mutex_lock(&working_flag_mutex);\n tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(operation);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n return tmp_counter;\n}\n\nvoid KD_TREE::acquire_removed_points(PointVector &removed_points)\n{ // 获取删除的点\n pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); // 加锁\n for (int i = 0; i < Points_deleted.size(); i++)\n { // 将删除的点放入removed_points中,这里的点在flatten函数中加入\n removed_points.push_back(Points_deleted[i]);\n }\n for (int i = 0; i < Multithread_Points_deleted.size(); i++)\n { // 将多线程重建时删除的点放入removed_points中,这里的点在flatten函数中加入\n removed_points.push_back(Multithread_Points_deleted[i]);\n }\n Points_deleted.clear();\n Multithread_Points_deleted.clear();\n pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); // 解锁\n return;\n}\n\n// 建树的过程与构建有序二叉树的过程类似，取中间的节点作为节点值进行递归\nvoid KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage)\n{\n if (l > r)\n return;\n //给**root分配内存\n *root = new KD_TREE_NODE;\n // 初始化根节点\n InitTreeNode(*root);\n // 相当于mid=(1+r)/2\n int mid = (l + r) >> 1;\n // 初始化划分轴\n int div_axis = 0;\n int i;\n // Find the best division Axis\n // 旋转最优的划分轴，这里优先选用这团点云中区间最大的那个轴\n float min_value[3] = {INFINITY, INFINITY, INFINITY};\n float max_value[3] = {-INFINITY, -INFINITY, -INFINITY};\n float dim_range[3] = {0, 0, 0};\n for (i = l; i <= r; i++)\n {\n min_value[0] = min(min_value[0], Storage[i].x);\n min_value[1] = min(min_value[1], Storage[i].y);\n min_value[2] = min(min_value[2], Storage[i].z);\n max_value[0] = max(max_value[0], Storage[i].x);\n max_value[1] = max(max_value[1], Storage[i].y);\n max_value[2] = max(max_value[2], Storage[i].z);\n }\n // Select the longest dimension as division axis\n // 选用跨度最大的维度\n for (i = 0; i < 3; i++)\n dim_range[i] = max_value[i] - min_value[i];\n for (i = 1; i < 3; i++)\n if (dim_range[i] > dim_range[div_axis])\n div_axis = i;\n // Divide by the division axis and recursively build.\n\n // 给该节点标记划分轴\n (*root)->division_axis = div_axis;\n /*********************************************\n * nth_element(first, nth, last, compare)\n 求[first, last]这个区间中第n大小的元素，如果参数加入了compare函数，就按compare函数的方式比较。\n 这一段就是找切割维度的中位数\n\n nth_element(a,a+k,a+n)，函数只是把下标为k的元素放在了正确位置，对其它元素并没有排序，\n 当然k左边元素都小于等于它，右边元素都大于等于它，所以可以利用这个函数快速定位某个元素。\n ******************************************************/\n switch (div_axis)\n {\n case 0:\n nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x);\n break;\n case 1:\n nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y);\n break;\n case 2:\n nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z);\n break;\n default:\n nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x);\n break;\n }\n (*root)->point = Storage[mid];\n KD_TREE_NODE *left_son = nullptr, *right_son = nullptr;\n //这就是一般的用队列建树的流程， 属于是二叉树前序遍历过程\n BuildTree(&left_son, l, mid - 1, Storage); // 处理左子树\n BuildTree(&right_son, mid + 1, r, Storage); // 处理右子树\n // 标记该节点的左右子树\n (*root)->left_son_ptr = left_son;\n (*root)->right_son_ptr = right_son;\n //更新node的属性\n Update((*root));\n return;\n}\n\n// 重建树\nvoid KD_TREE::Rebuild(KD_TREE_NODE **root)\n{\n KD_TREE_NODE *father_ptr;\n // 如果树的节点数大于阈值，则启动多线程进行重建\n if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num)\n {\n // pthread_mutex_trylock() 是 pthread_mutex_lock() 的非阻塞版本。\n // 如果 mutex 所引用的互斥对象当前被任何线程（包括当前线程）锁定，则将立即返\n // 回该调用。否则，该互斥锁将处于锁定状态，调用线程是其属主。\n if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock))\n { // true：成功加锁，fasle：其他线程已上锁\n // 如果重建树的指针为空，或者当前子树的尺寸比正在重建的子树大，则将重建树的根节点设为当前根节点\n if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize))\n {\n Rebuild_Ptr = root;\n }\n pthread_mutex_unlock(&rebuild_ptr_mutex_lock);\n // 解锁之后即开始启动多线程重建\n }\n }\n else\n { // 单线程重建\n // 暂存虚拟父节点\n father_ptr = (*root)->father_ptr;\n int size_rec = (*root)->TreeSize;\n PCL_Storage.clear();\n // 将当前子树的节点展平，同时做被删除点的记录\n flatten(*root, PCL_Storage, DELETE_POINTS_REC);\n // 释放被删除点的内存空间\n delete_tree_nodes(root);\n // 基于展平的点重新构建树\n BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage);\n // 重新连接节点间的父子关系\n if (*root != nullptr)\n (*root)->father_ptr = father_ptr;\n if (*root == Root_Node)\n STATIC_ROOT_NODE->left_son_ptr = *root;\n }\n return;\n}\n\n// box-wise delete\nint KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample)\n{\n if ((*root) == nullptr || (*root)->tree_deleted)\n return 0;\n (*root)->working_flag = true;\n // 下拉属性\n Push_Down(*root);\n int tmp_counter = 0;\n // 前三行是判断给定的Box范围是否与树有交集，没有的话就不用搜索了\n if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1])\n return 0;\n if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1])\n return 0;\n if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1])\n return 0;\n // Box包含了整一棵树的Box\n 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])\n {\n // 整棵树的属性都标记为删除\n (*root)->tree_deleted = true;\n (*root)->point_deleted = true;\n // 标记下，下次需要下拉属性\n (*root)->need_push_down_to_left = true;\n (*root)->need_push_down_to_right = true;\n // 当前子树需要被删除的点的个数\n tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num;\n (*root)->invalid_point_num = (*root)->TreeSize;\n if (is_downsample)\n {\n (*root)->tree_downsample_deleted = true;\n (*root)->point_downsample_deleted = true;\n (*root)->down_del_num = (*root)->TreeSize;\n }\n return tmp_counter;\n }\n // 当前节点是否被包含在给定box内\n 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)\n {\n (*root)->point_deleted = true;\n tmp_counter += 1;\n if (is_downsample)\n (*root)->point_downsample_deleted = true;\n }\n Operation_Logger_Type delete_box_log;\n struct timespec Timeout;\n // 在左右子树中递归调用盒式删除\n if (is_downsample)\n delete_box_log.op = DOWNSAMPLE_DELETE;\n else\n delete_box_log.op = DELETE_BOX;\n delete_box_log.boxpoint = boxpoint;\n if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)\n {\n tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample);\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(delete_box_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)\n {\n tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample);\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(delete_box_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n // 属性上拉\n Update(*root);\n // 平衡性准则判断是否需要重建树\n // 判断条件解读：重建树的指针的指针不为空，且重建树的指针为当前树的根节点，说明从上次重建后，就没有再更新这棵树的结构\n // 当前树的节点个数小于多线程重建的最小点阈值，则只需要单线程重建即可\n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)\n Rebuild_Ptr = nullptr; // 将Rebuild_Ptr设为空，即标记需要重建子树\n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n if (need_rebuild)\n Rebuild(root);\n if ((*root) != nullptr)\n (*root)->working_flag = false;\n return tmp_counter;\n}\n\n// 前序遍历，从树中删除指定点\nvoid KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild)\n{\n // 如果目标点不存在或者已经被标记为删除，则不操作即返回\n if ((*root) == nullptr || (*root)->tree_deleted)\n return;\n (*root)->working_flag = true;\n // 将属性下拉\n Push_Down(*root);\n // 搜索到目标点，且目标点还未被标记删除，则标记为删除\n if (same_point((*root)->point, point) && !(*root)->point_deleted)\n {\n (*root)->point_deleted = true;\n (*root)->invalid_point_num += 1;\n // 如果该树的无效点数正好等于树的总节点数，说明这棵树可以直接被删除，那么把树的删除标志设为true\n if ((*root)->invalid_point_num == (*root)->TreeSize)\n (*root)->tree_deleted = true;\n return;\n }\n Operation_Logger_Type delete_log;\n struct timespec Timeout;\n delete_log.op = DELETE_POINT;\n delete_log.point = point;\n // 到左子树递归查找\n 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))\n {\n if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)\n {\n Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild);\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n Delete_by_point(&(*root)->left_son_ptr, point, false);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(delete_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n else\n { // 到右子树递归查找\n if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)\n {\n Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild);\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n Delete_by_point(&(*root)->right_son_ptr, point, false);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(delete_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n // 将属性上拉\n Update(*root);\n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)\n Rebuild_Ptr = nullptr;\n // 平衡性准则判断是否需要重建\n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n if (need_rebuild)\n Rebuild(root);\n if ((*root) != nullptr)\n (*root)->working_flag = false;\n return;\n}\n\n// box-wise re-insertion 重插入\n// box更新，可以便利的对一整个范围的点进行更新\nvoid KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild)\n{\n if ((*root) == nullptr)\n return;\n (*root)->working_flag = true;\n Push_Down(*root);\n //论文中的判断部分，判断增加的范围是否与root的range的包含关系\n // 前三行是判断给定的Box范围是否与树有交集，没有的话就不用搜索了\n if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1])\n return;\n if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1])\n return;\n if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1])\n return;\n // Box包含了整一棵树的Box\n 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])\n {\n // 更新树的删除标签\n (*root)->tree_deleted = false || (*root)->tree_downsample_deleted;\n (*root)->point_deleted = false || (*root)->point_downsample_deleted;\n // 告诉树，下次要进行push down了\n (*root)->need_push_down_to_left = true;\n (*root)->need_push_down_to_right = true;\n (*root)->invalid_point_num = (*root)->down_del_num;\n return;\n }\n // box包含当前节点的点\n 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)\n {\n (*root)->point_deleted = (*root)->point_downsample_deleted;\n }\n Operation_Logger_Type add_box_log;\n struct timespec Timeout;\n add_box_log.op = ADD_BOX;\n add_box_log.boxpoint = boxpoint;\n // 接着到其左右子树上进行递归重插入\n if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)\n {\n Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild);\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_range(&((*root)->left_son_ptr), boxpoint, false);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(add_box_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)\n {\n Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild);\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_range(&((*root)->right_son_ptr), boxpoint, false);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(add_box_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n // 重插入完成后，将子树属性上拉到其父节点\n Update(*root);\n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)\n Rebuild_Ptr = nullptr;\n // 平衡性准则判断是否需要重建子树\n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n // 启动重建该子树\n if (need_rebuild)\n Rebuild(root);\n if ((*root) != nullptr)\n (*root)->working_flag = false;\n return;\n}\n\n// 前序遍历的点级更新\n// 在ik-d树上的点级更新以递归的方式实现，这类似于替罪羊k-d树。对于点级插入，\n// 该算法递归地从根节点向下搜索，并将新点的除法轴上的坐标与存储在树节点上的点进行比较，\n// 直到发现一个叶节点然后追加一个新的树节点。对于删除或重新插入一个点P，该算法会查找存\n// 储该点P的树节点，并修改deleted属性。\nvoid KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis)\n{\n // 如果当前节点为空的话，则新建节点\n if (*root == nullptr)\n {\n // 新建节点并初始化\n *root = new KD_TREE_NODE; // 注意到root是指针的指针，所以不需要返回地址即可实现父节点指向当前新节点\n InitTreeNode(*root);\n // 配置节点的值\n (*root)->point = point;\n // 标记该节点的划分轴\n (*root)->division_axis = (father_axis + 1) % 3;\n // 将新的节点信息汇总给父节点\n Update(*root);\n return;\n }\n // 当前节点不为空，说明还未到叶子节点，则递归查找\n (*root)->working_flag = true;\n // 创建Operation_Logger_Type用于记录新插入的点，重构树的时候需要用到\n Operation_Logger_Type add_log;\n struct timespec Timeout;\n // 设置动作为ADD_POINT\n add_log.op = ADD_POINT;\n add_log.point = point;\n // 顺便将当前节点的状态信息下拉\n Push_Down(*root);\n //如果在左边则进行左边的比较然后添加，首先确定当前节点的划分轴，然后判断该点应该分到左子树还是右子树\n 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))\n {\n if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)\n {\n Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis);\n }\n else\n {\n // 线程互斥锁\n pthread_mutex_lock(&working_flag_mutex);\n // // 递归，直到找到叶子节点再新建节点并插入树中\n Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(add_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n else\n { // 分到右子树\n if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)\n {\n Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis);\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis);\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(add_log);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n // 做一次pull-up, 更新root，判断是否需要进行重建\n Update(*root);\n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)\n Rebuild_Ptr = nullptr;\n // 判断子树是否平衡\n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n // 检验后发现不平衡的话就需要重建该子树\n if (need_rebuild)\n Rebuild(root);\n if ((*root) != nullptr)\n (*root)->working_flag = false;\n return;\n}\n\n// k近邻搜索，这是一个前序遍历的过程\nvoid KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist)\n{\n // 如果树都还没建，那就没必要搜索了\n if (root == nullptr || root->tree_deleted)\n return;\n // 计算目标点与tree的box边界的最小距离（假设目标点不在box内），如果最小距离大于限定距离，则不搜索了\n double cur_dist = calc_box_dist(root, point);\n if (cur_dist > max_dist)\n return;\n int retval;\n // The function Pushdown is applied before searching the\n // sub-tree rooted at node T to pass down its lazy labels.\n if (root->need_push_down_to_left || root->need_push_down_to_right)\n {\n retval = pthread_mutex_trylock(&(root->push_down_mutex_lock));\n if (retval == 0)\n {\n Push_Down(root);\n pthread_mutex_unlock(&(root->push_down_mutex_lock));\n }\n else\n {\n pthread_mutex_lock(&(root->push_down_mutex_lock));\n pthread_mutex_unlock(&(root->push_down_mutex_lock));\n }\n }\n // 该点的标签没有打成deteled的话，可以作为候选点\n if (!root->point_deleted)\n {\n // 计算目标点与当前节点的点的距离\n float dist = calc_dist(point, root->point);\n // 如果当前距离小于限定最大距离，则将其压入大顶堆q，大顶堆也是一个数组\n // 较大的值在前，较小的值在后\n if (dist <= max_dist && (q.size() < k_nearest || dist < q.top().dist))\n {\n // 如果堆的size超过k_nearest，则pop一个\n if (q.size() >= k_nearest)\n q.pop();\n // 根据当前搜索点及其距离，构建自定义的点类型\n PointType_CMP current_point{root->point, dist};\n // 压入堆中\n q.push(current_point);\n }\n }\n int cur_search_counter;\n // 计算目标点与左、右子树的Box边界最小距离\n float dist_left_node = calc_box_dist(root->left_son_ptr, point);\n float dist_right_node = calc_box_dist(root->right_son_ptr, point);\n // 如果堆还没满，或者左右子树中存在可能点，则进一步搜索\n if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist)\n {\n // 左子树的Box更近，则先搜索左子树\n if (dist_left_node <= dist_right_node)\n {\n // 到左子树递归搜索\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)\n {\n Search(root->left_son_ptr, k_nearest, point, q, max_dist);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter == -1)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter += 1;\n pthread_mutex_unlock(&search_flag_mutex);\n Search(root->left_son_ptr, k_nearest, point, q, max_dist);\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter -= 1;\n pthread_mutex_unlock(&search_flag_mutex);\n }\n if (q.size() < k_nearest || dist_right_node < q.top().dist)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)\n {\n Search(root->right_son_ptr, k_nearest, point, q, max_dist);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter == -1)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter += 1;\n pthread_mutex_unlock(&search_flag_mutex);\n Search(root->right_son_ptr, k_nearest, point, q, max_dist);\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter -= 1;\n pthread_mutex_unlock(&search_flag_mutex);\n }\n }\n }\n else\n { // 到右子树中递归搜索\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)\n {\n Search(root->right_son_ptr, k_nearest, point, q, max_dist);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter == -1)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter += 1;\n pthread_mutex_unlock(&search_flag_mutex);\n Search(root->right_son_ptr, k_nearest, point, q, max_dist);\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter -= 1;\n pthread_mutex_unlock(&search_flag_mutex);\n }\n if (q.size() < k_nearest || dist_left_node < q.top().dist)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)\n {\n Search(root->left_son_ptr, k_nearest, point, q, max_dist);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter == -1)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter += 1;\n pthread_mutex_unlock(&search_flag_mutex);\n Search(root->left_son_ptr, k_nearest, point, q, max_dist);\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter -= 1;\n pthread_mutex_unlock(&search_flag_mutex);\n }\n }\n }\n }\n else\n {\n if (dist_left_node < q.top().dist)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)\n {\n Search(root->left_son_ptr, k_nearest, point, q, max_dist);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter == -1)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter += 1;\n pthread_mutex_unlock(&search_flag_mutex);\n Search(root->left_son_ptr, k_nearest, point, q, max_dist);\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter -= 1;\n pthread_mutex_unlock(&search_flag_mutex);\n }\n }\n if (dist_right_node < q.top().dist)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)\n {\n Search(root->right_son_ptr, k_nearest, point, q, max_dist);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n while (search_mutex_counter == -1)\n {\n pthread_mutex_unlock(&search_flag_mutex);\n usleep(1);\n pthread_mutex_lock(&search_flag_mutex);\n }\n search_mutex_counter += 1;\n pthread_mutex_unlock(&search_flag_mutex);\n Search(root->right_son_ptr, k_nearest, point, q, max_dist);\n pthread_mutex_lock(&search_flag_mutex);\n search_mutex_counter -= 1;\n pthread_mutex_unlock(&search_flag_mutex);\n }\n }\n }\n return;\n}\n\n// 前序遍历搜索，根据给定的box，从树中搜索被Box包围的点集\nvoid KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage)\n{\n if (root == nullptr)\n return;\n // 现将当前节点的属性Push down一下\n Push_Down(root);\n // 前三行是判断给定的Box范围是否与树有交集，没有的话就不用搜索了\n if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1])\n return;\n if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1])\n return;\n if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1])\n return;\n // 给定的box完全包围当前的树，则将当前树展平，将所有点放到Storage内，直接返回即可\n 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])\n {\n flatten(root, Storage, NOT_RECORD);\n return;\n }\n // 给定的box完全包围当前节点，如果该点没有被删除，则将该点压入Storage\n 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)\n {\n if (!root->point_deleted)\n Storage.push_back(root->point);\n }\n // 到左右子树中递归搜索，直到叶子节点结束\n if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr)\n {\n Search_by_range(root->left_son_ptr, boxpoint, Storage);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n Search_by_range(root->left_son_ptr, boxpoint, Storage);\n pthread_mutex_unlock(&search_flag_mutex);\n }\n if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr)\n {\n Search_by_range(root->right_son_ptr, boxpoint, Storage);\n }\n else\n {\n pthread_mutex_lock(&search_flag_mutex);\n Search_by_range(root->right_son_ptr, boxpoint, Storage);\n pthread_mutex_unlock(&search_flag_mutex);\n }\n return;\n}\n\nbool KD_TREE::Criterion_Check(KD_TREE_NODE *root)\n{\n // 如果当前树的总节点个数小于最小的树的size，则没必要重建，返回false\n if (root->TreeSize <= Minimal_Unbalanced_Tree_Size)\n {\n return false;\n }\n // 平衡性归零\n float balance_evaluation = 0.0f;\n float delete_evaluation = 0.0f;\n // 新建一个节点指针存储树的根节点，root是虚拟头结点\n KD_TREE_NODE *son_ptr = root->left_son_ptr;\n if (son_ptr == nullptr)\n son_ptr = root->right_son_ptr;\n // 无效点在树中所占的比例\n delete_evaluation = float(root->invalid_point_num) / root->TreeSize;\n // 有效点在树中所占的比例\n balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1);\n // 论文中的平衡性判断准则\n if (delete_evaluation > delete_criterion_param)\n {\n return true;\n }\n if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param)\n {\n return true;\n }\n return false;\n}\n\n// The Pushdown function copies the labels deleted, treedeleted, and pushdown of T to its sons (but\n// not further offsprings) when the attribute pushdown is true.\n// 当属性Pushdown 为true时，Pushdown 函数将被deleted, treedeleted,\n// 和 pushdown的标签复制到它的子节点（但没有复制到它的的后代。\nvoid KD_TREE::Push_Down(KD_TREE_NODE *root)\n{\n if (root == nullptr)\n return;\n // 实例化一个操作记录器\n Operation_Logger_Type operation;\n // 操作器设置为PUSH_DOWN状态\n operation.op = PUSH_DOWN;\n // 获取当前节点的删除标签，用于传递给子节点\n operation.tree_deleted = root->tree_deleted;\n // 同样，获取当前节点的降采样删除标签，用于传递给子节点\n operation.tree_downsample_deleted = root->tree_downsample_deleted;\n // 左子节点不为空，且该节点需要push down\n if (root->need_push_down_to_left && root->left_son_ptr != nullptr)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)\n {\n // 将当前节点的删除标签传递给左子节点\n root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;\n root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;\n root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;\n root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;\n if (root->tree_downsample_deleted)\n root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;\n if (root->tree_deleted)\n root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;\n else\n root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;\n // 标记下左子节点的状态更新情况，设为true意味着左子节点更新完，它的属性也要传递给后代节点\n root->left_son_ptr->need_push_down_to_left = true;\n root->left_son_ptr->need_push_down_to_right = true;\n // 当前节点属性已经Push down了，所以可以将其更新push down需求设为false\n root->need_push_down_to_left = false;\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;\n root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;\n root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;\n root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;\n if (root->tree_downsample_deleted)\n root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;\n if (root->tree_deleted)\n root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;\n else\n root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;\n root->left_son_ptr->need_push_down_to_left = true;\n root->left_son_ptr->need_push_down_to_right = true;\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(operation);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n root->need_push_down_to_left = false;\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n // 对于右子节点也是相同的Push down操作，传递当前节点的属性下去\n if (root->need_push_down_to_right && root->right_son_ptr != nullptr)\n {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)\n {\n root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;\n root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;\n root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;\n root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;\n if (root->tree_downsample_deleted)\n root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;\n if (root->tree_deleted)\n root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;\n else\n root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;\n root->right_son_ptr->need_push_down_to_left = true;\n root->right_son_ptr->need_push_down_to_right = true;\n root->need_push_down_to_right = false;\n }\n else\n {\n pthread_mutex_lock(&working_flag_mutex);\n root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;\n root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;\n root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;\n root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;\n if (root->tree_downsample_deleted)\n root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;\n if (root->tree_deleted)\n root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;\n else\n root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;\n root->right_son_ptr->need_push_down_to_left = true;\n root->right_son_ptr->need_push_down_to_right = true;\n if (rebuild_flag)\n {\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n Rebuild_Logger.push(operation);\n pthread_mutex_unlock(&rebuild_logger_mutex_lock);\n }\n root->need_push_down_to_right = false;\n pthread_mutex_unlock(&working_flag_mutex);\n }\n }\n return;\n}\n\n// Update函数的主要目的是为了更新该节点以及它子树的点的范围以及size，还有他的del，bal，\n// 用来判断树是否平衡，是否需要重建。这个函数再原论文中是pull-up操作（树的属性由子向父传播）。\n\n// Pullup函数将基于T的子树的信息汇总给T的以下属性：treesize（见数据结构1，第5行）保存子树上\n// 所有节点数，invalidnum 存储子树上标记为“删除”的节点数，range（见数据结构1，第7行）汇总子\n// 树上所有点坐标轴的范围，其中k为点维度。\nvoid KD_TREE::Update(KD_TREE_NODE *root)\n{ // 该函数不需要递归，因为父节点的属性只与左右子节点相关\n // 该节点的左右子节点\n KD_TREE_NODE *left_son_ptr = root->left_son_ptr;\n KD_TREE_NODE *right_son_ptr = root->right_son_ptr;\n float tmp_range_x[2] = {INFINITY, -INFINITY};\n float tmp_range_y[2] = {INFINITY, -INFINITY};\n float tmp_range_z[2] = {INFINITY, -INFINITY};\n // Update Tree Size\n // 左右子树皆不为空\n if (left_son_ptr != nullptr && right_son_ptr != nullptr)\n {\n // 那当前树的节点数=左子树节点数+右子树节点数+1(当前节点本身)\n root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1;\n // 同上，树中的无效点个数也类似计算\n root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);\n // 降采样删除的节点个数和\n root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);\n // 降采样删除的标志位\n root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;\n // 删除树的标志位\n root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted;\n // 获取包围当前树的box\n if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted))\n {\n tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x);\n tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x);\n tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y);\n tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y);\n tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z);\n tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z);\n }\n else\n { // 否则的话，肯定有一边子树被标记了删除了\n // 仅取左子树\n if (!left_son_ptr->tree_deleted)\n {\n tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);\n tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);\n tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);\n tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);\n tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);\n tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);\n }\n // 仅取右子树\n if (!right_son_ptr->tree_deleted)\n {\n tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);\n tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);\n tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);\n tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);\n tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);\n tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);\n }\n // 子树的box+root的点\n if (!root->point_deleted)\n {\n tmp_range_x[0] = min(tmp_range_x[0], root->point.x);\n tmp_range_x[1] = max(tmp_range_x[1], root->point.x);\n tmp_range_y[0] = min(tmp_range_y[0], root->point.y);\n tmp_range_y[1] = max(tmp_range_y[1], root->point.y);\n tmp_range_z[0] = min(tmp_range_z[0], root->point.z);\n tmp_range_z[1] = max(tmp_range_z[1], root->point.z);\n }\n }\n // 仅有左子树，右子树为空\n }\n else if (left_son_ptr != nullptr)\n {\n // 仅需要将左子树的信息汇总给root\n root->TreeSize = left_son_ptr->TreeSize + 1;\n root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);\n root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);\n root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;\n root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted;\n // 获取包围左子树+root的box\n if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted))\n {\n tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x);\n tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x);\n tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y);\n tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y);\n tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z);\n tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z);\n }\n else\n {\n if (!left_son_ptr->tree_deleted)\n {\n tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);\n tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);\n tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);\n tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);\n tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);\n tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);\n }\n if (!root->point_deleted)\n {\n tmp_range_x[0] = min(tmp_range_x[0], root->point.x);\n tmp_range_x[1] = max(tmp_range_x[1], root->point.x);\n tmp_range_y[0] = min(tmp_range_y[0], root->point.y);\n tmp_range_y[1] = max(tmp_range_y[1], root->point.y);\n tmp_range_z[0] = min(tmp_range_z[0], root->point.z);\n tmp_range_z[1] = max(tmp_range_z[1], root->point.z);\n }\n }\n // 左子树为空，右子树不为空\n }\n else if (right_son_ptr != nullptr)\n {\n // 同理，仅需要将右子树的信息汇总给root\n root->TreeSize = right_son_ptr->TreeSize + 1;\n root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);\n root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);\n root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;\n root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted;\n // 获取包围右子树和root的box\n if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted))\n {\n tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x);\n tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x);\n tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y);\n tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y);\n tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z);\n tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z);\n }\n else\n {\n if (!right_son_ptr->tree_deleted)\n {\n tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);\n tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);\n tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);\n tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);\n tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);\n tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);\n }\n if (!root->point_deleted)\n {\n tmp_range_x[0] = min(tmp_range_x[0], root->point.x);\n tmp_range_x[1] = max(tmp_range_x[1], root->point.x);\n tmp_range_y[0] = min(tmp_range_y[0], root->point.y);\n tmp_range_y[1] = max(tmp_range_y[1], root->point.y);\n tmp_range_z[0] = min(tmp_range_z[0], root->point.z);\n tmp_range_z[1] = max(tmp_range_z[1], root->point.z);\n }\n }\n // 左右子树都为空\n }\n else\n {\n root->TreeSize = 1;\n root->invalid_point_num = (root->point_deleted ? 1 : 0);\n root->down_del_num = (root->point_downsample_deleted ? 1 : 0);\n root->tree_downsample_deleted = root->point_downsample_deleted;\n root->tree_deleted = root->point_deleted;\n tmp_range_x[0] = root->point.x;\n tmp_range_x[1] = root->point.x;\n tmp_range_y[0] = root->point.y;\n tmp_range_y[1] = root->point.y;\n tmp_range_z[0] = root->point.z;\n tmp_range_z[1] = root->point.z;\n }\n // 将树的区间范围拷贝给节点属性 node_range_x = [node_range_x_min, node_range_x_max]\n memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x));\n memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y));\n memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z));\n // 让子节点知道自身父节点是谁\n if (left_son_ptr != nullptr)\n left_son_ptr->father_ptr = root;\n if (right_son_ptr != nullptr)\n right_son_ptr->father_ptr = root;\n // 如果当前节点为整个树的根节点，且树的节点数大于3\n if (root == Root_Node && root->TreeSize > 3)\n {\n KD_TREE_NODE *son_ptr = root->left_son_ptr;\n if (son_ptr == nullptr)\n son_ptr = root->right_son_ptr;\n float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1);\n root->alpha_del = float(root->invalid_point_num) / root->TreeSize;\n root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal;\n }\n return;\n}\n\n// 前序遍历和后序遍历都有，将一个树的所有节点展平，按顺序放到一个容器内\nvoid KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type)\n{\n if (root == nullptr)\n return;\n Push_Down(root);\n // 只要该点没有被删除，那么就把它记录下来， 前序遍历\n if (!root->point_deleted)\n {\n Storage.push_back(root->point);\n }\n flatten(root->left_son_ptr, Storage, storage_type);\n flatten(root->right_son_ptr, Storage, storage_type);\n // 根据枚举变量storage_type判断使用什么存储动作， 后序遍历\n switch (storage_type)\n {\n case NOT_RECORD: // 不操作\n break;\n case DELETE_POINTS_REC: // 删除点的记录\n // 只有该点是真正被删除，而不是被降采样删除，该点才会被记录\n if (root->point_deleted && !root->point_downsample_deleted)\n {\n Points_deleted.push_back(root->point);\n }\n break;\n case MULTI_THREAD_REC: // 多线程重建树时的点的记录\n if (root->point_deleted && !root->point_downsample_deleted)\n {\n Multithread_Points_deleted.push_back(root->point);\n }\n break;\n default:\n break;\n }\n return;\n}\n\n// 后序遍历，删除子树的根节点，那么也要递归删除它的后代节点\nvoid KD_TREE::delete_tree_nodes(KD_TREE_NODE **root)\n{\n if (*root == nullptr)\n return;\n Push_Down(*root); // 先把该节点的子节点全部压下来\n delete_tree_nodes(&(*root)->left_son_ptr); // 删除左子树\n delete_tree_nodes(&(*root)->right_son_ptr); // 删除右子树\n\n delete *root;\n *root = nullptr;\n\n return;\n}\n\n// 判断两点是否为相同点\nbool KD_TREE::same_point(PointType a, PointType b)\n{\n return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS);\n}\n\n// 计算两点之间的欧氏距离\nfloat KD_TREE::calc_dist(PointType a, PointType b)\n{\n float dist = 0.0f;\n 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);\n return dist;\n}\n\n// 首先获取node构成的树的范围，就是一个Box。然后逐个面判断point是否在box外，如果在Box外，则\n// 计算离point的面的距离之“平方”和\n// 有个疑惑，这样计算的结果真的是点到Box的距离吗\nfloat KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point)\n{\n if (node == nullptr)\n return INFINITY;\n float min_dist = 0.0;\n if (point.x < node->node_range_x[0])\n min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]);\n if (point.x > node->node_range_x[1])\n min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]);\n if (point.y < node->node_range_y[0])\n min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]);\n if (point.y > node->node_range_y[1])\n min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]);\n if (point.z < node->node_range_z[0])\n min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]);\n if (point.z > node->node_range_z[1])\n min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]);\n return min_dist;\n}\n\nbool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; }\nbool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; }\nbool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; }\n\n// Manual heap\nMANUAL_HEAP::MANUAL_HEAP(int max_capacity)\n{\n cap = max_capacity;\n heap = new PointType_CMP[max_capacity];\n heap_size = 0;\n}\n\nMANUAL_HEAP::~MANUAL_HEAP()\n{\n delete[] heap;\n}\n\n// 弹出最大的元素，也就是堆顶\nvoid MANUAL_HEAP::pop()\n{\n if (heap_size == 0)\n return;\n // 我们取出数组中的最后一个元素，将它放到堆的顶部，然后再修复堆属性\n heap[0] = heap[heap_size - 1];\n // 将size指针左移，相当于pop一个元素\n heap_size--;\n // 修复大顶堆\n MoveDown(0);\n return;\n}\n\n// 返回堆顶\nPointType_CMP MANUAL_HEAP::top()\n{\n return heap[0];\n}\n\nvoid MANUAL_HEAP::push(PointType_CMP point)\n{\n // 如果堆的size大于设定容量，则不压入\n if (heap_size >= cap)\n return;\n // 将新进的元素插入到大顶堆的尾部\n heap[heap_size] = point;\n // 维护大顶堆的内部关系\n FloatUp(heap_size);\n heap_size++;\n return;\n}\n\nint MANUAL_HEAP::size()\n{\n return heap_size;\n}\n\nvoid MANUAL_HEAP::clear()\n{\n heap_size = 0;\n return;\n}\n\nvoid MANUAL_HEAP::MoveDown(int heap_index)\n{\n // 节点的左子树索引\n int l = heap_index * 2 + 1;\n // 当前节点值\n PointType_CMP tmp = heap[heap_index];\n // 从顶堆遍历到最后一个节点\n while (l < heap_size)\n {\n // 左右子节点比较，如果右子节点比左子节点大，则将l改为右子节点的索引\n if (l + 1 < heap_size && heap[l] < heap[l + 1])\n l++;\n // 如果父节点小于l节点，则交换值，另一边的子树的结构没有被破坏，所以不需要处理\n if (tmp < heap[l])\n {\n heap[heap_index] = heap[l];\n heap_index = l;\n l = heap_index * 2 + 1;\n }\n else\n break;\n }\n heap[heap_index] = tmp;\n return;\n}\n\n// 此时最大堆已经被破坏，需要重新调整, 因加入的节点比父节点大，则新节点跟父节点调换即可。\n// 调整后，新节点如果比新的父节点小，则已经调整到位，如果比新的父节点大，则\n// 需要和父节点重新进行交换。至此，最大堆调整完成。\nvoid MANUAL_HEAP::FloatUp(int heap_index)\n{\n // 找到父节点\n int ancestor = (heap_index - 1) / 2;\n PointType_CMP tmp = heap[heap_index];\n while (heap_index > 0)\n {\n // 父节点小于当前节点\n if (heap[ancestor] < tmp)\n {\n // 交换两个节点值\n heap[heap_index] = heap[ancestor];\n // 指针移到父节点的位置\n heap_index = ancestor;\n // 继续查找父节点\n ancestor = (heap_index - 1) / 2;\n }\n else\n break; // 父节点大于当前节点，则调整完毕\n }\n // 将当前点放到该位置\n heap[heap_index] = tmp;\n return;\n}\n\n// manual queue\nvoid MANUAL_Q::clear()\n{\n head = 0;\n tail = 0;\n counter = 0;\n is_empty = true;\n return;\n}\n\nvoid MANUAL_Q::pop()\n{\n if (counter == 0)\n return;\n head++;\n head %= Q_LEN;\n counter--;\n if (counter == 0)\n is_empty = true;\n return;\n}\n\nOperation_Logger_Type MANUAL_Q::front()\n{\n return q[head];\n}\n\nOperation_Logger_Type MANUAL_Q::back()\n{\n return q[tail];\n}\n\nvoid MANUAL_Q::push(Operation_Logger_Type op)\n{\n q[tail] = op;\n counter++;\n if (is_empty)\n is_empty = false;\n tail++;\n tail %= Q_LEN;\n}\n\nbool MANUAL_Q::empty()\n{\n return is_empty;\n}\n\nint MANUAL_Q::size()\n{\n return counter;\n}\n" }, { "path": "FAST-LIO/include/ikd-Tree/ikd_Tree.h", "content": "#pragma once\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#define EPSS 1e-6\n#define Minimal_Unbalanced_Tree_Size 10\n#define Multi_Thread_Rebuild_Point_Num 1500\n#define DOWNSAMPLE_SWITCH true\n#define ForceRebuildPercentage 0.2\n#define Q_LEN 1000000\n\nusing namespace std;\n\ntypedef pcl::PointXYZINormal PointType;\ntypedef vector> PointVector;\n\nconst PointType ZeroP;\n\nstruct KD_TREE_NODE\n{\n // 该节点对应的点坐标\n PointType point;\n // 划分轴\n int division_axis;\n int TreeSize = 1;\n int invalid_point_num = 0;\n int down_del_num = 0;\n bool point_deleted = false;\n bool tree_deleted = false;\n bool point_downsample_deleted = false;\n bool tree_downsample_deleted = false;\n bool need_push_down_to_left = false;\n bool need_push_down_to_right = false;\n bool working_flag = false;\n pthread_mutex_t push_down_mutex_lock;\n float node_range_x[2], node_range_y[2], node_range_z[2];\n // 左右子节点\n KD_TREE_NODE *left_son_ptr = nullptr;\n KD_TREE_NODE *right_son_ptr = nullptr;\n // 父节点\n KD_TREE_NODE *father_ptr = nullptr;\n // For paper data record\n float alpha_del;\n float alpha_bal;\n};\n\nstruct PointType_CMP\n{\n PointType point;\n float dist = 0.0;\n PointType_CMP(PointType p = ZeroP, float d = INFINITY)\n {\n this->point = p;\n this->dist = d;\n };\n bool operator<(const PointType_CMP &a) const\n {\n if (fabs(dist - a.dist) < 1e-10)\n return point.x < a.point.x;\n else\n return dist < a.dist;\n }\n};\n\nstruct BoxPointType\n{\n float vertex_min[3];\n float vertex_max[3];\n};\n\nenum operation_set\n{\n ADD_POINT,\n DELETE_POINT,\n DELETE_BOX,\n ADD_BOX,\n DOWNSAMPLE_DELETE,\n PUSH_DOWN\n};\n\nenum delete_point_storage_set\n{\n NOT_RECORD,\n DELETE_POINTS_REC,\n MULTI_THREAD_REC\n};\n\nstruct Operation_Logger_Type\n{\n PointType point;\n BoxPointType boxpoint;\n bool tree_deleted, tree_downsample_deleted;\n operation_set op;\n};\n\n// 自定义的循环队列\nclass MANUAL_Q\n{\nprivate:\n int head = 0, tail = 0, counter = 0;\n Operation_Logger_Type q[Q_LEN];\n bool is_empty;\n\npublic:\n void pop();\n Operation_Logger_Type front();\n Operation_Logger_Type back();\n void clear();\n void push(Operation_Logger_Type op);\n bool empty();\n int size();\n};\n\n// 自定义的大顶堆，用于堆排序，使用堆方法依次弹出最大值，执行效率高于冒泡排序\n// 参考：https://blog.csdn.net/qq_54169998/article/details/121098648\nclass MANUAL_HEAP\n{\npublic:\n MANUAL_HEAP(int max_capacity = 100);\n ~MANUAL_HEAP();\n void pop();\n PointType_CMP top();\n void push(PointType_CMP point);\n int size();\n void clear();\n\nprivate:\n PointType_CMP *heap; // 存储堆元素的数组\n void MoveDown(int heap_index);\n void FloatUp(int heap_index);\n int heap_size = 0; //当前已存储的元素个数\n int cap = 0; //当前的存储容量\n};\n\nclass KD_TREE\n{\nprivate:\n // Multi-thread Tree Rebuild\n bool termination_flag = false; // 用于终止多线程重建树的标志\n bool rebuild_flag = false; // 用于标志是否需要重建树的标志\n pthread_t rebuild_thread; // 用于多线程重建树的线程\n pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; // 用于多线程重建树的互斥锁\n pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; // 用于多线程重建树的互斥锁\n // queue Rebuild_Logger;\n MANUAL_Q Rebuild_Logger; // 用于多线程重建树的队列\n PointVector Rebuild_PCL_Storage; // 用于多线程重建树的点云存储\n KD_TREE_NODE **Rebuild_Ptr = nullptr; // 用于多线程重建树的树指针\n int search_mutex_counter = 0; // 用于多线程重建树的互斥计数器\n static void *multi_thread_ptr(void *arg);\n void multi_thread_rebuild();\n void start_thread();\n void stop_thread();\n void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation);\n // KD Tree Functions and augmented variables\n int Treesize_tmp = 0, Validnum_tmp = 0; // 用于多线程重建树的临时变量\n float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; // 用于多线程重建树的临时变量\n float delete_criterion_param = 0.5f; // 用于多线程重建树的临时变量\n float balance_criterion_param = 0.7f; // 用于多线程重建树的临时变量\n float downsample_size = 0.2f; // 用于多线程重建树的临时变量\n bool Delete_Storage_Disabled = false;\n KD_TREE_NODE *STATIC_ROOT_NODE = nullptr;\n PointVector Points_deleted; // 用于多线程重建树的点云存储\n PointVector Downsample_Storage; // 用于多线程重建树的点云存储\n PointVector Multithread_Points_deleted; // 用于多线程重建树的点云存储\n void InitTreeNode(KD_TREE_NODE *root);\n void Test_Lock_States(KD_TREE_NODE *root);\n void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage);\n void Rebuild(KD_TREE_NODE **root);\n int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample);\n void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild);\n void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis);\n void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild);\n void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist); // priority_queue\n void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage);\n bool Criterion_Check(KD_TREE_NODE *root);\n void Push_Down(KD_TREE_NODE *root);\n void Update(KD_TREE_NODE *root);\n void delete_tree_nodes(KD_TREE_NODE **root);\n void downsample(KD_TREE_NODE **root);\n bool same_point(PointType a, PointType b);\n float calc_dist(PointType a, PointType b);\n float calc_box_dist(KD_TREE_NODE *node, PointType point);\n static bool point_cmp_x(PointType a, PointType b);\n static bool point_cmp_y(PointType a, PointType b);\n static bool point_cmp_z(PointType a, PointType b);\n\npublic:\n KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2);\n ~KD_TREE();\n void Set_delete_criterion_param(float delete_param);\n void Set_balance_criterion_param(float balance_param);\n void set_downsample_param(float box_length);\n void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2);\n int size();\n int validnum();\n void root_alpha(float &alpha_bal, float &alpha_del);\n void Build(PointVector point_cloud);\n void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, double max_dist = INFINITY);\n int Add_Points(PointVector &PointToAdd, bool downsample_on);\n void Add_Point_Boxes(vector &BoxPoints);\n void Delete_Points(PointVector &PointToDel);\n int Delete_Point_Boxes(vector &BoxPoints);\n void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type);\n void acquire_removed_points(PointVector &removed_points);\n BoxPointType tree_range();\n PointVector PCL_Storage; // for debug\n KD_TREE_NODE *Root_Node = nullptr; //设置根节点一开始为空\n int max_queue_size = 0; //设置最大队列长度为0\n};\n" }, { "path": "FAST-LIO/include/matplotlibcpp.h", "content": "#pragma once\n\n// Python headers must be included before any system headers, since\n// they define _POSIX_C_SOURCE\n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include // requires c++11 support\n#include \n\n#ifndef WITHOUT_NUMPY\n# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION\n# include \n\n# ifdef WITH_OPENCV\n# include \n# endif // WITH_OPENCV\n\n/*\n * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so\n * define the ones we need here.\n */\n# if CV_MAJOR_VERSION > 3\n# define CV_BGR2RGB cv::COLOR_BGR2RGB\n# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA\n# endif\n#endif // WITHOUT_NUMPY\n\n#if PY_MAJOR_VERSION >= 3\n# define PyString_FromString PyUnicode_FromString\n# define PyInt_FromLong PyLong_FromLong\n# define PyString_FromString PyUnicode_FromString\n#endif\n\n\nnamespace matplotlibcpp {\nnamespace detail {\n\nstatic std::string s_backend;\n\nstruct _interpreter {\n PyObject* s_python_function_arrow;\n PyObject *s_python_function_show;\n PyObject *s_python_function_close;\n PyObject *s_python_function_draw;\n PyObject *s_python_function_pause;\n PyObject *s_python_function_save;\n PyObject *s_python_function_figure;\n PyObject *s_python_function_fignum_exists;\n PyObject *s_python_function_plot;\n PyObject *s_python_function_quiver;\n PyObject* s_python_function_contour;\n PyObject *s_python_function_semilogx;\n PyObject *s_python_function_semilogy;\n PyObject *s_python_function_loglog;\n PyObject *s_python_function_fill;\n PyObject *s_python_function_fill_between;\n PyObject *s_python_function_hist;\n PyObject *s_python_function_imshow;\n PyObject *s_python_function_scatter;\n PyObject *s_python_function_boxplot;\n PyObject *s_python_function_subplot;\n PyObject *s_python_function_subplot2grid;\n PyObject *s_python_function_legend;\n PyObject *s_python_function_xlim;\n PyObject *s_python_function_ion;\n PyObject *s_python_function_ginput;\n PyObject *s_python_function_ylim;\n PyObject *s_python_function_title;\n PyObject *s_python_function_axis;\n PyObject *s_python_function_axvline;\n PyObject *s_python_function_axvspan;\n PyObject *s_python_function_xlabel;\n PyObject *s_python_function_ylabel;\n PyObject *s_python_function_gca;\n PyObject *s_python_function_xticks;\n PyObject *s_python_function_yticks;\n PyObject* s_python_function_margins;\n PyObject *s_python_function_tick_params;\n PyObject *s_python_function_grid;\n PyObject* s_python_function_cla;\n PyObject *s_python_function_clf;\n PyObject *s_python_function_errorbar;\n PyObject *s_python_function_annotate;\n PyObject *s_python_function_tight_layout;\n PyObject *s_python_colormap;\n PyObject *s_python_empty_tuple;\n PyObject *s_python_function_stem;\n PyObject *s_python_function_xkcd;\n PyObject *s_python_function_text;\n PyObject *s_python_function_suptitle;\n PyObject *s_python_function_bar;\n PyObject *s_python_function_colorbar;\n PyObject *s_python_function_subplots_adjust;\n\n\n /* For now, _interpreter is implemented as a singleton since its currently not possible to have\n multiple independent embedded python interpreters without patching the python source code\n or starting a separate process for each.\n http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program\n */\n\n static _interpreter& get() {\n static _interpreter ctx;\n return ctx;\n }\n\n PyObject* safe_import(PyObject* module, std::string fname) {\n PyObject* fn = PyObject_GetAttrString(module, fname.c_str());\n\n if (!fn)\n throw std::runtime_error(std::string(\"Couldn't find required function: \") + fname);\n\n if (!PyFunction_Check(fn))\n throw std::runtime_error(fname + std::string(\" is unexpectedly not a PyFunction.\"));\n\n return fn;\n }\n\nprivate:\n\n#ifndef WITHOUT_NUMPY\n# if PY_MAJOR_VERSION >= 3\n\n void *import_numpy() {\n import_array(); // initialize C-API\n return NULL;\n }\n\n# else\n\n void import_numpy() {\n import_array(); // initialize C-API\n }\n\n# endif\n#endif\n\n _interpreter() {\n\n // optional but recommended\n#if PY_MAJOR_VERSION >= 3\n wchar_t name[] = L\"plotting\";\n#else\n char name[] = \"plotting\";\n#endif\n Py_SetProgramName(name);\n Py_Initialize();\n\n#ifndef WITHOUT_NUMPY\n import_numpy(); // initialize numpy C-API\n#endif\n\n PyObject* matplotlibname = PyString_FromString(\"matplotlib\");\n PyObject* pyplotname = PyString_FromString(\"matplotlib.pyplot\");\n PyObject* cmname = PyString_FromString(\"matplotlib.cm\");\n PyObject* pylabname = PyString_FromString(\"pylab\");\n if (!pyplotname || !pylabname || !matplotlibname || !cmname) {\n throw std::runtime_error(\"couldnt create string\");\n }\n\n PyObject* matplotlib = PyImport_Import(matplotlibname);\n Py_DECREF(matplotlibname);\n if (!matplotlib) {\n PyErr_Print();\n throw std::runtime_error(\"Error loading module matplotlib!\");\n }\n\n // matplotlib.use() must be called *before* pylab, matplotlib.pyplot,\n // or matplotlib.backends is imported for the first time\n if (!s_backend.empty()) {\n PyObject_CallMethod(matplotlib, const_cast(\"use\"), const_cast(\"s\"), s_backend.c_str());\n }\n\n PyObject* pymod = PyImport_Import(pyplotname);\n Py_DECREF(pyplotname);\n if (!pymod) { throw std::runtime_error(\"Error loading module matplotlib.pyplot!\"); }\n\n s_python_colormap = PyImport_Import(cmname);\n Py_DECREF(cmname);\n if (!s_python_colormap) { throw std::runtime_error(\"Error loading module matplotlib.cm!\"); }\n\n PyObject* pylabmod = PyImport_Import(pylabname);\n Py_DECREF(pylabname);\n if (!pylabmod) { throw std::runtime_error(\"Error loading module pylab!\"); }\n\n s_python_function_arrow = safe_import(pymod, \"arrow\");\n s_python_function_show = safe_import(pymod, \"show\");\n s_python_function_close = safe_import(pymod, \"close\");\n s_python_function_draw = safe_import(pymod, \"draw\");\n s_python_function_pause = safe_import(pymod, \"pause\");\n s_python_function_figure = safe_import(pymod, \"figure\");\n s_python_function_fignum_exists = safe_import(pymod, \"fignum_exists\");\n s_python_function_plot = safe_import(pymod, \"plot\");\n s_python_function_quiver = safe_import(pymod, \"quiver\");\n s_python_function_contour = safe_import(pymod, \"contour\");\n s_python_function_semilogx = safe_import(pymod, \"semilogx\");\n s_python_function_semilogy = safe_import(pymod, \"semilogy\");\n s_python_function_loglog = safe_import(pymod, \"loglog\");\n s_python_function_fill = safe_import(pymod, \"fill\");\n s_python_function_fill_between = safe_import(pymod, \"fill_between\");\n s_python_function_hist = safe_import(pymod,\"hist\");\n s_python_function_scatter = safe_import(pymod,\"scatter\");\n s_python_function_boxplot = safe_import(pymod,\"boxplot\");\n s_python_function_subplot = safe_import(pymod, \"subplot\");\n s_python_function_subplot2grid = safe_import(pymod, \"subplot2grid\");\n s_python_function_legend = safe_import(pymod, \"legend\");\n s_python_function_ylim = safe_import(pymod, \"ylim\");\n s_python_function_title = safe_import(pymod, \"title\");\n s_python_function_axis = safe_import(pymod, \"axis\");\n s_python_function_axvline = safe_import(pymod, \"axvline\");\n s_python_function_axvspan = safe_import(pymod, \"axvspan\");\n s_python_function_xlabel = safe_import(pymod, \"xlabel\");\n s_python_function_ylabel = safe_import(pymod, \"ylabel\");\n s_python_function_gca = safe_import(pymod, \"gca\");\n s_python_function_xticks = safe_import(pymod, \"xticks\");\n s_python_function_yticks = safe_import(pymod, \"yticks\");\n s_python_function_margins = safe_import(pymod, \"margins\");\n s_python_function_tick_params = safe_import(pymod, \"tick_params\");\n s_python_function_grid = safe_import(pymod, \"grid\");\n s_python_function_xlim = safe_import(pymod, \"xlim\");\n s_python_function_ion = safe_import(pymod, \"ion\");\n s_python_function_ginput = safe_import(pymod, \"ginput\");\n s_python_function_save = safe_import(pylabmod, \"savefig\");\n s_python_function_annotate = safe_import(pymod,\"annotate\");\n s_python_function_cla = safe_import(pymod, \"cla\");\n s_python_function_clf = safe_import(pymod, \"clf\");\n s_python_function_errorbar = safe_import(pymod, \"errorbar\");\n s_python_function_tight_layout = safe_import(pymod, \"tight_layout\");\n s_python_function_stem = safe_import(pymod, \"stem\");\n s_python_function_xkcd = safe_import(pymod, \"xkcd\");\n s_python_function_text = safe_import(pymod, \"text\");\n s_python_function_suptitle = safe_import(pymod, \"suptitle\");\n s_python_function_bar = safe_import(pymod,\"bar\");\n s_python_function_colorbar = PyObject_GetAttrString(pymod, \"colorbar\");\n s_python_function_subplots_adjust = safe_import(pymod,\"subplots_adjust\");\n#ifndef WITHOUT_NUMPY\n s_python_function_imshow = safe_import(pymod, \"imshow\");\n#endif\n s_python_empty_tuple = PyTuple_New(0);\n }\n\n ~_interpreter() {\n Py_Finalize();\n }\n};\n\n} // end namespace detail\n\n/// Select the backend\n///\n/// **NOTE:** This must be called before the first plot command to have\n/// any effect.\n///\n/// Mainly useful to select the non-interactive 'Agg' backend when running\n/// matplotlibcpp in headless mode, for example on a machine with no display.\n///\n/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use\ninline void backend(const std::string& name)\n{\n detail::s_backend = name;\n}\n\ninline bool annotate(std::string annotation, double x, double y)\n{\n detail::_interpreter::get();\n\n PyObject * xy = PyTuple_New(2);\n PyObject * str = PyString_FromString(annotation.c_str());\n\n PyTuple_SetItem(xy,0,PyFloat_FromDouble(x));\n PyTuple_SetItem(xy,1,PyFloat_FromDouble(y));\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"xy\", xy);\n\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, str);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n\n if(res) Py_DECREF(res);\n\n return res;\n}\n\nnamespace detail {\n\n#ifndef WITHOUT_NUMPY\n// Type selector for numpy array conversion\ntemplate struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; };\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; };\n\n// Sanity checks; comment them out or change the numpy type below if you're compiling on\n// a platform where they don't apply\nstatic_assert(sizeof(long long) == 8);\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; };\nstatic_assert(sizeof(unsigned long long) == 8);\ntemplate <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; };\n// TODO: add int, long, etc.\n\ntemplate\nPyObject* get_array(const std::vector& v)\n{\n npy_intp vsize = v.size();\n NPY_TYPES type = select_npy_type::type;\n if (type == NPY_NOTYPE) {\n size_t memsize = v.size()*sizeof(double);\n double* dp = static_cast(::malloc(memsize));\n for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA);\n return varray;\n }\n \n PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data()));\n return varray;\n}\n\n\ntemplate\nPyObject* get_2darray(const std::vector<::std::vector>& v)\n{\n if (v.size() < 1) throw std::runtime_error(\"get_2d_array v too small\");\n\n npy_intp vsize[2] = {static_cast(v.size()),\n static_cast(v[0].size())};\n\n PyArrayObject *varray =\n (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE);\n\n double *vd_begin = static_cast(PyArray_DATA(varray));\n\n for (const ::std::vector &v_row : v) {\n if (v_row.size() != static_cast(vsize[1]))\n throw std::runtime_error(\"Missmatched array size\");\n std::copy(v_row.begin(), v_row.end(), vd_begin);\n vd_begin += vsize[1];\n }\n\n return reinterpret_cast(varray);\n}\n\n#else // fallback if we don't have numpy: copy every element of the given vector\n\ntemplate\nPyObject* get_array(const std::vector& v)\n{\n PyObject* list = PyList_New(v.size());\n for(size_t i = 0; i < v.size(); ++i) {\n PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i)));\n }\n return list;\n}\n\n#endif // WITHOUT_NUMPY\n\n// sometimes, for labels and such, we need string arrays\ninline PyObject * get_array(const std::vector& strings)\n{\n PyObject* list = PyList_New(strings.size());\n for (std::size_t i = 0; i < strings.size(); ++i) {\n PyList_SetItem(list, i, PyString_FromString(strings[i].c_str()));\n }\n return list;\n}\n\n// not all matplotlib need 2d arrays, some prefer lists of lists\ntemplate\nPyObject* get_listlist(const std::vector>& ll)\n{\n PyObject* listlist = PyList_New(ll.size());\n for (std::size_t i = 0; i < ll.size(); ++i) {\n PyList_SetItem(listlist, i, get_array(ll[i]));\n }\n return listlist;\n}\n\n} // namespace detail\n\n/// Plot a line through the given x and y data points..\n/// \n/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html\ntemplate\nbool plot(const std::vector &x, const std::vector &y, const std::map& keywords)\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n // using numpy arrays\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n // construct positional args\n PyObject* args = PyTuple_New(2);\n PyTuple_SetItem(args, 0, xarray);\n PyTuple_SetItem(args, 1, yarray);\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\n// TODO - it should be possible to make this work by implementing\n// a non-numpy alternative for `detail::get_2darray()`.\n#ifndef WITHOUT_NUMPY\ntemplate \nvoid plot_surface(const std::vector<::std::vector> &x,\n const std::vector<::std::vector> &y,\n const std::vector<::std::vector> &z,\n const std::map &keywords =\n std::map())\n{\n detail::_interpreter::get();\n\n // We lazily load the modules here the first time this function is called\n // because I'm not sure that we can assume \"matplotlib installed\" implies\n // \"mpl_toolkits installed\" on all platforms, and we don't want to require\n // it for people who don't need 3d plots.\n static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;\n if (!mpl_toolkitsmod) {\n detail::_interpreter::get();\n\n PyObject* mpl_toolkits = PyString_FromString(\"mpl_toolkits\");\n PyObject* axis3d = PyString_FromString(\"mpl_toolkits.mplot3d\");\n if (!mpl_toolkits || !axis3d) { throw std::runtime_error(\"couldnt create string\"); }\n\n mpl_toolkitsmod = PyImport_Import(mpl_toolkits);\n Py_DECREF(mpl_toolkits);\n if (!mpl_toolkitsmod) { throw std::runtime_error(\"Error loading module mpl_toolkits!\"); }\n\n axis3dmod = PyImport_Import(axis3d);\n Py_DECREF(axis3d);\n if (!axis3dmod) { throw std::runtime_error(\"Error loading module mpl_toolkits.mplot3d!\"); }\n }\n\n assert(x.size() == y.size());\n assert(y.size() == z.size());\n\n // using numpy arrays\n PyObject *xarray = detail::get_2darray(x);\n PyObject *yarray = detail::get_2darray(y);\n PyObject *zarray = detail::get_2darray(z);\n\n // construct positional args\n PyObject *args = PyTuple_New(3);\n PyTuple_SetItem(args, 0, xarray);\n PyTuple_SetItem(args, 1, yarray);\n PyTuple_SetItem(args, 2, zarray);\n\n // Build up the kw args.\n PyObject *kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"rstride\", PyInt_FromLong(1));\n PyDict_SetItemString(kwargs, \"cstride\", PyInt_FromLong(1));\n\n PyObject *python_colormap_coolwarm = PyObject_GetAttrString(\n detail::_interpreter::get().s_python_colormap, \"coolwarm\");\n\n PyDict_SetItemString(kwargs, \"cmap\", python_colormap_coolwarm);\n\n for (std::map::const_iterator it = keywords.begin();\n it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(),\n PyString_FromString(it->second.c_str()));\n }\n\n\n PyObject *fig =\n PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,\n detail::_interpreter::get().s_python_empty_tuple);\n if (!fig) throw std::runtime_error(\"Call to figure() failed.\");\n\n PyObject *gca_kwargs = PyDict_New();\n PyDict_SetItemString(gca_kwargs, \"projection\", PyString_FromString(\"3d\"));\n\n PyObject *gca = PyObject_GetAttrString(fig, \"gca\");\n if (!gca) throw std::runtime_error(\"No gca\");\n Py_INCREF(gca);\n PyObject *axis = PyObject_Call(\n gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);\n\n if (!axis) throw std::runtime_error(\"No axis\");\n Py_INCREF(axis);\n\n Py_DECREF(gca);\n Py_DECREF(gca_kwargs);\n\n PyObject *plot_surface = PyObject_GetAttrString(axis, \"plot_surface\");\n if (!plot_surface) throw std::runtime_error(\"No surface\");\n Py_INCREF(plot_surface);\n PyObject *res = PyObject_Call(plot_surface, args, kwargs);\n if (!res) throw std::runtime_error(\"failed surface\");\n Py_DECREF(plot_surface);\n\n Py_DECREF(axis);\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if (res) Py_DECREF(res);\n}\n#endif // WITHOUT_NUMPY\n\ntemplate \nvoid plot3(const std::vector &x,\n const std::vector &y,\n const std::vector &z,\n const std::map &keywords =\n std::map())\n{\n detail::_interpreter::get();\n\n // Same as with plot_surface: We lazily load the modules here the first time \n // this function is called because I'm not sure that we can assume \"matplotlib \n // installed\" implies \"mpl_toolkits installed\" on all platforms, and we don't \n // want to require it for people who don't need 3d plots.\n static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;\n if (!mpl_toolkitsmod) {\n detail::_interpreter::get();\n\n PyObject* mpl_toolkits = PyString_FromString(\"mpl_toolkits\");\n PyObject* axis3d = PyString_FromString(\"mpl_toolkits.mplot3d\");\n if (!mpl_toolkits || !axis3d) { throw std::runtime_error(\"couldnt create string\"); }\n\n mpl_toolkitsmod = PyImport_Import(mpl_toolkits);\n Py_DECREF(mpl_toolkits);\n if (!mpl_toolkitsmod) { throw std::runtime_error(\"Error loading module mpl_toolkits!\"); }\n\n axis3dmod = PyImport_Import(axis3d);\n Py_DECREF(axis3d);\n if (!axis3dmod) { throw std::runtime_error(\"Error loading module mpl_toolkits.mplot3d!\"); }\n }\n\n assert(x.size() == y.size());\n assert(y.size() == z.size());\n\n PyObject *xarray = detail::get_array(x);\n PyObject *yarray = detail::get_array(y);\n PyObject *zarray = detail::get_array(z);\n\n // construct positional args\n PyObject *args = PyTuple_New(3);\n PyTuple_SetItem(args, 0, xarray);\n PyTuple_SetItem(args, 1, yarray);\n PyTuple_SetItem(args, 2, zarray);\n\n // Build up the kw args.\n PyObject *kwargs = PyDict_New();\n\n for (std::map::const_iterator it = keywords.begin();\n it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(),\n PyString_FromString(it->second.c_str()));\n }\n\n PyObject *fig =\n PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,\n detail::_interpreter::get().s_python_empty_tuple);\n if (!fig) throw std::runtime_error(\"Call to figure() failed.\");\n\n PyObject *gca_kwargs = PyDict_New();\n PyDict_SetItemString(gca_kwargs, \"projection\", PyString_FromString(\"3d\"));\n\n PyObject *gca = PyObject_GetAttrString(fig, \"gca\");\n if (!gca) throw std::runtime_error(\"No gca\");\n Py_INCREF(gca);\n PyObject *axis = PyObject_Call(\n gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);\n\n if (!axis) throw std::runtime_error(\"No axis\");\n Py_INCREF(axis);\n\n Py_DECREF(gca);\n Py_DECREF(gca_kwargs);\n\n PyObject *plot3 = PyObject_GetAttrString(axis, \"plot\");\n if (!plot3) throw std::runtime_error(\"No 3D line plot\");\n Py_INCREF(plot3);\n PyObject *res = PyObject_Call(plot3, args, kwargs);\n if (!res) throw std::runtime_error(\"Failed 3D line plot\");\n Py_DECREF(plot3);\n\n Py_DECREF(axis);\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if (res) Py_DECREF(res);\n}\n\ntemplate\nbool stem(const std::vector &x, const std::vector &y, const std::map& keywords)\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n // using numpy arrays\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n // construct positional args\n PyObject* args = PyTuple_New(2);\n PyTuple_SetItem(args, 0, xarray);\n PyTuple_SetItem(args, 1, yarray);\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for (std::map::const_iterator it =\n keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(),\n PyString_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(\n detail::_interpreter::get().s_python_function_stem, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if (res)\n Py_DECREF(res);\n\n return res;\n}\n\ntemplate< typename Numeric >\nbool fill(const std::vector& x, const std::vector& y, const std::map& keywords)\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n // using numpy arrays\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n // construct positional args\n PyObject* args = PyTuple_New(2);\n PyTuple_SetItem(args, 0, xarray);\n PyTuple_SetItem(args, 1, yarray);\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for (auto it = keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n\n if (res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate< typename Numeric >\nbool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords)\n{\n assert(x.size() == y1.size());\n assert(x.size() == y2.size());\n\n detail::_interpreter::get();\n\n // using numpy arrays\n PyObject* xarray = detail::get_array(x);\n PyObject* y1array = detail::get_array(y1);\n PyObject* y2array = detail::get_array(y2);\n\n // construct positional args\n PyObject* args = PyTuple_New(3);\n PyTuple_SetItem(args, 0, xarray);\n PyTuple_SetItem(args, 1, y1array);\n PyTuple_SetItem(args, 2, y2array);\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate \nbool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = \"r\",\n const std::string ec = \"k\", Numeric head_length = 0.25, Numeric head_width = 0.1625) {\n PyObject* obj_x = PyFloat_FromDouble(x);\n PyObject* obj_y = PyFloat_FromDouble(y);\n PyObject* obj_end_x = PyFloat_FromDouble(end_x);\n PyObject* obj_end_y = PyFloat_FromDouble(end_y);\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"fc\", PyString_FromString(fc.c_str()));\n PyDict_SetItemString(kwargs, \"ec\", PyString_FromString(ec.c_str()));\n PyDict_SetItemString(kwargs, \"head_width\", PyFloat_FromDouble(head_width));\n PyDict_SetItemString(kwargs, \"head_length\", PyFloat_FromDouble(head_length));\n\n PyObject* plot_args = PyTuple_New(4);\n PyTuple_SetItem(plot_args, 0, obj_x);\n PyTuple_SetItem(plot_args, 1, obj_y);\n PyTuple_SetItem(plot_args, 2, obj_end_x);\n PyTuple_SetItem(plot_args, 3, obj_end_y);\n\n PyObject* res =\n PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs);\n\n Py_DECREF(plot_args);\n Py_DECREF(kwargs);\n if (res)\n Py_DECREF(res);\n\n return res;\n}\n\ntemplate< typename Numeric>\nbool hist(const std::vector& y, long bins=10,std::string color=\"b\",\n double alpha=1.0, bool cumulative=false)\n{\n detail::_interpreter::get();\n\n PyObject* yarray = detail::get_array(y);\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"bins\", PyLong_FromLong(bins));\n PyDict_SetItemString(kwargs, \"color\", PyString_FromString(color.c_str()));\n PyDict_SetItemString(kwargs, \"alpha\", PyFloat_FromDouble(alpha));\n PyDict_SetItemString(kwargs, \"cumulative\", cumulative ? Py_True : Py_False);\n\n PyObject* plot_args = PyTuple_New(1);\n\n PyTuple_SetItem(plot_args, 0, yarray);\n\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs);\n\n\n Py_DECREF(plot_args);\n Py_DECREF(kwargs);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\n#ifndef WITHOUT_NUMPY\nnamespace detail {\n\ninline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out)\n{\n assert(type == NPY_UINT8 || type == NPY_FLOAT);\n assert(colors == 1 || colors == 3 || colors == 4);\n\n detail::_interpreter::get();\n\n // construct args\n npy_intp dims[3] = { rows, columns, colors };\n PyObject *args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr));\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs);\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if (!res)\n throw std::runtime_error(\"Call to imshow() failed\");\n if (out)\n *out = res;\n else\n Py_DECREF(res);\n}\n\n} // namespace detail\n\ninline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr)\n{\n detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out);\n}\n\ninline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr)\n{\n detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out);\n}\n\n#ifdef WITH_OPENCV\nvoid imshow(const cv::Mat &image, const std::map &keywords = {})\n{\n // Convert underlying type of matrix, if needed\n cv::Mat image2;\n NPY_TYPES npy_type = NPY_UINT8;\n switch (image.type() & CV_MAT_DEPTH_MASK) {\n case CV_8U:\n image2 = image;\n break;\n case CV_32F:\n image2 = image;\n npy_type = NPY_FLOAT;\n break;\n default:\n image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels()));\n }\n\n // If color image, convert from BGR to RGB\n switch (image2.channels()) {\n case 3:\n cv::cvtColor(image2, image2, CV_BGR2RGB);\n break;\n case 4:\n cv::cvtColor(image2, image2, CV_BGRA2RGBA);\n }\n\n detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords);\n}\n#endif // WITH_OPENCV\n#endif // WITHOUT_NUMPY\n\ntemplate\nbool scatter(const std::vector& x,\n const std::vector& y,\n const double s=1.0, // The marker size in points**2\n const std::map & keywords = {})\n{\n detail::_interpreter::get();\n\n assert(x.size() == y.size());\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"s\", PyLong_FromLong(s));\n for (const auto& it : keywords)\n {\n PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));\n }\n\n PyObject* plot_args = PyTuple_New(2);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs);\n\n Py_DECREF(plot_args);\n Py_DECREF(kwargs);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool boxplot(const std::vector>& data,\n const std::vector& labels = {},\n const std::map & keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject* listlist = detail::get_listlist(data);\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, listlist);\n\n PyObject* kwargs = PyDict_New();\n\n // kwargs needs the labels, if there are (the correct number of) labels\n if (!labels.empty() && labels.size() == data.size()) {\n PyDict_SetItemString(kwargs, \"labels\", detail::get_array(labels));\n }\n\n // take care of the remaining keywords\n for (const auto& it : keywords)\n {\n PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool boxplot(const std::vector& data,\n const std::map & keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject* vector = detail::get_array(data);\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, vector);\n\n PyObject* kwargs = PyDict_New();\n for (const auto& it : keywords)\n {\n PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate \nbool bar(const std::vector & x,\n const std::vector & y,\n std::string ec = \"black\",\n std::string ls = \"-\",\n double lw = 1.0,\n const std::map & keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject * xarray = detail::get_array(x);\n PyObject * yarray = detail::get_array(y);\n\n PyObject * kwargs = PyDict_New();\n\n PyDict_SetItemString(kwargs, \"ec\", PyString_FromString(ec.c_str()));\n PyDict_SetItemString(kwargs, \"ls\", PyString_FromString(ls.c_str()));\n PyDict_SetItemString(kwargs, \"lw\", PyFloat_FromDouble(lw));\n\n for (std::map::const_iterator it =\n keywords.begin();\n it != keywords.end();\n ++it) {\n PyDict_SetItemString(\n kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject * plot_args = PyTuple_New(2);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n\n PyObject * res = PyObject_Call(\n detail::_interpreter::get().s_python_function_bar, plot_args, kwargs);\n\n Py_DECREF(plot_args);\n Py_DECREF(kwargs);\n if (res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate \nbool bar(const std::vector & y,\n std::string ec = \"black\",\n std::string ls = \"-\",\n double lw = 1.0,\n const std::map & keywords = {})\n{\n using T = typename std::remove_reference::type::value_type;\n\n detail::_interpreter::get();\n\n std::vector x;\n for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); }\n\n return bar(x, y, ec, ls, lw, keywords);\n}\n\ninline bool subplots_adjust(const std::map& keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject* kwargs = PyDict_New();\n for (std::map::const_iterator it =\n keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(),\n PyFloat_FromDouble(it->second));\n }\n\n\n PyObject* plot_args = PyTuple_New(0);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs);\n\n Py_DECREF(plot_args);\n Py_DECREF(kwargs);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate< typename Numeric>\nbool named_hist(std::string label,const std::vector& y, long bins=10, std::string color=\"b\", double alpha=1.0)\n{\n detail::_interpreter::get();\n\n PyObject* yarray = detail::get_array(y);\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"label\", PyString_FromString(label.c_str()));\n PyDict_SetItemString(kwargs, \"bins\", PyLong_FromLong(bins));\n PyDict_SetItemString(kwargs, \"color\", PyString_FromString(color.c_str()));\n PyDict_SetItemString(kwargs, \"alpha\", PyFloat_FromDouble(alpha));\n\n\n PyObject* plot_args = PyTuple_New(1);\n PyTuple_SetItem(plot_args, 0, yarray);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs);\n\n Py_DECREF(plot_args);\n Py_DECREF(kwargs);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool plot(const std::vector& x, const std::vector& y, const std::string& s = \"\")\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(s.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args);\n\n Py_DECREF(plot_args);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate \nbool contour(const std::vector& x, const std::vector& y,\n const std::vector& z,\n const std::map& keywords = {}) {\n assert(x.size() == y.size() && x.size() == z.size());\n\n PyObject* xarray = get_array(x);\n PyObject* yarray = get_array(y);\n PyObject* zarray = get_array(z);\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, zarray);\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for (std::map::const_iterator it = keywords.begin();\n it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res =\n PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n if (res)\n Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {})\n{\n assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size());\n\n detail::_interpreter::get();\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n PyObject* uarray = detail::get_array(u);\n PyObject* warray = detail::get_array(w);\n\n PyObject* plot_args = PyTuple_New(4);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, uarray);\n PyTuple_SetItem(plot_args, 3, warray);\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(\n detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n if (res)\n Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool stem(const std::vector& x, const std::vector& y, const std::string& s = \"\")\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(s.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_CallObject(\n detail::_interpreter::get().s_python_function_stem, plot_args);\n\n Py_DECREF(plot_args);\n if (res)\n Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool semilogx(const std::vector& x, const std::vector& y, const std::string& s = \"\")\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(s.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args);\n\n Py_DECREF(plot_args);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool semilogy(const std::vector& x, const std::vector& y, const std::string& s = \"\")\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(s.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args);\n\n Py_DECREF(plot_args);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool loglog(const std::vector& x, const std::vector& y, const std::string& s = \"\")\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(s.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args);\n\n Py_DECREF(plot_args);\n if(res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {})\n{\n assert(x.size() == y.size());\n\n detail::_interpreter::get();\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n PyObject* yerrarray = detail::get_array(yerr);\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n PyDict_SetItemString(kwargs, \"yerr\", yerrarray);\n\n PyObject *plot_args = PyTuple_New(2);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n\n PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n\n if (res)\n Py_DECREF(res);\n else\n throw std::runtime_error(\"Call to errorbar() failed.\");\n\n return res;\n}\n\ntemplate\nbool named_plot(const std::string& name, const std::vector& y, const std::string& format = \"\")\n{\n detail::_interpreter::get();\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"label\", PyString_FromString(name.c_str()));\n\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(format.c_str());\n\n PyObject* plot_args = PyTuple_New(2);\n\n PyTuple_SetItem(plot_args, 0, yarray);\n PyTuple_SetItem(plot_args, 1, pystring);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n if (res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = \"\")\n{\n detail::_interpreter::get();\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"label\", PyString_FromString(name.c_str()));\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(format.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n if (res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = \"\")\n{\n detail::_interpreter::get();\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"label\", PyString_FromString(name.c_str()));\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(format.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n if (res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = \"\")\n{\n detail::_interpreter::get();\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"label\", PyString_FromString(name.c_str()));\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(format.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n if (res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = \"\")\n{\n detail::_interpreter::get();\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"label\", PyString_FromString(name.c_str()));\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(format.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n if (res) Py_DECREF(res);\n\n return res;\n}\n\ntemplate\nbool plot(const std::vector& y, const std::string& format = \"\")\n{\n std::vector x(y.size());\n for(size_t i=0; i\nbool plot(const std::vector& y, const std::map& keywords)\n{\n std::vector x(y.size());\n for(size_t i=0; i\nbool stem(const std::vector& y, const std::string& format = \"\")\n{\n std::vector x(y.size());\n for (size_t i = 0; i < x.size(); ++i) x.at(i) = i;\n return stem(x, y, format);\n}\n\ntemplate\nvoid text(Numeric x, Numeric y, const std::string& s = \"\")\n{\n detail::_interpreter::get();\n\n PyObject* args = PyTuple_New(3);\n PyTuple_SetItem(args, 0, PyFloat_FromDouble(x));\n PyTuple_SetItem(args, 1, PyFloat_FromDouble(y));\n PyTuple_SetItem(args, 2, PyString_FromString(s.c_str()));\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args);\n if(!res) throw std::runtime_error(\"Call to text() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ninline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {})\n{\n if (mappable == NULL)\n throw std::runtime_error(\"Must call colorbar with PyObject* returned from an image, contour, surface, etc.\");\n\n detail::_interpreter::get();\n\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, mappable);\n\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs);\n if(!res) throw std::runtime_error(\"Call to colorbar() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n Py_DECREF(res);\n}\n\n\ninline long figure(long number = -1)\n{\n detail::_interpreter::get();\n\n PyObject *res;\n if (number == -1)\n res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple);\n else {\n assert(number > 0);\n\n // Make sure interpreter is initialised\n detail::_interpreter::get();\n\n PyObject *args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, PyLong_FromLong(number));\n res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args);\n Py_DECREF(args);\n }\n\n if(!res) throw std::runtime_error(\"Call to figure() failed.\");\n\n PyObject* num = PyObject_GetAttrString(res, \"number\");\n if (!num) throw std::runtime_error(\"Could not get number attribute of figure object\");\n const long figureNumber = PyLong_AsLong(num);\n\n Py_DECREF(num);\n Py_DECREF(res);\n\n return figureNumber;\n}\n\ninline bool fignum_exists(long number)\n{\n detail::_interpreter::get();\n\n PyObject *args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, PyLong_FromLong(number));\n PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args);\n if(!res) throw std::runtime_error(\"Call to fignum_exists() failed.\");\n\n bool ret = PyObject_IsTrue(res);\n Py_DECREF(res);\n Py_DECREF(args);\n\n return ret;\n}\n\ninline void figure_size(size_t w, size_t h)\n{\n detail::_interpreter::get();\n\n const size_t dpi = 100;\n PyObject* size = PyTuple_New(2);\n PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi));\n PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi));\n\n PyObject* kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"figsize\", size);\n PyDict_SetItemString(kwargs, \"dpi\", PyLong_FromSize_t(dpi));\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure,\n detail::_interpreter::get().s_python_empty_tuple, kwargs);\n\n Py_DECREF(kwargs);\n\n if(!res) throw std::runtime_error(\"Call to figure_size() failed.\");\n Py_DECREF(res);\n}\n\ninline void legend()\n{\n detail::_interpreter::get();\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple);\n if(!res) throw std::runtime_error(\"Call to legend() failed.\");\n\n Py_DECREF(res);\n}\n\ninline void legend(const std::map& keywords)\n{\n detail::_interpreter::get();\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs);\n if(!res) throw std::runtime_error(\"Call to legend() failed.\");\n\n Py_DECREF(kwargs);\n Py_DECREF(res); \n}\n\ntemplate\nvoid ylim(Numeric left, Numeric right)\n{\n detail::_interpreter::get();\n\n PyObject* list = PyList_New(2);\n PyList_SetItem(list, 0, PyFloat_FromDouble(left));\n PyList_SetItem(list, 1, PyFloat_FromDouble(right));\n\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, list);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args);\n if(!res) throw std::runtime_error(\"Call to ylim() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ntemplate\nvoid xlim(Numeric left, Numeric right)\n{\n detail::_interpreter::get();\n\n PyObject* list = PyList_New(2);\n PyList_SetItem(list, 0, PyFloat_FromDouble(left));\n PyList_SetItem(list, 1, PyFloat_FromDouble(right));\n\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, list);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args);\n if(!res) throw std::runtime_error(\"Call to xlim() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\n\ninline double* xlim()\n{\n detail::_interpreter::get();\n\n PyObject* args = PyTuple_New(0);\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args);\n PyObject* left = PyTuple_GetItem(res,0);\n PyObject* right = PyTuple_GetItem(res,1);\n\n double* arr = new double[2];\n arr[0] = PyFloat_AsDouble(left);\n arr[1] = PyFloat_AsDouble(right);\n\n if(!res) throw std::runtime_error(\"Call to xlim() failed.\");\n\n Py_DECREF(res);\n return arr;\n}\n\n\ninline double* ylim()\n{\n detail::_interpreter::get();\n\n PyObject* args = PyTuple_New(0);\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args);\n PyObject* left = PyTuple_GetItem(res,0);\n PyObject* right = PyTuple_GetItem(res,1);\n\n double* arr = new double[2];\n arr[0] = PyFloat_AsDouble(left);\n arr[1] = PyFloat_AsDouble(right);\n\n if(!res) throw std::runtime_error(\"Call to ylim() failed.\");\n\n Py_DECREF(res);\n return arr;\n}\n\ntemplate\ninline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {})\n{\n assert(labels.size() == 0 || ticks.size() == labels.size());\n\n detail::_interpreter::get();\n\n // using numpy array\n PyObject* ticksarray = detail::get_array(ticks);\n\n PyObject* args;\n if(labels.size() == 0) {\n // construct positional args\n args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, ticksarray);\n } else {\n // make tuple of tick labels\n PyObject* labelstuple = PyTuple_New(labels.size());\n for (size_t i = 0; i < labels.size(); i++)\n PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str()));\n\n // construct positional args\n args = PyTuple_New(2);\n PyTuple_SetItem(args, 0, ticksarray);\n PyTuple_SetItem(args, 1, labelstuple);\n }\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if(!res) throw std::runtime_error(\"Call to xticks() failed\");\n\n Py_DECREF(res);\n}\n\ntemplate\ninline void xticks(const std::vector &ticks, const std::map& keywords)\n{\n xticks(ticks, {}, keywords);\n}\n\ntemplate\ninline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {})\n{\n assert(labels.size() == 0 || ticks.size() == labels.size());\n\n detail::_interpreter::get();\n\n // using numpy array\n PyObject* ticksarray = detail::get_array(ticks);\n\n PyObject* args;\n if(labels.size() == 0) {\n // construct positional args\n args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, ticksarray);\n } else {\n // make tuple of tick labels\n PyObject* labelstuple = PyTuple_New(labels.size());\n for (size_t i = 0; i < labels.size(); i++)\n PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str()));\n\n // construct positional args\n args = PyTuple_New(2);\n PyTuple_SetItem(args, 0, ticksarray);\n PyTuple_SetItem(args, 1, labelstuple);\n }\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if(!res) throw std::runtime_error(\"Call to yticks() failed\");\n\n Py_DECREF(res);\n}\n\ntemplate\ninline void yticks(const std::vector &ticks, const std::map& keywords)\n{\n yticks(ticks, {}, keywords);\n}\n\ntemplate inline void margins(Numeric margin)\n{\n // construct positional args\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin));\n\n PyObject* res =\n PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args);\n if (!res)\n throw std::runtime_error(\"Call to margins() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ntemplate inline void margins(Numeric margin_x, Numeric margin_y)\n{\n // construct positional args\n PyObject* args = PyTuple_New(2);\n PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x));\n PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y));\n\n PyObject* res =\n PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args);\n if (!res)\n throw std::runtime_error(\"Call to margins() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\n\ninline void tick_params(const std::map& keywords, const std::string axis = \"both\")\n{\n detail::_interpreter::get();\n\n // construct positional args\n PyObject* args;\n args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str()));\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if (!res) throw std::runtime_error(\"Call to tick_params() failed\");\n\n Py_DECREF(res);\n}\n\ninline void subplot(long nrows, long ncols, long plot_number)\n{\n detail::_interpreter::get();\n \n // construct positional args\n PyObject* args = PyTuple_New(3);\n PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows));\n PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols));\n PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number));\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args);\n if(!res) throw std::runtime_error(\"Call to subplot() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ninline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1)\n{\n detail::_interpreter::get();\n\n PyObject* shape = PyTuple_New(2);\n PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows));\n PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols));\n\n PyObject* loc = PyTuple_New(2);\n PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid));\n PyTuple_SetItem(loc, 1, PyLong_FromLong(colid));\n\n PyObject* args = PyTuple_New(4);\n PyTuple_SetItem(args, 0, shape);\n PyTuple_SetItem(args, 1, loc);\n PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan));\n PyTuple_SetItem(args, 3, PyLong_FromLong(colspan));\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args);\n if(!res) throw std::runtime_error(\"Call to subplot2grid() failed.\");\n\n Py_DECREF(shape);\n Py_DECREF(loc);\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ninline void title(const std::string &titlestr, const std::map &keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject* pytitlestr = PyString_FromString(titlestr.c_str());\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, pytitlestr);\n\n PyObject* kwargs = PyDict_New();\n for (auto it = keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs);\n if(!res) throw std::runtime_error(\"Call to title() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n Py_DECREF(res);\n}\n\ninline void suptitle(const std::string &suptitlestr, const std::map &keywords = {})\n{\n detail::_interpreter::get();\n \n PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str());\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, pysuptitlestr);\n\n PyObject* kwargs = PyDict_New();\n for (auto it = keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs);\n if(!res) throw std::runtime_error(\"Call to suptitle() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n Py_DECREF(res);\n}\n\ninline void axis(const std::string &axisstr)\n{\n detail::_interpreter::get();\n\n PyObject* str = PyString_FromString(axisstr.c_str());\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, str);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args);\n if(!res) throw std::runtime_error(\"Call to title() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ninline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map())\n{\n detail::_interpreter::get();\n\n // construct positional args\n PyObject* args = PyTuple_New(3);\n PyTuple_SetItem(args, 0, PyFloat_FromDouble(x));\n PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin));\n PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax));\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs);\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n\n if(res) Py_DECREF(res);\n}\n\ninline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map())\n{\n // construct positional args\n PyObject* args = PyTuple_New(4);\n PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin));\n PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax));\n PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin));\n PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax));\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n\tif (it->first == \"linewidth\" || it->first == \"alpha\")\n \t PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second)));\n \telse\n \t PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs);\n Py_DECREF(args);\n Py_DECREF(kwargs);\n\n if(res) Py_DECREF(res);\n}\n\ninline void xlabel(const std::string &str, const std::map &keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject* pystr = PyString_FromString(str.c_str());\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, pystr);\n\n PyObject* kwargs = PyDict_New();\n for (auto it = keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs);\n if(!res) throw std::runtime_error(\"Call to xlabel() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n Py_DECREF(res);\n}\n\ninline void ylabel(const std::string &str, const std::map& keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject* pystr = PyString_FromString(str.c_str());\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, pystr);\n\n PyObject* kwargs = PyDict_New();\n for (auto it = keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs);\n if(!res) throw std::runtime_error(\"Call to ylabel() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(kwargs);\n Py_DECREF(res);\n}\n\ninline void set_zlabel(const std::string &str, const std::map& keywords = {})\n{\n detail::_interpreter::get();\n\n // Same as with plot_surface: We lazily load the modules here the first time \n // this function is called because I'm not sure that we can assume \"matplotlib \n // installed\" implies \"mpl_toolkits installed\" on all platforms, and we don't \n // want to require it for people who don't need 3d plots.\n static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;\n if (!mpl_toolkitsmod) {\n PyObject* mpl_toolkits = PyString_FromString(\"mpl_toolkits\");\n PyObject* axis3d = PyString_FromString(\"mpl_toolkits.mplot3d\");\n if (!mpl_toolkits || !axis3d) { throw std::runtime_error(\"couldnt create string\"); }\n\n mpl_toolkitsmod = PyImport_Import(mpl_toolkits);\n Py_DECREF(mpl_toolkits);\n if (!mpl_toolkitsmod) { throw std::runtime_error(\"Error loading module mpl_toolkits!\"); }\n\n axis3dmod = PyImport_Import(axis3d);\n Py_DECREF(axis3d);\n if (!axis3dmod) { throw std::runtime_error(\"Error loading module mpl_toolkits.mplot3d!\"); }\n }\n\n PyObject* pystr = PyString_FromString(str.c_str());\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, pystr);\n\n PyObject* kwargs = PyDict_New();\n for (auto it = keywords.begin(); it != keywords.end(); ++it) {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject *ax =\n PyObject_CallObject(detail::_interpreter::get().s_python_function_gca,\n detail::_interpreter::get().s_python_empty_tuple);\n if (!ax) throw std::runtime_error(\"Call to gca() failed.\");\n Py_INCREF(ax);\n\n PyObject *zlabel = PyObject_GetAttrString(ax, \"set_zlabel\");\n if (!zlabel) throw std::runtime_error(\"Attribute set_zlabel not found.\");\n Py_INCREF(zlabel);\n\n PyObject *res = PyObject_Call(zlabel, args, kwargs);\n if (!res) throw std::runtime_error(\"Call to set_zlabel() failed.\");\n Py_DECREF(zlabel);\n\n Py_DECREF(ax);\n Py_DECREF(args);\n Py_DECREF(kwargs);\n if (res) Py_DECREF(res);\n}\n\ninline void grid(bool flag)\n{\n detail::_interpreter::get();\n\n PyObject* pyflag = flag ? Py_True : Py_False;\n Py_INCREF(pyflag);\n\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, pyflag);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args);\n if(!res) throw std::runtime_error(\"Call to grid() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ninline void show(const bool block = true)\n{\n detail::_interpreter::get();\n\n PyObject* res;\n if(block)\n {\n res = PyObject_CallObject(\n detail::_interpreter::get().s_python_function_show,\n detail::_interpreter::get().s_python_empty_tuple);\n }\n else\n {\n PyObject *kwargs = PyDict_New();\n PyDict_SetItemString(kwargs, \"block\", Py_False);\n res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs);\n Py_DECREF(kwargs);\n }\n\n\n if (!res) throw std::runtime_error(\"Call to show() failed.\");\n\n Py_DECREF(res);\n}\n\ninline void close()\n{\n detail::_interpreter::get();\n\n PyObject* res = PyObject_CallObject(\n detail::_interpreter::get().s_python_function_close,\n detail::_interpreter::get().s_python_empty_tuple);\n\n if (!res) throw std::runtime_error(\"Call to close() failed.\");\n\n Py_DECREF(res);\n}\n\ninline void xkcd() {\n detail::_interpreter::get();\n\n PyObject* res;\n PyObject *kwargs = PyDict_New();\n\n res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd,\n detail::_interpreter::get().s_python_empty_tuple, kwargs);\n\n Py_DECREF(kwargs);\n\n if (!res)\n throw std::runtime_error(\"Call to show() failed.\");\n\n Py_DECREF(res);\n}\n\ninline void draw()\n{\n detail::_interpreter::get();\n\n PyObject* res = PyObject_CallObject(\n detail::_interpreter::get().s_python_function_draw,\n detail::_interpreter::get().s_python_empty_tuple);\n\n if (!res) throw std::runtime_error(\"Call to draw() failed.\");\n\n Py_DECREF(res);\n}\n\ntemplate\ninline void pause(Numeric interval)\n{\n detail::_interpreter::get();\n\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval));\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args);\n if(!res) throw std::runtime_error(\"Call to pause() failed.\");\n\n Py_DECREF(args);\n Py_DECREF(res);\n}\n\ninline void save(const std::string& filename)\n{\n detail::_interpreter::get();\n\n PyObject* pyfilename = PyString_FromString(filename.c_str());\n\n PyObject* args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, pyfilename);\n std::cout<<\"args:\"<> ginput(const int numClicks = 1, const std::map& keywords = {})\n{\n detail::_interpreter::get();\n\n PyObject *args = PyTuple_New(1);\n PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks));\n\n // construct keyword args\n PyObject* kwargs = PyDict_New();\n for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it)\n {\n PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));\n }\n\n PyObject* res = PyObject_Call(\n detail::_interpreter::get().s_python_function_ginput, args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(args);\n if (!res) throw std::runtime_error(\"Call to ginput() failed.\");\n\n const size_t len = PyList_Size(res);\n std::vector> out;\n out.reserve(len);\n for (size_t i = 0; i < len; i++) {\n PyObject *current = PyList_GetItem(res, i);\n std::array position;\n position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0));\n position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1));\n out.push_back(position);\n }\n Py_DECREF(res);\n\n return out;\n}\n\n// Actually, is there any reason not to call this automatically for every plot?\ninline void tight_layout() {\n detail::_interpreter::get();\n\n PyObject *res = PyObject_CallObject(\n detail::_interpreter::get().s_python_function_tight_layout,\n detail::_interpreter::get().s_python_empty_tuple);\n\n if (!res) throw std::runtime_error(\"Call to tight_layout() failed.\");\n\n Py_DECREF(res);\n}\n\n// Support for variadic plot() and initializer lists:\n\nnamespace detail {\n\ntemplate\nusing is_function = typename std::is_function>>::type;\n\ntemplate\nstruct is_callable_impl;\n\ntemplate\nstruct is_callable_impl\n{\n typedef is_function type;\n}; // a non-object is callable iff it is a function\n\ntemplate\nstruct is_callable_impl\n{\n struct Fallback { void operator()(); };\n struct Derived : T, Fallback { };\n\n template struct Check;\n\n template\n static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match\n\n template\n static std::false_type test( Check* );\n\npublic:\n typedef decltype(test(nullptr)) type;\n typedef decltype(&Fallback::operator()) dtype;\n static constexpr bool value = type::value;\n}; // an object is callable iff it defines operator()\n\ntemplate\nstruct is_callable\n{\n // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not\n typedef typename is_callable_impl::value, T>::type type;\n};\n\ntemplate\nstruct plot_impl { };\n\ntemplate<>\nstruct plot_impl\n{\n template\n bool operator()(const IterableX& x, const IterableY& y, const std::string& format)\n {\n // 2-phase lookup for distance, begin, end\n using std::distance;\n using std::begin;\n using std::end;\n\n auto xs = distance(begin(x), end(x));\n auto ys = distance(begin(y), end(y));\n assert(xs == ys && \"x and y data must have the same number of elements!\");\n\n PyObject* xlist = PyList_New(xs);\n PyObject* ylist = PyList_New(ys);\n PyObject* pystring = PyString_FromString(format.c_str());\n\n auto itx = begin(x), ity = begin(y);\n for(size_t i = 0; i < xs; ++i) {\n PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++));\n PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++));\n }\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xlist);\n PyTuple_SetItem(plot_args, 1, ylist);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args);\n\n Py_DECREF(plot_args);\n if(res) Py_DECREF(res);\n\n return res;\n }\n};\n\ntemplate<>\nstruct plot_impl\n{\n template\n bool operator()(const Iterable& ticks, const Callable& f, const std::string& format)\n {\n if(begin(ticks) == end(ticks)) return true;\n\n // We could use additional meta-programming to deduce the correct element type of y,\n // but all values have to be convertible to double anyways\n std::vector y;\n for(auto x : ticks) y.push_back(f(x));\n return plot_impl()(ticks,y,format);\n }\n};\n\n} // end namespace detail\n\n// recursion stop for the above\ntemplate\nbool plot() { return true; }\n\ntemplate\nbool plot(const A& a, const B& b, const std::string& format, Args... args)\n{\n return detail::plot_impl::type>()(a,b,format) && plot(args...);\n}\n\n/*\n * This group of plot() functions is needed to support initializer lists, i.e. calling\n * plot( {1,2,3,4} )\n */\ninline bool plot(const std::vector& x, const std::vector& y, const std::string& format = \"\") {\n return plot(x,y,format);\n}\n\ninline bool plot(const std::vector& y, const std::string& format = \"\") {\n return plot(y,format);\n}\n\ninline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) {\n return plot(x,y,keywords);\n}\n\n/*\n * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting\n */\nclass Plot\n{\npublic:\n // default initialization with plot label, some data and format\n template\n Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = \"\") {\n detail::_interpreter::get();\n\n assert(x.size() == y.size());\n\n PyObject* kwargs = PyDict_New();\n if(name != \"\")\n PyDict_SetItemString(kwargs, \"label\", PyString_FromString(name.c_str()));\n\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* pystring = PyString_FromString(format.c_str());\n\n PyObject* plot_args = PyTuple_New(3);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n PyTuple_SetItem(plot_args, 2, pystring);\n\n PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);\n\n Py_DECREF(kwargs);\n Py_DECREF(plot_args);\n\n if(res)\n {\n line= PyList_GetItem(res, 0);\n\n if(line)\n set_data_fct = PyObject_GetAttrString(line,\"set_data\");\n else\n Py_DECREF(line);\n Py_DECREF(res);\n }\n }\n\n // shorter initialization with name or format only\n // basically calls line, = plot([], [])\n Plot(const std::string& name = \"\", const std::string& format = \"\")\n : Plot(name, std::vector(), std::vector(), format) {}\n\n template\n bool update(const std::vector& x, const std::vector& y) {\n assert(x.size() == y.size());\n if(set_data_fct)\n {\n PyObject* xarray = detail::get_array(x);\n PyObject* yarray = detail::get_array(y);\n\n PyObject* plot_args = PyTuple_New(2);\n PyTuple_SetItem(plot_args, 0, xarray);\n PyTuple_SetItem(plot_args, 1, yarray);\n\n PyObject* res = PyObject_CallObject(set_data_fct, plot_args);\n if (res) Py_DECREF(res);\n return res;\n }\n return false;\n }\n\n // clears the plot but keep it available\n bool clear() {\n return update(std::vector(), std::vector());\n }\n\n // definitely remove this line\n void remove() {\n if(line)\n {\n auto remove_fct = PyObject_GetAttrString(line,\"remove\");\n PyObject* args = PyTuple_New(0);\n PyObject* res = PyObject_CallObject(remove_fct, args);\n if (res) Py_DECREF(res);\n }\n decref();\n }\n\n ~Plot() {\n decref();\n }\nprivate:\n\n void decref() {\n if(line)\n Py_DECREF(line);\n if(set_data_fct)\n Py_DECREF(set_data_fct);\n }\n\n\n PyObject* line = nullptr;\n PyObject* set_data_fct = nullptr;\n};\n\n} // end namespace matplotlibcpp\n" }, { "path": "FAST-LIO/include/so3_math.h", "content": "#ifndef SO3_MATH_H\n#define SO3_MATH_H\n\n#include \n#include \n\n#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0\n\ntemplate\nEigen::Matrix skew_sym_mat(const Eigen::Matrix &v)\n{\n Eigen::Matrix skew_sym_mat;\n skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0;\n return skew_sym_mat;\n}\n\ntemplate\nEigen::Matrix Exp(const Eigen::Matrix &&ang)\n{\n T ang_norm = ang.norm();\n Eigen::Matrix Eye3 = Eigen::Matrix::Identity();\n if (ang_norm > 0.0000001)\n {\n Eigen::Matrix r_axis = ang / ang_norm;\n Eigen::Matrix K;\n K << SKEW_SYM_MATRX(r_axis);\n /// Roderigous Tranformation\n return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;\n }\n else\n {\n return Eye3;\n }\n}\n\ntemplate\nEigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt)\n{\n T ang_vel_norm = ang_vel.norm();\n Eigen::Matrix Eye3 = Eigen::Matrix::Identity();\n\n if (ang_vel_norm > 0.0000001)\n {\n Eigen::Matrix r_axis = ang_vel / ang_vel_norm;\n Eigen::Matrix K;\n\n K << SKEW_SYM_MATRX(r_axis);\n\n T r_ang = ang_vel_norm * dt;\n\n /// Roderigous Tranformation\n return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;\n }\n else\n {\n return Eye3;\n }\n}\n\ntemplate\nEigen::Matrix Exp(const T &v1, const T &v2, const T &v3)\n{\n T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);\n Eigen::Matrix Eye3 = Eigen::Matrix::Identity();\n if (norm > 0.00001)\n {\n T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};\n Eigen::Matrix K;\n K << SKEW_SYM_MATRX(r_ang);\n\n /// Roderigous Tranformation\n return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;\n }\n else\n {\n return Eye3;\n }\n}\n\n/* Logrithm of a Rotation Matrix */\ntemplate\nEigen::Matrix Log(const Eigen::Matrix &R)\n{\n T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));\n Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));\n return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);\n}\n\ntemplate\nEigen::Matrix RotMtoEuler(const Eigen::Matrix &rot)\n{\n T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));\n bool singular = sy < 1e-6;\n T x, y, z;\n if(!singular)\n {\n x = atan2(rot(2, 1), rot(2, 2));\n y = atan2(-rot(2, 0), sy); \n z = atan2(rot(1, 0), rot(0, 0)); \n }\n else\n { \n x = atan2(-rot(1, 2), rot(1, 1)); \n y = atan2(-rot(2, 0), sy); \n z = 0;\n }\n Eigen::Matrix ang(x, y, z);\n return ang;\n}\n\n#endif\n" }, { "path": "FAST-LIO/include/use-ikfom.hpp", "content": "#ifndef USE_IKFOM_H\n#define USE_IKFOM_H\n\n#include \n\ntypedef MTK::vect<3, double> vect3;\ntypedef MTK::SO3 SO3;\ntypedef MTK::S2 S2; // S2 流形\ntypedef MTK::vect<1, double> vect1;\ntypedef MTK::vect<2, double> vect2;\n\n// 定义的ieskf状态空间\nMTK_BUILD_MANIFOLD(state_ikfom,\n\t\t\t\t ((vect3, pos))((SO3, rot))((SO3, offset_R_L_I))((vect3, offset_T_L_I))((vect3, vel))((vect3, bg))((vect3, ba)) // S2流形,grav为负值\n\t\t\t\t ((S2, grav)));\n\n// 定义的输入状态\nMTK_BUILD_MANIFOLD(input_ikfom,\n\t\t\t\t ((vect3, acc))((vect3, gyro)));\n\n//定义的协方差噪声格式\n//角速度(3),加速度(3),角速度偏置(3),加速度偏置(3)\nMTK_BUILD_MANIFOLD(process_noise_ikfom,\n\t\t\t\t ((vect3, ng))((vect3, na))((vect3, nbg))((vect3, nba)));\n\n// 噪声协方差初始化\nMTK::get_cov::type process_noise_cov()\n{\n\tMTK::get_cov::type cov = MTK::get_cov::type::Zero();\n\tMTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);\t // 0.03\n\tMTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001);\t // *dt 0.01 0.01 * dt * dt 0.05\n\tMTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01\n\tMTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); // 0.001 0.05 0.0001/out 0.01\n\treturn cov;\n}\n\n// double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia\n// vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);\n// fast_lio2论文公式(2), 起始这里的f就是将imu的积分方程组成矩阵形式然后再去计算\nEigen::Matrix get_f(state_ikfom &s, const input_ikfom &in)\n{\n\tEigen::Matrix res = Eigen::Matrix::Zero(); // 将imu积分方程矩阵初始化为0,这里的24个对应了速度(3)，角速度(3),外参偏置T(3),外参偏置R(3)，加速度(3),角速度偏置(3),加速度偏置(3),位置(3)，与论文公式不一致\n\tvect3 omega;\n\tin.gyro.boxminus(omega, s.bg); // 得到imu的角速度\n\t// 加速度转到世界坐标系\n\tvect3 a_inertial = s.rot * (in.acc - s.ba);\n\tfor (int i = 0; i < 3; i++)\n\t{\n\t\tres(i) = s.vel[i];\t\t\t\t\t\t //更新的速度\n\t\tres(i + 3) = omega[i];\t\t\t\t\t //更新的角速度\n\t\tres(i + 12) = a_inertial[i] + s.grav[i]; //更新的加速度\n\t}\n\treturn res;\n}\n// 对应fast_lio2论文公式(7)\nEigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in)\n{\n\t//当中的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不同需要注意\n\tEigen::Matrix cov = Eigen::Matrix::Zero();\n\tcov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); //一开始是一个R3的单位阵，代表速度转移\n\tvect3 acc_;\n\tin.acc.boxminus(acc_, s.ba); //拿到加速度\n\tvect3 omega;\n\tin.gyro.boxminus(omega, s.bg);\t\t\t\t\t\t\t\t\t\t\t\t //拿到角速度\n\tcov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix() * MTK::hat(acc_); //这里的-s.rot.toRotationMatrix()是因为论文中的矩阵是逆时针旋转的\n\tcov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix();\t\t\t\t // 将角度转到存入的矩阵中（应该与上面颠倒了）\n\tEigen::Matrix vec = Eigen::Matrix::Zero();\n\tEigen::Matrix grav_matrix;\n\ts.S2_Mx(grav_matrix, vec, 21);\t\t\t\t\t\t\t\t\t//将vec的2*1矩阵转为grav_matrix的3*2矩阵\n\tcov.template block<3, 2>(12, 21) = grav_matrix;\t\t\t\t\t//存入到矩阵中\n\tcov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); //角速度存入\n\treturn cov;\n}\n// 对应fast_lio2论文公式(7)\nEigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in)\n{\n\tEigen::Matrix cov = Eigen::Matrix::Zero();\n\tcov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); //加速度\n\tcov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); //角速度\n\tcov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); //角速度偏置\n\tcov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); //加速度偏置\n\treturn cov;\n}\n\nvect3 SO3ToEuler(const SO3 &orient) //将SO3转为Euler角\n{\n\tEigen::Matrix _ang;\n\tEigen::Vector4d q_data = orient.coeffs().transpose(); //将SO3转为4*1的矩阵\n\t// scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2];\n\tdouble sqw = q_data[3] * q_data[3];\n\tdouble sqx = q_data[0] * q_data[0];\n\tdouble sqy = q_data[1] * q_data[1];\n\tdouble sqz = q_data[2] * q_data[2];\n\tdouble unit = sqx + sqy + sqz + sqw; //如果归一为1，否则为校正因子\n\tdouble test = q_data[3] * q_data[1] - q_data[2] * q_data[0];\n\n\tif (test > 0.49999 * unit)\n\t{ // 在北极点\n\n\t\t_ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI / 2, 0;\n\t\tdouble temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};\n\t\tvect3 euler_ang(temp, 3);\n\t\treturn euler_ang;\n\t}\n\tif (test < -0.49999 * unit)\n\t{ // 在南极点\n\t\t_ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI / 2, 0;\n\t\tdouble temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};\n\t\tvect3 euler_ang(temp, 3);\n\t\treturn euler_ang;\n\t}\n\t// 否则正常计算\n\t_ang << std::atan2(2 * q_data[0] * q_data[3] + 2 * q_data[1] * q_data[2], -sqx - sqy + sqz + sqw),\n\t\tstd::asin(2 * test / unit),\n\t\tstd::atan2(2 * q_data[2] * q_data[3] + 2 * q_data[1] * q_data[0], sqx - sqy - sqz + sqw); //转为弧度\n\tdouble temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};\t\t\t\t\t\t\t //转为角度\n\tvect3 euler_ang(temp, 3);\n\t// euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw\n\treturn euler_ang;\n}\n\n#endif" }, { "path": "FAST-LIO/launch/gdb_debug_example.launch", "content": "\n\n \n\n \n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n \n\n \n\n\n" }, { "path": "FAST-LIO/launch/mapping_avia.launch", "content": "\n\n\n\t\n \n\n\t\n\n\t\n\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n \n\n\t\n\t\n\t\n\n\n" }, { "path": "FAST-LIO/launch/mapping_horizon.launch", "content": "\n\n\n\t\n\n\t\n\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n\t\n \n\n\t\n\t\n\t\n\n" }, { "path": "FAST-LIO/launch/mapping_ouster64.launch", "content": "\n\n\n \n\n \n\n \n \n \n \n\t\n \n \n \n \n \n \n\n \n \n \n\n\n" }, { "path": "FAST-LIO/launch/mapping_ouster64_mulran.launch", "content": "\n\n\n \n\n \n\n \n \n\n \n \n \n \n \n \n \n \n \n\n \n \n \n\n\n" }, { "path": "FAST-LIO/launch/mapping_velodyne.launch", "content": "\n \n\n \n\n \n\n \n \n \n \n\t \n \n \n \n \n \n \n\n \n \n \n\n" }, { "path": "FAST-LIO/msg/Pose6D.msg", "content": "# the preintegrated Lidar states at the time of IMU measurements in a frame\nfloat64 offset_time # the offset time of IMU measurement w.r.t the first lidar point\nfloat64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin\nfloat64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin\nfloat64[3] vel # the preintegrated velocity (global frame) at the Lidar origin\nfloat64[3] pos # the preintegrated position (global frame) at the Lidar origin\nfloat64[9] rot # the preintegrated rotation (global frame) at the Lidar origin" }, { "path": "FAST-LIO/package.xml", "content": "\n\n fast_lio\n 0.0.0\n\n \n This is a modified version of LOAM which is original algorithm\n is described in the following paper:\n J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n \n\n claydergc\n\n BSD\n\n Ji Zhang\n \n catkin\n geometry_msgs\n nav_msgs\n roscpp\n rospy\n std_msgs\n sensor_msgs\n tf\n pcl_ros\n livox_ros_driver\n message_generation\n\n geometry_msgs\n nav_msgs\n sensor_msgs\n roscpp\n rospy\n std_msgs\n tf\n pcl_ros\n livox_ros_driver\n message_runtime\n\n rostest\n rosbag\n\n \n \n\n" }, { "path": "FAST-LIO/rviz_cfg/.gitignore", "content": "" }, { "path": "FAST-LIO/rviz_cfg/loam_livox.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /mapping1\n - /mapping1/surround1\n - /mapping1/currPoints1\n - /mapping1/currPoints1/Autocompute Value Bounds1\n - /Odometry1/Odometry1\n - /Odometry1/Odometry1/Shape1\n - /Odometry1/Odometry1/Covariance1\n - /Odometry1/Odometry1/Covariance1/Position1\n - /Odometry1/Odometry1/Covariance1/Orientation1\n - /MarkerArray1/Namespaces1\n Splitter Ratio: 0.6432291865348816\n Tree Height: 441\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: surround\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 1\n Cell Size: 1000\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 40\n Reference Frame: \n Value: false\n - Class: rviz/Axes\n Enabled: false\n Length: 0.699999988079071\n Name: Axes\n Radius: 0.05999999865889549\n Reference Frame: \n Value: false\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 238; 238; 236\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: surround\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: false\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Points\n Topic: /cloud_registered\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 0.15000000596046448\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 15\n Min Value: -5\n Value: false\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 1000\n Enabled: true\n Invert Rainbow: true\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: currPoints\n Position Transformer: XYZ\n Queue Size: 100000\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /cloud_registered\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 0; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /Laser_map\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n Enabled: true\n Name: mapping\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.009999999776482582\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.0010000000474974513\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.20000000298023224\n Color: 255; 85; 0\n Head Length: 0\n Head Radius: 0\n Shaft Length: 0.05000000074505806\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /Odometry\n Unreliable: false\n Value: true\n Enabled: true\n Name: Odometry\n - Class: rviz/Axes\n Enabled: true\n Length: 0.699999988079071\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: \n Value: true\n - Alpha: 0\n Buffer Length: 2\n Class: rviz/Path\n Color: 25; 255; 255\n Enabled: true\n Head Diameter: 0\n Head Length: 0\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.20000000298023224\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 25; 255; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.4000000059604645\n Shaft Length: 0.4000000059604645\n Topic: /path\n Unreliable: false\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: false\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 239; 41; 41\n Max Intensity: 0\n Min Color: 239; 41; 41\n Min Intensity: 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 4\n Size (m): 0.30000001192092896\n Style: Spheres\n Topic: /cloud_effected\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 13.139549255371094\n Min Value: -32.08251953125\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 138; 226; 52\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 138; 226; 52\n Min Color: 138; 226; 52\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /Laser_map\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Class: rviz/MarkerArray\n Enabled: false\n Marker Topic: /MarkerArray\n Name: MarkerArray\n Namespaces:\n {}\n Queue Size: 100\n Value: false\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: camera_init\n Frame Rate: 10\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 24.759632110595703\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 0\n Y: 0\n Z: 0\n Focal Shape Fixed Size: false\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.8303981423377991\n Target Frame: body\n Value: Orbit (rviz)\n Yaw: 1.6953977346420288\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: true\n Height: 1025\n Hide Left Dock: true\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd0000000400000000000001c80000034ffc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000034f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c7000000000000000000000001000001520000034ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000052fc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000073d0000034f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1853\n X: 67\n Y: 27\n" }, { "path": "FAST-LIO/src/IMU_Processing.hpp", "content": "#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \"use-ikfom.hpp\"\n\n/// *************Preconfiguration\n\n#define MAX_INI_COUNT (20)\n\n//判断点的时间是否先后颠倒\nconst bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); };\n\n/// *************IMU Process and undistortion\nclass ImuProcess\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n ImuProcess();\n ~ImuProcess();\n\n void Reset();\n void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);\n void set_extrinsic(const V3D &transl, const M3D &rot);\n void set_extrinsic(const V3D &transl);\n void set_extrinsic(const MD(4, 4) & T);\n void set_gyr_cov(const V3D &scaler);\n void set_acc_cov(const V3D &scaler);\n void set_gyr_bias_cov(const V3D &b_g);\n void set_acc_bias_cov(const V3D &b_a);\n Eigen::Matrix Q;\n void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_);\n\n ofstream fout_imu; // imu参数输出文件\n V3D cov_acc; //加速度测量协方差\n V3D cov_gyr; //角速度测量协方差\n V3D cov_acc_scale; //加速度测量协方差\n V3D cov_gyr_scale; //角速度测量协方差\n V3D cov_bias_gyr; //角速度测量协方差偏置\n V3D cov_bias_acc; //加速度测量协方差偏置\n double first_lidar_time; //当前帧第一个点云时间\n\nprivate:\n void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N);\n void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out);\n\n PointCloudXYZI::Ptr cur_pcl_un_; //当前帧点云未去畸变\n sensor_msgs::ImuConstPtr last_imu_; // 上一帧imu最后的一个值\n deque v_imu_; // imu队列\n vector IMUpose; // imu位姿\n vector v_rot_pcl_; //未使用\n M3D Lidar_R_wrt_IMU; // lidar到IMU的旋转外参\n V3D Lidar_T_wrt_IMU; // lidar到IMU的位置外参\n V3D mean_acc; //加速度均值,用于计算方差\n V3D mean_gyr; //角速度均值，用于计算方差\n V3D angvel_last; //上一帧角速度\n V3D acc_s_last; //上一帧加速度\n double start_timestamp_; //开始时间戳\n double last_lidar_end_time_; //上一帧结束时间戳\n int init_iter_num = 1; //初始化迭代次数\n bool b_first_frame_ = true; //是否是第一帧\n bool imu_need_init_ = true; //是否需要初始化imu\n};\n\nImuProcess::ImuProcess()\n : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1)\n{\n init_iter_num = 1; //初始化迭代次数\n Q = process_noise_cov(); //调用use-ikfom.hpp里面的process_noise_cov完成噪声协方差的初始化\n cov_acc = V3D(0.1, 0.1, 0.1); //加速度测量协方差初始化\n cov_gyr = V3D(0.1, 0.1, 0.1); //角速度测量协方差初始化\n cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); //角速度测量协方差偏置初始化\n cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); //加速度测量协方差偏置初始化\n mean_acc = V3D(0, 0, -1.0);\n mean_gyr = V3D(0, 0, 0);\n angvel_last = Zero3d; //上一帧角速度初始化\n Lidar_T_wrt_IMU = Zero3d; // lidar到IMU的位置外参初始化\n Lidar_R_wrt_IMU = Eye3d; // lidar到IMU的旋转外参初始化\n last_imu_.reset(new sensor_msgs::Imu()); //上一帧imu初始化\n}\n\nImuProcess::~ImuProcess() {}\n\n//重置参数\nvoid ImuProcess::Reset()\n{\n // ROS_WARN(\"Reset ImuProcess\");\n mean_acc = V3D(0, 0, -1.0);\n mean_gyr = V3D(0, 0, 0);\n angvel_last = Zero3d;\n imu_need_init_ = true; //是否需要初始化imu\n start_timestamp_ = -1; //开始时间戳\n init_iter_num = 1; //初始化迭代次数\n v_imu_.clear(); // imu队列清空\n IMUpose.clear(); // imu位姿清空\n last_imu_.reset(new sensor_msgs::Imu()); //上一帧imu初始化\n cur_pcl_un_.reset(new PointCloudXYZI()); //当前帧点云未去畸变初始化\n}\n\n//传入外参，包含R,T\nvoid ImuProcess::set_extrinsic(const MD(4, 4) & T)\n{\n Lidar_T_wrt_IMU = T.block<3, 1>(0, 3);\n Lidar_R_wrt_IMU = T.block<3, 3>(0, 0);\n}\n\n//传入外参，包含T\nvoid ImuProcess::set_extrinsic(const V3D &transl)\n{\n Lidar_T_wrt_IMU = transl;\n Lidar_R_wrt_IMU.setIdentity();\n}\n\n// 传入外参，包含R,T\nvoid ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)\n{\n Lidar_T_wrt_IMU = transl;\n Lidar_R_wrt_IMU = rot;\n}\n\n// 传入陀螺仪角速度协方差\nvoid ImuProcess::set_gyr_cov(const V3D &scaler)\n{\n cov_gyr_scale = scaler;\n}\n\n// 传入加速度计加速度协方差\nvoid ImuProcess::set_acc_cov(const V3D &scaler)\n{\n cov_acc_scale = scaler;\n}\n// 传入陀螺仪角速度协方差偏置\nvoid ImuProcess::set_gyr_bias_cov(const V3D &b_g)\n{\n cov_bias_gyr = b_g;\n}\n// 传入加速度计加速度协方差偏置\nvoid ImuProcess::set_acc_bias_cov(const V3D &b_a)\n{\n cov_bias_acc = b_a;\n}\n\nvoid ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N)\n{\n /** 1. 初始化重力、陀螺偏差、acc和陀螺仪协方差\n /** 2. 将加速度测量值标准化为单位重力**/\n //这里应该是静止初始化\n\n V3D cur_acc, cur_gyr;\n\n if (b_first_frame_) //判断是否为第一帧\n {\n Reset(); //重置参数\n N = 1; //将迭代次数置1\n b_first_frame_ = false;\n const auto &imu_acc = meas.imu.front()->linear_acceleration; //从common_lib.h中拿到imu初始时刻的加速度\n const auto &gyr_acc = meas.imu.front()->angular_velocity; //从common_lib.h中拿到imu初始时刻的角速度\n mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; //加速度测量作为初始化均值\n mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; //角速度测量作为初始化均值\n first_lidar_time = meas.lidar_beg_time; //将当期imu帧对应的lidar时间作为初始时间\n }\n //计算方差\n for (const auto &imu : meas.imu) //拿到所有的imu帧\n {\n const auto &imu_acc = imu->linear_acceleration;\n const auto &gyr_acc = imu->angular_velocity;\n cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;\n cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;\n //根据当前帧和均值差作为均值的更新\n mean_acc += (cur_acc - mean_acc) / N;\n mean_gyr += (cur_gyr - mean_gyr) / N;\n //.cwiseProduct()对应系数相乘\n //每次迭代之后均值都会发生变化，最后的方差公式中减的应该是最后的均值\n // https://blog.csdn.net/weixin_44479136/article/details/90510374 方差迭代计算公式\n //按照博客推导出来的下面方差递推公式有两种\n //第一种是\n cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) / (N - 1.0);\n cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) / (N - 1.0);\n //第二种是\n // cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - 上一次的mean_acc) / N;\n // cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - 上一次的mean_gyr) / N;\n // cout<<\"acc norm: \"<::cov init_P = kf_state.get_P(); //在esekfom.hpp获得P_的协方差矩阵\n init_P.setIdentity(); //将协方差矩阵置为单位阵\n init_P(6, 6) = init_P(7, 7) = init_P(8, 8) = 0.00001; //将协方差矩阵的位置和旋转的协方差置为0.00001\n init_P(9, 9) = init_P(10, 10) = init_P(11, 11) = 0.00001; //将协方差矩阵的速度和位姿的协方差置为0.00001\n init_P(15, 15) = init_P(16, 16) = init_P(17, 17) = 0.0001; //将协方差矩阵的重力和姿态的协方差置为0.0001\n init_P(18, 18) = init_P(19, 19) = init_P(20, 20) = 0.001; //将协方差矩阵的陀螺仪偏差和姿态的协方差置为0.001\n init_P(21, 21) = init_P(22, 22) = 0.00001; //将协方差矩阵的lidar和imu外参位移量的协方差置为0.00001\n kf_state.change_P(init_P); //将初始化协方差矩阵传入esekfom.hpp中的P_\n last_imu_ = meas.imu.back(); //将最后一帧的imu数据传入last_imu_中，在UndistortPcl使用到了\n}\n\n//正向传播 反向传播 去畸变，这里涉及到了Lidar的去畸变问题\nvoid ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out)\n{\n auto v_imu = meas.imu; //拿到当前的imu数据\n v_imu.push_front(last_imu_); //将上一帧最后尾部的imu添加到当前帧头部的imu\n const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); //拿到当前帧头部的imu的时间（也就是上一帧尾部的imu时间戳）\n const double &imu_end_time = v_imu.back()->header.stamp.toSec(); //拿到当前帧尾部的imu的时间\n const double &pcl_beg_time = meas.lidar_beg_time; // pcl开始的时间戳\n\n /*** sort point clouds by offset time ***/\n // 根据点云中每个点的时间戳对点云进行重排序\n pcl_out = *(meas.lidar);\n sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);\n const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); //拿到最后一帧时间戳加上最后一帧的所需要的时间/1000得到点云的结束时间戳\n // cout<<\"[ IMU Process ]: Process lidar from \"<header.stamp.toSec() < last_lidar_end_time_)\n continue;\n\n // 中值积分\n angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x),\n 0.5 * (head->angular_velocity.y + tail->angular_velocity.y),\n 0.5 * (head->angular_velocity.z + tail->angular_velocity.z);\n acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),\n 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),\n 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);\n\n // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << \" \" << angvel_avr.transpose() << \" \" << acc_avr.transpose() << endl;\n //通过重力数值对加速度进行一下微调\n acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;\n\n //如果IMU开始时刻早于上次雷达最晚时刻(因为将上次最后一个IMU插入到此次开头了，所以会出现一次这种情况)\n if (head->header.stamp.toSec() < last_lidar_end_time_)\n {\n //从上次雷达时刻末尾开始传播 计算与此次IMU结尾之间的时间差\n dt = tail->header.stamp.toSec() - last_lidar_end_time_;\n // dt = tail->header.stamp.toSec() - pcl_beg_time;\n }\n else\n {\n //两个IMU时刻之间的时间间隔\n dt = tail->header.stamp.toSec() - head->header.stamp.toSec();\n }\n\n // 原始测量的中值作为更新\n in.acc = acc_avr;\n in.gyro = angvel_avr;\n // 配置协方差矩阵\n Q.block<3, 3>(0, 0).diagonal() = cov_gyr;\n Q.block<3, 3>(3, 3).diagonal() = cov_acc;\n Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;\n Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;\n\n // IMU前向传播，每次传播的时间间隔为dt\n kf_state.predict(dt, Q, in);\n\n /* save the poses at each IMU measurements */\n // 保存IMU预测过程的状态\n imu_state = kf_state.get_x();\n angvel_last = angvel_avr - imu_state.bg; //计算出来的角速度与预测的角速度的差值\n acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); //计算出来的加速度与预测的加速度的差值,并转到IMU坐标系下\n for (int i = 0; i < 3; i++)\n {\n acc_s_last[i] += imu_state.grav[i]; //加上重力得到世界坐标系的加速度\n }\n double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; //后一个IMU时刻距离此次雷达开始的时间间隔\n IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix())); //保存IMU预测过程的状态\n }\n\n // 把最后一帧IMU测量也补上\n // 判断雷达结束时间是否晚于IMU，最后一个IMU时刻可能早于雷达末尾 也可能晚于雷达末尾\n double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;\n dt = note * (pcl_end_time - imu_end_time);\n kf_state.predict(dt, Q, in);\n\n imu_state = kf_state.get_x(); //更新IMU状态，以便于下一帧使用\n last_imu_ = meas.imu.back(); //保存最后一个IMU测量，以便于下一帧使用\n last_lidar_end_time_ = pcl_end_time; //保存这一帧最后一个雷达测量的结束时间，以便于下一帧使用\n\n /*** 在处理完所有的IMU预测后，剩下的就是对激光的去畸变了 ***/\n // 基于IMU预测对lidar点云去畸变\n auto it_pcl = pcl_out.points.end() - 1;\n for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)\n {\n auto head = it_kp - 1;\n auto tail = it_kp;\n R_imu << MAT_FROM_ARRAY(head->rot); //拿到前一帧的IMU旋转矩阵\n // cout<<\"head imu acc: \"<vel); //拿到前一帧的IMU速度\n pos_imu << VEC_FROM_ARRAY(head->pos); //拿到前一帧的IMU位置\n acc_imu << VEC_FROM_ARRAY(tail->acc); //拿到后一帧的IMU加速度\n angvel_avr << VEC_FROM_ARRAY(tail->gyr); //拿到后一帧的IMU角速度\n\n //点云时间需要迟于前一个IMU时刻 因为是在两个IMU时刻之间去畸变，此时默认雷达的时间戳在后一个IMU时刻之前\n for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) //时间除以1000单位化为s\n {\n dt = it_pcl->curvature / double(1000) - head->offset_time; //点到IMU开始时刻的时间间隔\n\n /*变换到“结束”帧，仅使用旋转\n *注意：补偿方向与帧的移动方向相反\n *所以如果我们想补偿时间戳i到帧e的一个点\n * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) 其中T_ei在全局框架中表示*/\n M3D R_i(R_imu * Exp(angvel_avr, dt)); //点所在时刻的旋转\n\n V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); //点所在时刻的位置(雷达坐标系下)\n V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); //从点所在的世界位置-雷达末尾世界位置\n //.conjugate()取旋转矩阵的共轭,rot.conjugate（）是四元数共轭，即旋转求逆\n // imu_state.offset_R_L_I是从雷达到惯性的旋转矩阵 简单记为I^R_L\n // imu_state.offset_T_L_I是惯性系下雷达坐标系原点的位置简单记为I^t_L\n //下面去畸变补偿的公式这里倒推一下\n // e代表end时刻\n // P_compensate是点在末尾时刻在雷达系的坐标 简记为L^P_e\n //将右侧矩阵乘过来并加上右侧平移\n //左边变为I^R_L * L^P_e + I^t_L= I^P_e 也就是end时刻点在IMU系下的坐标\n //右边剩下imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei\n // imu_state.rot.conjugate()是结束时刻IMU到世界坐标系的旋转矩阵的逆函数 也就是(W^R_i_e)^T\n // 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\n //现在等式两边变为 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\n //(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\n // 世界坐标系也无所谓时刻了 因为只有一个世界坐标系 两边变为\n // W^P = R_i * I^P+ W^t_I\n // W^P = W^P\n 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!\n\n // save Undistorted points and their rotation\n it_pcl->x = P_compensate(0);\n it_pcl->y = P_compensate(1);\n it_pcl->z = P_compensate(2);\n\n if (it_pcl == pcl_out.points.begin())\n break;\n }\n }\n}\n\nvoid ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)\n{\n double t1, t2, t3;\n t1 = omp_get_wtime();\n\n if (meas.imu.empty())\n {\n return;\n }; // 拿到的当前帧的imu测量为空，则直接返回\n ROS_ASSERT(meas.lidar != nullptr);\n\n if (imu_need_init_)\n {\n /// 第一个激光雷达帧\n IMU_init(meas, kf_state, init_iter_num);\n\n imu_need_init_ = true;\n\n last_imu_ = meas.imu.back();\n\n state_ikfom imu_state = kf_state.get_x();\n if (init_iter_num > MAX_INI_COUNT)\n {\n cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); //在上面IMU_init()基础上乘上缩放系数\n imu_need_init_ = false;\n\n cov_acc = cov_acc_scale;\n cov_gyr = cov_gyr_scale;\n ROS_INFO(\"IMU Initial Done\");\n // 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\",\\\n // 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]);\n fout_imu.open(DEBUG_FILE_DIR(\"imu.txt\"), ios::out);\n }\n\n return;\n }\n //正向传播 反向传播 去畸变\n UndistortPcl(meas, kf_state, *cur_pcl_un_);\n\n t2 = omp_get_wtime();\n t3 = omp_get_wtime();\n\n // cout<<\"[ IMU Process ]: Time: \"<\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \"IMU_Processing.hpp\"\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \"preprocess.h\"\n#include \n\n#define INIT_TIME (0.1)\n#define LASER_POINT_COV (0.001)\n#define MAXN (720000)\n#define PUBFRAME_PERIOD (20)\n\n/*** Time Log Variables ***/\n// kdtree_incremental_time为kdtree建立时间，kdtree_search_time为kdtree搜索时间，kdtree_delete_time为kdtree删除时间;\ndouble kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;\n// T1为雷达初始时间戳，s_plot为整个流程耗时，s_plot2特征点数量,s_plot3为kdtree增量时间，s_plot4为kdtree搜索耗时，s_plot5为kdtree删除点数量\n//，s_plot6为kdtree删除耗时，s_plot7为kdtree初始大小，s_plot8为kdtree结束大小,s_plot9为平均消耗时间，s_plot10为添加点数量，s_plot11为点云预处理的总时间\ndouble 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];\n\n// 定义全局变量，用于记录时间,match_time为匹配时间，solve_time为求解时间，solve_const_H_time为求解H矩阵时间\ndouble match_time = 0, solve_time = 0, solve_const_H_time = 0;\n// kdtree_size_st为ikd-tree获得的节点数，kdtree_size_end为ikd-tree结束时的节点数，add_point_size为添加点的数量，kdtree_delete_counter为删除点的数量\nint kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0;\n// runtime_pos_log运行时的log是否开启，pcd_save_en是否保存pcd文件，time_sync_en是否同步时间\nbool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false;\n/**************************/\n\nfloat res_last[100000] = {0.0}; //残差，点到面距离平方和\nfloat DET_RANGE = 300.0f; //设置的当前雷达系中心到各个地图边缘的距离\nconst float MOV_THRESHOLD = 1.5f; //设置的当前雷达系中心到各个地图边缘的权重\n\nmutex mtx_buffer; // 互斥锁\ncondition_variable sig_buffer; // 条件变量\n\nstring root_dir = ROOT_DIR; //设置根目录\nstring map_file_path, lid_topic, imu_topic; //设置地图文件路径，雷达topic，imu topic\n\ndouble res_mean_last = 0.05, total_residual = 0.0; //设置残差平均值，残差总和\ndouble last_timestamp_lidar = 0, last_timestamp_imu = -1.0; //设置雷达时间戳，imu时间戳\ndouble gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001; //设置imu的角速度协方差，加速度协方差，角速度协方差偏置，加速度协方差偏置\ndouble filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; //设置滤波器的最小尺寸，地图的最小尺寸，视野角度\n\n//设置立方体长度，视野一半的角度，视野总角度，总距离，雷达结束时间，雷达初始时间\ndouble cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0;\n//设置有效特征点数，时间log计数器, scan_count：接收到的激光雷达Msg的总数，publish_count：接收到的IMU的Msg的总数\nint effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;\n//设置迭代次数，下采样的点数，最大迭代次数，有效点数\nint iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0;\nbool point_selected_surf[100000] = {0}; // 是否为平面特征点\n// lidar_pushed：用于判断激光雷达数据是否从缓存队列中拿到meas中的数据, flg_EKF_inited用于判断EKF是否初始化完成\nbool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited;\n//设置是否发布激光雷达数据，是否发布稠密数据，是否发布激光雷达数据的身体数据\nbool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;\n\nvector> pointSearchInd_surf; //每个点的索引,暂时没用到\nvector cub_needrm; // ikd-tree中，地图需要移除的包围盒序列\nvector Nearest_Points; //每个点的最近点序列\nvector extrinT(3, 0.0); //雷达相对于IMU的外参T\nvector extrinR(9, 0.0); //雷达相对于IMU的外参R\ndeque time_buffer; // 激光雷达数据时间戳缓存队列\ndeque lidar_buffer; //记录特征提取或间隔采样后的lidar（特征）数据\ndeque imu_buffer; // IMU数据缓存队列\n\n//一些点云变量\nPointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); //提取地图中的特征点，IKD-tree获得\nPointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); //去畸变的特征\nPointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，lidar系\nPointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，w系\nPointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); //特征点在地图中对应点的，局部平面参数,w系\nPointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); // laserCloudOri是畸变纠正后降采样的单帧点云，body系\nPointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); //对应点法相量\nPointCloudXYZI::Ptr _featsArray; // ikd-tree中，map需要移除的点云序列\n\n//下采样的体素点云\npcl::VoxelGrid downSizeFilterSurf; //单帧内降采样使用voxel grid\npcl::VoxelGrid downSizeFilterMap; //未使用\n\nKD_TREE ikdtree; // ikd-tree类\n\nV3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); //雷达相对于body系的X轴方向的点\nV3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); //雷达相对于world系的X轴方向的点\nV3D euler_cur; //当前的欧拉角\nV3D position_last(Zero3d); //上一帧的位置\nV3D Lidar_T_wrt_IMU(Zero3d); // T lidar to imu (imu = r * lidar + t)\nM3D Lidar_R_wrt_IMU(Eye3d); // R lidar to imu (imu = r * lidar + t)\n\n/*** EKF inputs and output ***/\n// ESEKF操作\nMeasureGroup Measures;\nesekfom::esekf kf; // 状态，噪声维度，输入\nstate_ikfom state_point; // 状态\nvect3 pos_lid; // world系下lidar坐标\n\n//输出的路径参数\nnav_msgs::Path path; //包含了一系列位姿\nnav_msgs::Odometry odomAftMapped; //只包含了一个位姿\ngeometry_msgs::Quaternion geoQuat; //四元数\ngeometry_msgs::PoseStamped msg_body_pose; //位姿\n\n//激光和imu处理操作\nshared_ptr p_pre(new Preprocess()); // 定义指向激光雷达数据的预处理类Preprocess的智能指针\nshared_ptr p_imu(new ImuProcess()); // 定义指向IMU数据预处理类ImuProcess的智能指针\n\n//按下ctrl+c后唤醒所有线程\nvoid SigHandle(int sig)\n{\n flg_exit = true;\n ROS_WARN(\"catch sig %d\", sig);\n sig_buffer.notify_all(); // 会唤醒所有等待队列中阻塞的线程 线程被唤醒后，会通过轮询方式获得锁，获得锁前也一直处理运行状态，不会被再次阻塞。\n}\n\n//将fast lio2信息打印到log中\ninline void dump_lio_state_to_log(FILE *fp)\n{\n V3D rot_ang(Log(state_point.rot.toRotationMatrix()));\n fprintf(fp, \"%lf \", Measures.lidar_beg_time - first_lidar_time);\n fprintf(fp, \"%lf %lf %lf \", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle\n fprintf(fp, \"%lf %lf %lf \", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos\n fprintf(fp, \"%lf %lf %lf \", 0.0, 0.0, 0.0); // omega\n fprintf(fp, \"%lf %lf %lf \", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel\n fprintf(fp, \"%lf %lf %lf \", 0.0, 0.0, 0.0); // Acc\n fprintf(fp, \"%lf %lf %lf \", state_point.bg(0), state_point.bg(1), state_point.bg(2)); // Bias_g\n fprintf(fp, \"%lf %lf %lf \", state_point.ba(0), state_point.ba(1), state_point.ba(2)); // Bias_a\n fprintf(fp, \"%lf %lf %lf \", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a\n fprintf(fp, \"\\r\\n\");\n fflush(fp);\n}\n\n//把点从body系转到world系，通过ikfom的位置和姿态\nvoid pointBodyToWorld_ikfom(PointType const *const pi, PointType *const po, state_ikfom &s)\n{\n V3D p_body(pi->x, pi->y, pi->z);\n //下面式子最里面的括号是从雷达到IMU坐标系 然后从IMU转到世界坐标系\n V3D p_global(s.rot * (s.offset_R_L_I * p_body + s.offset_T_L_I) + s.pos);\n\n po->x = p_global(0);\n po->y = p_global(1);\n po->z = p_global(2);\n po->intensity = pi->intensity;\n}\n//把点从body系转到world系\nvoid pointBodyToWorld(PointType const *const pi, PointType *const po)\n{\n V3D p_body(pi->x, pi->y, pi->z);\n V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);\n\n po->x = p_global(0);\n po->y = p_global(1);\n po->z = p_global(2);\n po->intensity = pi->intensity;\n}\n//把点从body系转到world系\ntemplate \nvoid pointBodyToWorld(const Matrix &pi, Matrix &po)\n{\n V3D p_body(pi[0], pi[1], pi[2]);\n V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);\n\n po[0] = p_global(0);\n po[1] = p_global(1);\n po[2] = p_global(2);\n}\n// 含有RGB的点云从body系转到world系\nvoid RGBpointBodyToWorld(PointType const *const pi, PointType *const po)\n{\n V3D p_body(pi->x, pi->y, pi->z);\n V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);\n\n po->x = p_global(0);\n po->y = p_global(1);\n po->z = p_global(2);\n po->intensity = pi->intensity;\n}\n// 含有RGB的点云从Lidar系转到IMU系\nvoid RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)\n{\n V3D p_body_lidar(pi->x, pi->y, pi->z);\n V3D p_body_imu(state_point.offset_R_L_I * p_body_lidar + state_point.offset_T_L_I);\n\n po->x = p_body_imu(0);\n po->y = p_body_imu(1);\n po->z = p_body_imu(2);\n po->intensity = pi->intensity;\n}\n\n//得到被剔除的点\nvoid points_cache_collect()\n{\n PointVector points_history;\n ikdtree.acquire_removed_points(points_history); //返回被剔除的点\n for (int i = 0; i < points_history.size(); i++)\n _featsArray->push_back(points_history[i]); //存入到缓存中，后面没有用到该数据\n}\n\n// 在拿到eskf前馈结果后，动态调整地图区域，防止地图过大而内存溢出，类似LOAM中提取局部地图的方法\nBoxPointType LocalMap_Points; // ikd-tree中,局部地图的包围盒角点\nbool Localmap_Initialized = false; // 局部地图是否初始化\nvoid lasermap_fov_segment()\n{\n cub_needrm.clear(); // 清空需要移除的区域\n kdtree_delete_counter = 0;\n kdtree_delete_time = 0.0;\n // X轴分界点转换到w系下，好像没有用到\n pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);\n // global系下lidar位置\n V3D pos_LiD = pos_lid;\n //初始化局部地图包围盒角点，以为w系下lidar位置为中心,得到长宽高200*200*200的局部地图\n if (!Localmap_Initialized)\n { // 系统起始需要初始化局部地图的大小和位置\n for (int i = 0; i < 3; i++)\n {\n LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;\n LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;\n }\n Localmap_Initialized = true;\n return;\n }\n // 各个方向上Lidar与局部地图边界的距离，或者说是lidar与立方体盒子六个面的距离\n float dist_to_map_edge[3][2];\n bool need_move = false;\n // 当前雷达系中心到各个地图边缘的距离\n for (int i = 0; i < 3; i++)\n {\n dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);\n dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);\n // 与某个方向上的边界距离（例如1.5*300m）太小，标记需要移除need_move，参考论文Fig3\n if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)\n need_move = true;\n }\n // 不需要挪动就直接退回了\n if (!need_move)\n return;\n // 否则需要计算移动的距离\n BoxPointType New_LocalMap_Points, tmp_boxpoints;\n // 新的局部地图盒子边界点\n New_LocalMap_Points = LocalMap_Points;\n float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1)));\n for (int i = 0; i < 3; i++)\n {\n tmp_boxpoints = LocalMap_Points;\n //与包围盒最小值边界点距离\n if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE)\n {\n New_LocalMap_Points.vertex_max[i] -= mov_dist;\n New_LocalMap_Points.vertex_min[i] -= mov_dist;\n tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;\n cub_needrm.push_back(tmp_boxpoints); // 移除较远包围盒\n }\n else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)\n {\n New_LocalMap_Points.vertex_max[i] += mov_dist;\n New_LocalMap_Points.vertex_min[i] += mov_dist;\n tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;\n cub_needrm.push_back(tmp_boxpoints);\n }\n }\n LocalMap_Points = New_LocalMap_Points;\n\n points_cache_collect();\n double delete_begin = omp_get_wtime();\n // 使用Boxs删除指定盒内的点\n if (cub_needrm.size() > 0)\n kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);\n kdtree_delete_time = omp_get_wtime() - delete_begin;\n}\n// 除了AVIA类型之外的雷达点云回调函数，将数据引入到buffer当中\nvoid standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)\n{\n mtx_buffer.lock(); //加锁\n scan_count++;\n double preprocess_start_time = omp_get_wtime(); //记录时间\n if (msg->header.stamp.toSec() < last_timestamp_lidar)\n {\n ROS_ERROR(\"lidar loop back, clear buffer\");\n lidar_buffer.clear();\n }\n\n PointCloudXYZI::Ptr ptr(new PointCloudXYZI());\n p_pre->process(msg, ptr); //点云预处理\n lidar_buffer.push_back(ptr); //将点云放入缓冲区\n time_buffer.push_back(msg->header.stamp.toSec()); //将时间放入缓冲区\n last_timestamp_lidar = msg->header.stamp.toSec(); //记录最后一个时间\n s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; //预处理时间\n mtx_buffer.unlock();\n sig_buffer.notify_all(); // 唤醒所有线程\n}\n\ndouble timediff_lidar_wrt_imu = 0.0; //雷达时间与imu时间差\nbool timediff_set_flg = false; // 时间同步flag，false表示未进行时间同步，true表示已经进行过时间同步\n\n// 订阅器sub_pcl的回调函数：接收Livox激光雷达的点云数据，对点云数据进行预处理（特征提取、降采样、滤波），并将处理后的数据保存到激光雷达数据队列中\nvoid livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)\n{\n // 互斥锁\n mtx_buffer.lock();\n // 点云预处理的开始时间\n double preprocess_start_time = omp_get_wtime();\n // 激光雷达扫描的总次数\n scan_count++;\n // 如果当前扫描的激光雷达数据的时间戳比上一次扫描的激光雷达数据的时间戳早，需要将激光雷达数据缓存队列清空\n if (msg->header.stamp.toSec() < last_timestamp_lidar)\n {\n ROS_ERROR(\"lidar loop back, clear buffer\");\n lidar_buffer.clear();\n }\n last_timestamp_lidar = msg->header.stamp.toSec();\n // 如果不需要进行时间同步，而imu时间戳和雷达时间戳相差大于10s，则输出错误信息\n if (!time_sync_en && abs(last_timestamp_imu - lidar_end_time) > 10.0)\n {\n printf(\"IMU and LiDAR not Synced, IMU time: %lf, lidar scan end time: %lf\", last_timestamp_imu, lidar_end_time);\n }\n // time_sync_en为true时，当imu时间戳和雷达时间戳相差大于1s时，进行时间同步\n if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty())\n {\n timediff_set_flg = true; // 标记已经进行时间同步\n timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu;\n printf(\"Self sync IMU and LiDAR, time diff is %.10lf \\n\", timediff_lidar_wrt_imu);\n }\n // 用pcl点云格式保存接收到的激光雷达数据\n PointCloudXYZI::Ptr ptr(new PointCloudXYZI());\n // 对激光雷达数据进行预处理（特征提取或者降采样），其中p_pre是Preprocess类的智能指针\n p_pre->process(msg, ptr);\n lidar_buffer.push_back(ptr);\n time_buffer.push_back(last_timestamp_lidar);\n // 点云预处理的总时间\n s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;\n mtx_buffer.unlock();\n sig_buffer.notify_all(); // 唤醒所有线程\n}\n// 订阅器sub_imu的回调函数：接收IMU数据，将IMU数据保存到IMU数据缓存队列中\nvoid imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)\n{\n publish_count++;\n // cout<<\"IMU got at: \"<header.stamp.toSec()< 0.1 && time_sync_en) //时间同步校准\n {\n msg->header.stamp =\n ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec()); //将IMU时间戳对齐到激光雷达时间戳\n }\n\n double timestamp = msg->header.stamp.toSec(); // IMU时间戳\n\n mtx_buffer.lock();\n // 如果当前IMU的时间戳小于上一个时刻IMU的时间戳，则IMU数据有误，将IMU数据缓存队列清空\n if (timestamp < last_timestamp_imu)\n {\n ROS_WARN(\"imu loop back, clear buffer\");\n imu_buffer.clear();\n }\n // 将当前的IMU数据保存到IMU数据缓存队列中\n last_timestamp_imu = timestamp;\n\n imu_buffer.push_back(msg);\n mtx_buffer.unlock(); //解锁\n sig_buffer.notify_all(); // 唤醒阻塞的线程，当持有锁的线程释放锁时，这些线程中的一个会获得锁。而其余的会接着尝试获得锁\n}\n//处理buffer中的数据，将两帧激光雷达点云数据时间内的IMU数据从缓存队列中取出，进行时间对齐，并保存到meas中\nbool sync_packages(MeasureGroup &meas)\n{\n if (lidar_buffer.empty() || imu_buffer.empty()) //如果缓存队列中没有数据，则返回false\n {\n return false;\n }\n /*** push a lidar scan ***/\n //如果还没有把雷达数据放到meas中的话，就执行一下操作\n if (!lidar_pushed)\n {\n // 从激光雷达点云缓存队列中取出点云数据，放到meas中\n meas.lidar = lidar_buffer.front();\n // 如果该lidar没有点云，则返回false\n if (meas.lidar->points.size() <= 1)\n {\n lidar_buffer.pop_front();\n return false;\n }\n // 该lidar测量起始的时间戳\n meas.lidar_beg_time = time_buffer.front();\n lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); //此次雷达点云结束时刻\n // 成功提取到lidar测量的标志\n lidar_pushed = true;\n }\n\n // 最新的IMU时间戳(也就是队尾的)不能早于雷达的end时间戳，因为last_timestamp_imu比较时是加了0.1的，所以要比较大于雷达的end时间戳\n if (last_timestamp_imu < lidar_end_time)\n {\n return false;\n }\n\n // 压入imu数据，并从imu缓冲区弹出\n double imu_time = imu_buffer.front()->header.stamp.toSec();\n meas.imu.clear();\n // 拿出lidar_beg_time到lidar_end_time之间的所有IMU数据\n while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) //如果imu缓存队列中的数据时间戳小于雷达结束时间戳，则将该数据放到meas中,代表了这一帧中的imu数据\n {\n imu_time = imu_buffer.front()->header.stamp.toSec(); //获取imu数据的时间戳\n if (imu_time > lidar_end_time)\n break;\n meas.imu.push_back(imu_buffer.front()); //将imu数据放到meas中\n imu_buffer.pop_front();\n }\n\n lidar_buffer.pop_front(); //将lidar数据弹出\n time_buffer.pop_front(); //将时间戳弹出\n lidar_pushed = false; //将lidar_pushed置为false，代表lidar数据已经被放到meas中了\n return true;\n}\n\nint process_increments = 0;\nvoid map_incremental() //地图的增量更新，主要完成对ikd-tree的地图建立\n{\n PointVector PointToAdd; //需要加入到ikd-tree中的点云\n PointVector PointNoNeedDownsample; //加入ikd-tree时，不需要降采样的点云\n PointToAdd.reserve(feats_down_size); //构建的地图点\n PointNoNeedDownsample.reserve(feats_down_size); //构建的地图点，不需要降采样的点云\n //根据点与所在包围盒中心点的距离，分类是否需要降采样\n for (int i = 0; i < feats_down_size; i++)\n {\n // 转换到世界坐标系下\n pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));\n // 判断是否有关键点需要加到地图中\n if (!Nearest_Points[i].empty() && flg_EKF_inited)\n {\n const PointVector &points_near = Nearest_Points[i]; //获取附近的点云\n bool need_add = true; //是否需要加入到地图中\n BoxPointType Box_of_Point; //点云所在的包围盒\n PointType downsample_result, mid_point; //降采样结果，中点\n // filter_size_map_min是地图体素降采样的栅格边长，设为0.1m\n // mid_point即为该特征点所属的栅格的中心点坐标\n mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min;\n mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min;\n mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min;\n // 当前点与box中心的距离\n float dist = calc_dist(feats_down_world->points[i], mid_point);\n //判断最近点在x、y、z三个方向上，与中心的距离，判断是否加入时需要降采样\n 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)\n {\n //若三个方向距离都大于地图栅格半轴长，无需降采样\n PointNoNeedDownsample.push_back(feats_down_world->points[i]);\n continue;\n }\n //判断当前点的 NUM_MATCH_POINTS 个邻近点与包围盒中心的范围\n for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i++)\n {\n if (points_near.size() < NUM_MATCH_POINTS) //若邻近点数小于NUM_MATCH_POINTS，则直接跳出，添加到PointToAdd中\n break;\n // 如果存在邻近点到中心的距离小于当前点到中心的距离，则不需要添加当前点\n if (calc_dist(points_near[readd_i], mid_point) < dist)\n {\n need_add = false;\n break;\n }\n }\n if (need_add)\n PointToAdd.push_back(feats_down_world->points[i]); //加入到PointToAdd中\n }\n else\n {\n PointToAdd.push_back(feats_down_world->points[i]); //如果周围没有点或者没有初始化EKF，则加入到PointToAdd中\n }\n }\n\n double st_time = omp_get_wtime(); //记录起始时间\n add_point_size = ikdtree.Add_Points(PointToAdd, true); //加入点时需要降采样\n ikdtree.Add_Points(PointNoNeedDownsample, false); //加入点时不需要降采样\n add_point_size = PointToAdd.size() + PointNoNeedDownsample.size(); //计算总共加入ikd-tree的点的数量\n kdtree_incremental_time = omp_get_wtime() - st_time; // kdtree建立时间更新\n}\n\nPointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); //创建一个点云用于存储等待发布的点云\nPointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); //创建一个点云用于存储等待保存的点云\nvoid publish_frame_world(const ros::Publisher &pubLaserCloudFull)\n{\n if (scan_pub_en) // 设置是否发布激光雷达数据，是否发布稠密数据，是否发布激光雷达数据的身体数据\n {\n PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); //判断是否需要降采样\n int size = laserCloudFullRes->points.size(); //获取待转换点云的大小\n PointCloudXYZI::Ptr laserCloudWorld(\n new PointCloudXYZI(size, 1)); //创建一个点云用于存储转换到世界坐标系的点云\n\n for (int i = 0; i < size; i++)\n {\n RGBpointBodyToWorld(&laserCloudFullRes->points[i],\n &laserCloudWorld->points[i]);\n }\n\n sensor_msgs::PointCloud2 laserCloudmsg;\n pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);\n laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);\n laserCloudmsg.header.frame_id = \"camera_init\";\n pubLaserCloudFull.publish(laserCloudmsg);\n publish_count -= PUBFRAME_PERIOD;\n }\n //把结果压入到pcd中\n /**************** save map ****************/\n /* 1. make sure you have enough memories\n /* 2. pcd save will largely influence the real-time performences **/\n if (pcd_save_en)\n {\n int size = feats_undistort->points.size();\n PointCloudXYZI::Ptr laserCloudWorld(\n new PointCloudXYZI(size, 1));\n\n for (int i = 0; i < size; i++)\n {\n RGBpointBodyToWorld(&feats_undistort->points[i],\n &laserCloudWorld->points[i]); //转换到世界坐标系\n }\n *pcl_wait_save += *laserCloudWorld; //把结果压入到pcd中\n }\n}\n//把去畸变的雷达系下的点云转到IMU系\nvoid publish_frame_body(const ros::Publisher &pubLaserCloudFull_body)\n{\n int size = feats_undistort->points.size();\n PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); //创建一个点云用于存储转换到IMU系的点云\n\n for (int i = 0; i < size; i++)\n {\n RGBpointBodyLidarToIMU(&feats_undistort->points[i],\n &laserCloudIMUBody->points[i]); //转换到IMU坐标系\n }\n\n sensor_msgs::PointCloud2 laserCloudmsg;\n pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);\n laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);\n laserCloudmsg.header.frame_id = \"body\";\n pubLaserCloudFull_body.publish(laserCloudmsg);\n publish_count -= PUBFRAME_PERIOD;\n}\n//发布雷达信息\nvoid publish_frame_lidar(const ros::Publisher &pubLaserCloudFull_lidar)\n{\n sensor_msgs::PointCloud2 laserCloudmsg;\n pcl::toROSMsg(*feats_undistort, laserCloudmsg); //转换到ROS消息格式,并直接发布信息\n laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);\n laserCloudmsg.header.frame_id = \"lidar\";\n pubLaserCloudFull_lidar.publish(laserCloudmsg);\n publish_count -= PUBFRAME_PERIOD;\n}\n//把起作用的特征点转到地图中\nvoid publish_effect_world(const ros::Publisher &pubLaserCloudEffect)\n{\n PointCloudXYZI::Ptr laserCloudWorld(\n new PointCloudXYZI(effct_feat_num, 1)); //创建一个点云用于存储转换到世界坐标系的点云,从h_share_model获得\n for (int i = 0; i < effct_feat_num; i++)\n {\n RGBpointBodyToWorld(&laserCloudOri->points[i],\n &laserCloudWorld->points[i]);\n }\n sensor_msgs::PointCloud2 laserCloudFullRes3;\n pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);\n laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time);\n laserCloudFullRes3.header.frame_id = \"camera_init\";\n pubLaserCloudEffect.publish(laserCloudFullRes3);\n}\n// 发布ikd-tree地图\nvoid publish_map(const ros::Publisher &pubLaserCloudMap)\n{\n sensor_msgs::PointCloud2 laserCloudMap;\n pcl::toROSMsg(*featsFromMap, laserCloudMap);\n laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time);\n laserCloudMap.header.frame_id = \"camera_init\";\n pubLaserCloudMap.publish(laserCloudMap);\n}\n// 设置输出的t,q，在publish_odometry，publish_path调用\ntemplate \nvoid set_posestamp(T &out)\n{\n out.pose.position.x = state_point.pos(0); //将eskf求得的位置传入\n out.pose.position.y = state_point.pos(1);\n out.pose.position.z = state_point.pos(2);\n out.pose.orientation.x = geoQuat.x; //将eskf求得的姿态传入\n out.pose.orientation.y = geoQuat.y;\n out.pose.orientation.z = geoQuat.z;\n out.pose.orientation.w = geoQuat.w;\n}\n//发布里程计\nvoid publish_odometry(const ros::Publisher &pubOdomAftMapped)\n{\n odomAftMapped.header.frame_id = \"camera_init\";\n odomAftMapped.child_frame_id = \"body\";\n odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); // ros::Time().fromSec(lidar_end_time);\n set_posestamp(odomAftMapped.pose);\n pubOdomAftMapped.publish(odomAftMapped);\n auto P = kf.get_P();\n for (int i = 0; i < 6; i++)\n {\n int k = i < 3 ? i + 3 : i - 3;\n //设置协方差 P里面先是旋转后是位置 这个POSE里面先是位置后是旋转 所以对应的协方差要对调一下\n odomAftMapped.pose.covariance[i * 6 + 0] = P(k, 3);\n odomAftMapped.pose.covariance[i * 6 + 1] = P(k, 4);\n odomAftMapped.pose.covariance[i * 6 + 2] = P(k, 5);\n odomAftMapped.pose.covariance[i * 6 + 3] = P(k, 0);\n odomAftMapped.pose.covariance[i * 6 + 4] = P(k, 1);\n odomAftMapped.pose.covariance[i * 6 + 5] = P(k, 2);\n }\n\n static tf::TransformBroadcaster br;\n tf::Transform transform;\n tf::Quaternion q;\n transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x,\n odomAftMapped.pose.pose.position.y,\n odomAftMapped.pose.pose.position.z));\n q.setW(odomAftMapped.pose.pose.orientation.w);\n q.setX(odomAftMapped.pose.pose.orientation.x);\n q.setY(odomAftMapped.pose.pose.orientation.y);\n q.setZ(odomAftMapped.pose.pose.orientation.z);\n transform.setRotation(q);\n br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, \"camera_init\", \"body\")); //发布tf变换\n}\n//每隔10个发布一下位姿\nvoid publish_path(const ros::Publisher pubPath)\n{\n set_posestamp(msg_body_pose);\n msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);\n msg_body_pose.header.frame_id = \"camera_init\";\n\n /*** if path is too large, the rvis will crash ***/\n static int jjj = 0;\n jjj++;\n if (jjj % 10 == 0)\n {\n path.poses.push_back(msg_body_pose);\n pubPath.publish(path);\n }\n}\n//计算残差信息\nvoid h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct &ekfom_data)\n{\n double match_start = omp_get_wtime(); //计算匹配的开始时间\n laserCloudOri->clear(); //将body系的有效点云存储清空\n corr_normvect->clear(); //将对应的法向量清空\n total_residual = 0.0;\n\n/** 最接近曲面搜索和残差计算 **/\n#ifdef MP_EN\n omp_set_num_threads(MP_PROC_NUM);\n#pragma omp parallel for\n#endif\n //对降采样后的每个特征点进行残差计算\n for (int i = 0; i < feats_down_size; i++)\n {\n PointType &point_body = feats_down_body->points[i]; //获取降采样后的每个特征点\n PointType &point_world = feats_down_world->points[i]; //获取降采样后的每个特征点的世界坐标\n\n /* transform to world frame */\n //将点转换至世界坐标系下\n V3D p_body(point_body.x, point_body.y, point_body.z);\n V3D p_global(s.rot * (s.offset_R_L_I * p_body + s.offset_T_L_I) + s.pos); //将点转换至世界坐标系下,从而来计算残差\n point_world.x = p_global(0);\n point_world.y = p_global(1);\n point_world.z = p_global(2);\n point_world.intensity = point_body.intensity;\n\n vector pointSearchSqDis(NUM_MATCH_POINTS);\n\n auto &points_near = Nearest_Points[i];\n\n if (ekfom_data.converge) //如果收敛了\n {\n /** Find the closest surfaces in the map **/\n //在已构造的地图上查找特征点的最近邻\n ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis);\n //如果最近邻的点数小于NUM_MATCH_POINTS或者最近邻的点到特征点的距离大于5m，则认为该点不是有效点\n point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false\n : true;\n }\n\n if (!point_selected_surf[i]) //如果该点不是有效点\n continue;\n\n VF(4)\n pabcd; //平面点信息\n point_selected_surf[i] = false; //将该点设置为无效点，用来计算是否为平面点\n\n //拟合平面方程ax+by+cz+d=0并求解点到平面距离\n if (esti_plane(pabcd, points_near, 0.1f)) //找平面点法向量寻找，common_lib.h中的函数\n {\n float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3); //计算点到平面的距离\n float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm()); //计算残差\n\n if (s > 0.9) //如果残差大于阈值，则认为该点是有效点\n {\n point_selected_surf[i] = true; //再次回复为有效点\n normvec->points[i].x = pabcd(0); //将法向量存储至normvec\n normvec->points[i].y = pabcd(1);\n normvec->points[i].z = pabcd(2);\n normvec->points[i].intensity = pd2; //将点到平面的距离存储至normvec的intensit中\n res_last[i] = abs(pd2); //将残差存储至res_last\n }\n }\n }\n\n effct_feat_num = 0; //有效特征点数\n\n for (int i = 0; i < feats_down_size; i++)\n {\n //根据point_selected_surf状态判断哪些点是可用的\n if (point_selected_surf[i])\n {\n // body点存到laserCloudOri中\n laserCloudOri->points[effct_feat_num] = feats_down_body->points[i]; //将降采样后的每个特征点存储至laserCloudOri\n //拟合平面点存到corr_normvect中\n corr_normvect->points[effct_feat_num] = normvec->points[i];\n total_residual += res_last[i]; //计算总残差\n effct_feat_num++; //有效特征点数加1\n }\n }\n\n res_mean_last = total_residual / effct_feat_num; //计算残差平均值\n match_time += omp_get_wtime() - match_start; //返回从匹配开始时候所经过的时间\n double solve_start_ = omp_get_wtime(); //下面是solve求解的时间\n\n // 测量雅可比矩阵H和测量向量的计算 H=J*P*J'\n ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //测量雅可比矩阵H，论文中的23\n ekfom_data.h.resize(effct_feat_num); //测量向量h\n\n //求观测值与误差的雅克比矩阵，如论文式14以及式12、13\n for (int i = 0; i < effct_feat_num; i++)\n {\n // 拿到的有效点的坐标\n const PointType &laser_p = laserCloudOri->points[i];\n V3D point_this_be(laser_p.x, laser_p.y, laser_p.z);\n M3D point_be_crossmat; //计算点的反对称矩阵\n // 从点值转换到叉乘矩阵\n point_be_crossmat << SKEW_SYM_MATRX(point_this_be);\n // 转换到IMU坐标系下\n 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坐标系下\n M3D point_crossmat;\n point_crossmat << SKEW_SYM_MATRX(point_this); //计算imu中点的反对称矩阵\n\n // 得到对应的曲面/角的法向量\n const PointType &norm_p = corr_normvect->points[i];\n V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);\n\n // 计算测量雅可比矩阵H，见fatlio v1的论文公式(14)，求导这部分和LINS相同：https://zhuanlan.zhihu.com/p/258972164\n //FAST-LIO2的特别之处\n\t\t// 1.在IESKF中，状态更新可以看成是一个优化问题，即对位姿状态先验 x_bk_bk+1 的偏差，以及基于观测模型引入的残差函数f 的优化问题。\n\t\t// 2.LINS的特别之处在于，将LOAM的后端优化放在了IESKF的更新过程中实现，也就是用IESKF的迭代更新过程代替了LOAM的高斯牛顿法。\n V3D C(s.rot.conjugate() * norm_vec); // R^-1 * 法向量, s.rot.conjugate（）是四元数共轭，即旋转求逆\n V3D A(point_crossmat * C); // imu坐标系的点坐标的反对称点乘C\n V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //带be的是激光雷达原始坐标系的点云，不带be的是imu坐标系的点坐标\n 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);\n\n // 测量:到最近表面/角落的距离\n ekfom_data.h(i) = -norm_p.intensity; //点到面的距离\n }\n solve_time += omp_get_wtime() - solve_start_; //返回从solve开始时候所经过的时间\n}\n// FAST_LIO2主函数\nint main(int argc, char **argv)\n{\n /*****************************初始化：读取参数、定义变量以及赋值*************************/\n // 初始化ros节点，节点名为laserMapping\n ros::init(argc, argv, \"laserMapping\");\n ros::NodeHandle nh;\n // 从参数服务器读取参数值赋给变量（包括launch文件和launch读取的yaml文件中的参数）\n\n nh.param(\"publish/scan_publish_en\", scan_pub_en, true); // 是否发布当前正在扫描的点云的topic\n nh.param(\"publish/dense_publish_en\", dense_pub_en, true); // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，\n nh.param(\"publish/scan_bodyframe_pub_en\", scan_body_pub_en, true); // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，需要该变量和上一个变量同时为true才发布\n nh.param(\"max_iteration\", NUM_MAX_ITERATIONS, 4); // 卡尔曼滤波的最大迭代次数\n nh.param(\"map_file_path\", map_file_path, \"\"); // 地图保存路径\n nh.param(\"common/lid_topic\", lid_topic, \"/livox/lidar\"); // 激光雷达点云topic名称\n nh.param(\"common/imu_topic\", imu_topic, \"/livox/imu\"); // IMU的topic名称\n nh.param(\"common/time_sync_en\", time_sync_en, false); // 是否需要时间同步，只有当外部未进行时间同步时设为true\n nh.param(\"filter_size_corner\", filter_size_corner_min, 0.5); // VoxelGrid降采样时的体素大小\n nh.param(\"filter_size_surf\", filter_size_surf_min, 0.5); // VoxelGrid降采样时的体素大小\n nh.param(\"filter_size_map\", filter_size_map_min, 0.5); // VoxelGrid降采样时的体素大小\n nh.param(\"cube_side_length\", cube_len, 200); // 地图的局部区域的长度（FastLio2论文中有解释）\n nh.param(\"mapping/det_range\", DET_RANGE, 300.f); // 激光雷达的最大探测范围\n nh.param(\"mapping/fov_degree\", fov_deg, 180); // 激光雷达的视场角\n nh.param(\"mapping/gyr_cov\", gyr_cov, 0.1); // IMU陀螺仪的协方差\n nh.param(\"mapping/acc_cov\", acc_cov, 0.1); // IMU加速度计的协方差\n nh.param(\"mapping/b_gyr_cov\", b_gyr_cov, 0.0001); // IMU陀螺仪偏置的协方差\n nh.param(\"mapping/b_acc_cov\", b_acc_cov, 0.0001); // IMU加速度计偏置的协方差\n nh.param(\"preprocess/blind\", p_pre->blind, 0.01); // 最小距离阈值，即过滤掉0～blind范围内的点云\n nh.param(\"preprocess/lidar_type\", p_pre->lidar_type, AVIA); // 激光雷达的类型\n nh.param(\"preprocess/scan_line\", p_pre->N_SCANS, 16); // 激光雷达扫描的线数（livox avia为6线）\n nh.param(\"point_filter_num\", p_pre->point_filter_num, 2); // 采样间隔，即每隔point_filter_num个点取1个点\n nh.param(\"feature_extract_enable\", p_pre->feature_enabled, false); // 是否提取特征点（FAST_LIO2默认不进行特征点提取）\n nh.param(\"runtime_pos_log_enable\", runtime_pos_log, 0); // 是否输出调试log信息\n nh.param(\"pcd_save/pcd_save_en\", pcd_save_en, false); // 是否将点云地图保存到PCD文件\n nh.param>(\"mapping/extrinsic_T\", extrinT, vector()); // 雷达相对于IMU的外参T（即雷达在IMU坐标系中的坐标）\n nh.param>(\"mapping/extrinsic_R\", extrinR, vector()); // 雷达相对于IMU的外参R\n cout << \"p_pre->lidar_type \" << p_pre->lidar_type << endl;\n\n // 初始化path的header（包括时间戳和帧id），path用于保存odemetry的路径\n path.header.stamp = ros::Time::now();\n path.header.frame_id = \"camera_init\";\n\n /*** variables definition ***/\n /** 变量定义\n * effect_feat_num （后面的代码中没有用到该变量）\n * frame_num 雷达总帧数\n * deltaT （后面的代码中没有用到该变量）\n * deltaR （后面的代码中没有用到该变量）\n * aver_time_consu 每帧平均的处理总时间\n * aver_time_icp 每帧中icp的平均时间\n * aver_time_match 每帧中匹配的平均时间\n * aver_time_incre 每帧中ikd-tree增量处理的平均时间\n * aver_time_solve 每帧中计算的平均时间\n * aver_time_const_H_time 每帧中计算的平均时间（当H恒定时）\n * flg_EKF_converged （后面的代码中没有用到该变量）\n * EKF_stop_flg （后面的代码中没有用到该变量）\n * FOV_DEG （后面的代码中没有用到该变量）\n * HALF_FOV_COS （后面的代码中没有用到该变量）\n * _featsArray （后面的代码中没有用到该变量）\n */\n int effect_feat_num = 0, frame_num = 0;\n 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;\n bool flg_EKF_converged, EKF_stop_flg = 0;\n\n //这里没用到\n FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0);\n HALF_FOV_COS = cos((FOV_DEG)*0.5 * PI_M / 180.0);\n\n _featsArray.reset(new PointCloudXYZI());\n // 将数组point_selected_surf内元素的值全部设为true，数组point_selected_surf用于选择平面点\n memset(point_selected_surf, true, sizeof(point_selected_surf));\n // 将数组res_last内元素的值全部设置为-1000.0f，数组res_last用于平面拟合中\n memset(res_last, -1000.0f, sizeof(res_last));\n // VoxelGrid滤波器参数，即进行滤波时的创建的体素边长为filter_size_surf_min\n downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);\n // VoxelGrid滤波器参数，即进行滤波时的创建的体素边长为filter_size_map_min\n downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);\n\n // 重复操作 没有必要\n memset(point_selected_surf, true, sizeof(point_selected_surf));\n memset(res_last, -1000.0f, sizeof(res_last));\n\n // 从雷达到IMU的外参R和T\n Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT); // 相对IMU的外参\n Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR);\n\n // 设置IMU的参数，对p_imu进行初始化，其中p_imu为ImuProcess的智能指针（ImuProcess是进行IMU处理的类）\n p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU);\n p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov));\n p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov));\n p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov));\n p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov));\n\n double epsi[23] = {0.001};\n fill(epsi, epsi + 23, 0.001); //从epsi填充到epsi+22 也就是全部数组置0.001\n // 将函数地址传入kf对象中，用于接收特定于系统的模型及其差异\n // 作为一个维数变化的特征矩阵进行测量。\n // 通过一个函数（h_dyn_share_in）同时计算测量（z）、估计测量（h）、偏微分矩阵（h_x，h_v）和噪声协方差（R）。\n kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);\n\n /*** debug record ***/\n // 将调试log输出到文件中\n FILE *fp;\n string pos_log_dir = root_dir + \"/Log/pos_log.txt\";\n fp = fopen(pos_log_dir.c_str(), \"w\");\n\n ofstream fout_pre, fout_out, fout_dbg;\n fout_pre.open(DEBUG_FILE_DIR(\"mat_pre.txt\"), ios::out);\n fout_out.open(DEBUG_FILE_DIR(\"mat_out.txt\"), ios::out);\n fout_dbg.open(DEBUG_FILE_DIR(\"dbg.txt\"), ios::out);\n if (fout_pre && fout_out)\n cout << \"~~~~\" << ROOT_DIR << \" file opened\" << endl;\n else\n cout << \"~~~~\" << ROOT_DIR << \" doesn't exist\" << endl;\n\n /*** ROS subscribe initialization ***/\n // ROS订阅器和发布器的定义和初始化\n\n // 雷达点云的订阅器sub_pcl，订阅点云的topic\n 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);\n // IMU的订阅器sub_imu，订阅IMU的topic\n ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);\n // 发布当前正在扫描的点云，topic名字为/cloud_registered\n ros::Publisher pubLaserCloudFull = nh.advertise(\"/cloud_registered\", 100000);\n // 发布经过运动畸变校正到IMU坐标系的点云，topic名字为/cloud_registered_body\n ros::Publisher pubLaserCloudFull_body = nh.advertise(\"/cloud_registered_body\", 100000);\n // 发布雷达的点云，topic名字为/cloud_registered_lidar\n ros::Publisher pubLaserCloudFull_lidar = nh.advertise(\"/cloud_registered_lidar\", 100000);\n // 后面的代码中没有用到\n ros::Publisher pubLaserCloudEffect = nh.advertise(\"/cloud_effected\", 100000);\n // 后面的代码中没有用到\n ros::Publisher pubLaserCloudMap = nh.advertise(\"/Laser_map\", 100000);\n // 发布当前里程计信息，topic名字为/Odometry\n ros::Publisher pubOdomAftMapped = nh.advertise(\"/Odometry\", 100000);\n // 发布里程计总的路径，topic名字为/path\n ros::Publisher pubPath = nh.advertise(\"/path\", 100000);\n //------------------------------------------------------------------------------------------------------\n // 中断处理函数，如果有中断信号（比如Ctrl+C），则执行第二个参数里面的SigHandle函数\n signal(SIGINT, SigHandle);\n\n // 设置ROS程序主循环每次运行的时间至少为0.0002秒（5000Hz）\n ros::Rate rate(5000);\n bool status = ros::ok();\n // 程序主循环\n while (status)\n {\n // 如果有中断产生，则结束主循环\n if (flg_exit)\n break;\n // ROS消息回调处理函数，放在ROS的主循环中\n ros::spinOnce();\n // 将激光雷达点云数据和IMU数据从缓存队列中取出，进行时间对齐，并保存到Measures中\n if (sync_packages(Measures))\n {\n // 激光雷达第一次扫描\n if (flg_reset)\n {\n ROS_WARN(\"reset when rosbag play back\");\n p_imu->Reset();\n flg_reset = false;\n Measures.imu.clear(); //判断状态，并把imu的数据清空\n continue;\n }\n\n double t0, t1, t2, t3, t4, t5, match_start, solve_start, svd_time;\n\n match_time = 0;\n kdtree_search_time = 0.0;\n solve_time = 0;\n solve_const_H_time = 0;\n svd_time = 0;\n t0 = omp_get_wtime();\n // 对IMU数据进行预处理，其中包含了点云畸变处理 前向传播 反向传播\n p_imu->Process(Measures, kf, feats_undistort);\n // 获取kf预测的全局状态（imu）\n state_point = kf.get_x();\n //世界系下雷达坐标系的位置\n //下面式子的意义是W^p_L = W^p_I + W^R_I * I^t_L\n pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;\n\n if (feats_undistort->empty() || (feats_undistort == NULL)) //如果点云数据为空，则代表了激光雷达没有完成去畸变，此时还不能初始化成功\n {\n first_lidar_time = Measures.lidar_beg_time; //记录第一次扫描的时间\n p_imu->first_lidar_time = first_lidar_time; //将第一帧的时间传给imu作为当前帧的第一个点云时间\n // cout<<\"FAST-LIO not ready\"<points.size(); //记录滤波后的点云数量\n /*** initialize the map kdtree ***/\n // 构建kd树\n if (ikdtree.Root_Node == nullptr)\n {\n if (feats_down_size > 5)\n {\n // 设置ikd tree的降采样参数\n ikdtree.set_downsample_param(filter_size_map_min);\n feats_down_world->resize(feats_down_size); //将下采样得到的地图点大小于body系大小一致\n for (int i = 0; i < feats_down_size; i++)\n {\n pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); //将下采样得到的地图点转换为世界坐标系下的点云\n }\n // 组织ikd tree\n ikdtree.Build(feats_down_world->points); //构建ikd树\n }\n continue;\n }\n // 获取ikd tree中的有效节点数，无效点就是被打了deleted标签的点\n int featsFromMapNum = ikdtree.validnum();\n // 获取Ikd tree中的节点数\n kdtree_size_st = ikdtree.size();\n\n // cout<<\"[ mapping ]: In num: \"<points.size()<<\" downsamp \"<resize(feats_down_size);\n feats_down_world->resize(feats_down_size);\n\n // 外参，旋转矩阵转欧拉角\n V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);\n 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()\n << \" \" << state_point.bg.transpose() << \" \" << state_point.ba.transpose() << \" \" << state_point.grav << endl; //输出预测的结果\n\n if (0) // If you need to see map point, change to \"if(1)\"\n {\n // 释放PCL_Storage的内存\n PointVector().swap(ikdtree.PCL_Storage);\n // 把树展平用于展示\n ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);\n featsFromMap->clear();\n featsFromMap->points = ikdtree.PCL_Storage;\n }\n\n pointSearchInd_surf.resize(feats_down_size); //搜索索引\n Nearest_Points.resize(feats_down_size); //将降采样处理后的点云用于搜索最近点\n int rematch_num = 0;\n bool nearest_search_en = true; //\n\n t2 = omp_get_wtime();\n\n /*** 迭代状态估计 ***/\n double t_update_start = omp_get_wtime();\n double solve_H_time = 0;\n //迭代卡尔曼滤波更新，更新地图信息\n kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);\n state_point = kf.get_x();\n euler_cur = SO3ToEuler(state_point.rot);\n pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;\n geoQuat.x = state_point.rot.coeffs()[0];\n geoQuat.y = state_point.rot.coeffs()[1];\n geoQuat.z = state_point.rot.coeffs()[2];\n geoQuat.w = state_point.rot.coeffs()[3];\n\n double t_update_end = omp_get_wtime();\n\n /******* 发布里程计 *******/\n publish_odometry(pubOdomAftMapped);\n\n /*** 向映射kdtree添加特性点 ***/\n t3 = omp_get_wtime();\n map_incremental();\n t5 = omp_get_wtime();\n\n /******* 发布轨迹和点 *******/\n publish_path(pubPath);\n if (scan_pub_en || pcd_save_en)\n publish_frame_world(pubLaserCloudFull);\n if (scan_pub_en && scan_body_pub_en)\n {\n publish_frame_body(pubLaserCloudFull_body);\n publish_frame_lidar(pubLaserCloudFull_lidar);\n }\n // publish_effect_world(pubLaserCloudEffect);\n // publish_map(pubLaserCloudMap);\n\n /*** Debug 参数 ***/\n if (runtime_pos_log)\n {\n frame_num++;\n kdtree_size_end = ikdtree.size();\n aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;\n aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t_update_end - t_update_start) / frame_num;\n aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num;\n aver_time_incre = aver_time_incre * (frame_num - 1) / frame_num + (kdtree_incremental_time) / frame_num;\n aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time + solve_H_time) / frame_num;\n aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_time / frame_num;\n T1[time_log_counter] = Measures.lidar_beg_time;\n s_plot[time_log_counter] = t5 - t0; //整个流程总时间\n s_plot2[time_log_counter] = feats_undistort->points.size(); //特征点数量\n s_plot3[time_log_counter] = kdtree_incremental_time; // kdtree增量时间\n s_plot4[time_log_counter] = kdtree_search_time; // kdtree搜索耗时\n s_plot5[time_log_counter] = kdtree_delete_counter; // kdtree删除点数量\n s_plot6[time_log_counter] = kdtree_delete_time; // kdtree删除耗时\n s_plot7[time_log_counter] = kdtree_size_st; // kdtree初始大小\n s_plot8[time_log_counter] = kdtree_size_end; // kdtree结束大小\n s_plot9[time_log_counter] = aver_time_consu; //平均消耗时间\n s_plot10[time_log_counter] = add_point_size; //添加点数量\n time_log_counter++;\n 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);\n ext_euler = SO3ToEuler(state_point.offset_R_L_I);\n 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()\n << \" \" << state_point.bg.transpose() << \" \" << state_point.ba.transpose() << \" \" << state_point.grav << \" \" << feats_undistort->points.size() << endl;\n dump_lio_state_to_log(fp);\n }\n }\n\n status = ros::ok();\n rate.sleep();\n }\n\n /**************** save map ****************/\n /* 1. make sure you have enough memories\n /* 2. pcd save will largely influence the real-time performences **/\n if (pcl_wait_save->size() > 0 && pcd_save_en)\n {\n string file_name = string(\"scans.pcd\");\n string all_points_dir(string(string(ROOT_DIR) + \"PCD/\") + file_name);\n pcl::PCDWriter pcd_writer;\n cout << \"current scan saved to /PCD/\" << file_name << endl;\n pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);\n }\n\n fout_out.close();\n fout_pre.close();\n\n if (runtime_pos_log)\n {\n vector t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;\n FILE *fp2;\n string log_dir = root_dir + \"/Log/fast_lio_time_log.csv\";\n fp2 = fopen(log_dir.c_str(), \"w\");\n 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\");\n for (int i = 0; i < time_log_counter; i++)\n {\n 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]);\n t.push_back(T1[i]);\n s_vec.push_back(s_plot9[i]);\n s_vec2.push_back(s_plot3[i] + s_plot6[i]);\n s_vec3.push_back(s_plot4[i]);\n s_vec5.push_back(s_plot[i]);\n }\n fclose(fp2);\n }\n\n return 0;\n}\n" }, { "path": "FAST-LIO/src/preprocess.cpp", "content": "#include \"preprocess.h\"\n\n#define RETURN0 0x00\n#define RETURN0AND1 0x10\n\nPreprocess::Preprocess()\n : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)\n{\n inf_bound = 10; // 有效点集合,大于10m则是盲区\n N_SCANS = 6; //多线激光雷达的线数\n group_size = 8; // 8个点为一组\n disA = 0.01; // 点集合的距离阈值,判断是否为平面\n disB = 0.1; // 点集合的距离阈值,判断是否为平面\n p2l_ratio = 225; // 点到线的距离阈值，需要大于这个值才能判断组成面\n limit_maxmid = 6.25; // 中点到左侧的距离变化率范围\n limit_midmin = 6.25; // 中点到右侧的距离变化率范围\n limit_maxmin = 3.24; // 左侧到右侧的距离变化率范围\n jump_up_limit = 170.0;\n jump_down_limit = 8.0;\n cos160 = 160.0;\n edgea = 2; //点与点距离超过两倍则认为遮挡\n edgeb = 0.1; //点与点距离超过0.1m则认为遮挡\n smallp_intersect = 172.5;\n smallp_ratio = 1.2; //三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面\n given_offset_time = false; //是否提供时间偏移\n\n jump_up_limit = cos(jump_up_limit / 180 * M_PI); //角度大于170度的点跳过，认为在\n jump_down_limit = cos(jump_down_limit / 180 * M_PI); //角度小于8度的点跳过\n cos160 = cos(cos160 / 180 * M_PI); //夹角限制\n smallp_intersect = cos(smallp_intersect / 180 * M_PI); //三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面\n}\n\nPreprocess::~Preprocess() {}\n\nvoid Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)\n{\n feature_enabled = feat_en; //是否提取特征点\n lidar_type = lid_type; //雷达的种类\n blind = bld; //最小距离阈值，即过滤掉0～blind范围内的点云\n point_filter_num = pfilt_num; //采样间隔，即每隔point_filter_num个点取1个点\n}\n/**\n * @brief Livox激光雷达点云预处理函数\n *\n * @param msg livox激光雷达点云数据，格式为livox_ros_driver::CustomMsg\n * @param pcl_out 输出处理后的点云数据，格式为pcl::PointCloud\n */\nvoid Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)\n{\n avia_handler(msg);\n *pcl_out = pl_surf;\n}\n\nvoid Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)\n{\n switch (lidar_type)\n {\n case OUST64:\n oust64_handler(msg);\n break;\n\n case VELO16:\n velodyne_handler(msg);\n break;\n\n default:\n printf(\"Error LiDAR Type\");\n break;\n }\n *pcl_out = pl_surf;\n}\n/**\n * @brief 对Livox激光雷达点云数据进行预处理\n *\n * @param msg livox激光雷达点云数据，格式为livox_ros_driver::CustomMsg\n */\nvoid Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)\n{\n // 清除之前的点云缓存\n pl_surf.clear(); // 清除之前的平面点云缓存\n pl_corn.clear(); // 清除之前的角点云缓存\n pl_full.clear(); // 清除之前的全点云缓存\n double t1 = omp_get_wtime(); // 后面没用到\n int plsize = msg->point_num; // 一帧中的点云总个数\n // cout<<\"plsie: \"<points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))\n {\n pl_full[i].x = msg->points[i].x; // 点云x轴坐标\n pl_full[i].y = msg->points[i].y; // 点云y轴坐标\n pl_full[i].z = msg->points[i].z; // 点云z轴坐标\n pl_full[i].intensity = msg->points[i].reflectivity; // 点云强度\n pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // 使用曲率作为每个激光点的时间\n\n bool is_new = false;\n // 只有当当前点和上一点的间距足够大（>1e-7），才将当前点认为是有用的点，分别加入到对应line的pl_buff队列中\n 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))\n {\n pl_buff[msg->points[i].line].push_back(pl_full[i]); // 将当前点加入到对应line的pl_buff队列中\n }\n }\n }\n static int count = 0;\n static double time = 0.0;\n count++;\n double t0 = omp_get_wtime();\n // 对每个line中的激光雷达分别进行处理\n for (int j = 0; j < N_SCANS; j++)\n {\n // 如果该line中的点云过小，则继续处理下一条line\n if (pl_buff[j].size() <= 5)\n continue;\n pcl::PointCloud &pl = pl_buff[j];\n plsize = pl.size();\n vector &types = typess[j];\n types.clear();\n types.resize(plsize);\n plsize--;\n for (uint i = 0; i < plsize; i++)\n {\n types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y; // 计算每个点到机器人本身的距离\n vx = pl[i].x - pl[i + 1].x;\n vy = pl[i].y - pl[i + 1].y;\n vz = pl[i].z - pl[i + 1].z;\n types[i].dista = vx * vx + vy * vy + vz * vz; // 计算两个间隔点的距离\n }\n //因为i最后一个点没有i+1了所以就单独求了一个range，没有distance\n types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;\n give_feature(pl, types); //给特征\n // pl_surf += pl;\n }\n time += omp_get_wtime() - t0;\n printf(\"Feature extraction time: %lf \\n\", time / count);\n }\n else\n {\n // 分别对每个点云进行处理\n for (uint i = 1; i < plsize; i++)\n {\n // 只取线数在0~N_SCANS内并且回波次序为0或者1的点云\n if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))\n {\n valid_num++; // 有效的点云数\n\n // 等间隔降采样\n if (valid_num % point_filter_num == 0)\n {\n pl_full[i].x = msg->points[i].x; // 点云x轴坐标\n pl_full[i].y = msg->points[i].y; // 点云y轴坐标\n pl_full[i].z = msg->points[i].z; // 点云z轴坐标\n pl_full[i].intensity = msg->points[i].reflectivity; // 点云强度\n pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points\n\n // 只有当当前点和上一点的间距足够大（>1e-7），并且在最小距离阈值之外，才将当前点认为是有用的点，加入到pl_surf队列中\n 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))\n {\n pl_surf.push_back(pl_full[i]);\n }\n }\n }\n }\n }\n}\n\nvoid Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)\n{\n pl_surf.clear();\n pl_corn.clear();\n pl_full.clear();\n pcl::PointCloud pl_orig;\n pcl::fromROSMsg(*msg, pl_orig);\n int plsize = pl_orig.size();\n pl_corn.reserve(plsize);\n pl_surf.reserve(plsize);\n if (feature_enabled)\n {\n for (int i = 0; i < N_SCANS; i++)\n {\n pl_buff[i].clear();\n pl_buff[i].reserve(plsize);\n }\n\n for (uint i = 0; i < plsize; i++)\n {\n 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;\n if (range < blind)\n continue;\n Eigen::Vector3d pt_vec;\n PointType added_pt;\n added_pt.x = pl_orig.points[i].x;\n added_pt.y = pl_orig.points[i].y;\n added_pt.z = pl_orig.points[i].z;\n added_pt.intensity = pl_orig.points[i].intensity;\n added_pt.normal_x = 0;\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;\n //这里好像没有用 因为atan2()返回在-pi到pi之间\n if (yaw_angle >= 180.0)\n yaw_angle -= 360.0;\n if (yaw_angle <= -180.0)\n yaw_angle += 360.0;\n\n added_pt.curvature = pl_orig.points[i].t / 1e6;\n if (pl_orig.points[i].ring < N_SCANS)\n {\n pl_buff[pl_orig.points[i].ring].push_back(added_pt);\n }\n }\n\n for (int j = 0; j < N_SCANS; j++)\n {\n PointCloudXYZI &pl = pl_buff[j];\n int linesize = pl.size();\n vector &types = typess[j];\n types.clear();\n types.resize(linesize);\n linesize--;\n for (uint i = 0; i < linesize; i++)\n {\n types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);\n vx = pl[i].x - pl[i + 1].x;\n vy = pl[i].y - pl[i + 1].y;\n vz = pl[i].z - pl[i + 1].z;\n types[i].dista = vx * vx + vy * vy + vz * vz;\n }\n types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);\n give_feature(pl, types);\n }\n }\n else\n {\n double time_stamp = msg->header.stamp.toSec();\n // cout << \"===================================\" << endl;\n // printf(\"Pt size = %d, N_SCANS = %d\\r\\n\", plsize, N_SCANS);\n for (int i = 0; i < pl_orig.points.size(); i++)\n {\n if (i % point_filter_num != 0)\n continue;\n\n 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;\n\n if (range < blind)\n continue;\n\n Eigen::Vector3d pt_vec;\n PointType added_pt;\n added_pt.x = pl_orig.points[i].x;\n added_pt.y = pl_orig.points[i].y;\n added_pt.z = pl_orig.points[i].z;\n added_pt.intensity = pl_orig.points[i].intensity;\n added_pt.normal_x = 0; // Normal结构体表示给定点所在样本曲面上的法线方向,以及对应曲率的测量值\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;\n if (yaw_angle >= 180.0)\n yaw_angle -= 360.0;\n if (yaw_angle <= -180.0)\n yaw_angle += 360.0;\n\n added_pt.curvature = pl_orig.points[i].t / 1e6;\n\n pl_surf.points.push_back(added_pt);\n }\n }\n // pub_func(pl_surf, pub_full, msg->header.stamp);\n // pub_func(pl_surf, pub_corn, msg->header.stamp);\n}\n\n#define MAX_LINE_NUM 64\n\nvoid Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)\n{\n pl_surf.clear();\n pl_corn.clear();\n pl_full.clear();\n\n pcl::PointCloud pl_orig;\n pcl::fromROSMsg(*msg, pl_orig);\n int plsize = pl_orig.points.size();\n pl_surf.reserve(plsize);\n\n bool is_first[MAX_LINE_NUM];\n double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point\n double omega_l = 3.61; // scan angular velocity\n float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point\n float time_last[MAX_LINE_NUM] = {0.0}; // last offset time\n\n if (pl_orig.points[plsize - 1].time > 0) //假如提供了每个点的时间戳\n {\n given_offset_time = true; //提供时间偏移\n }\n else //没有提供每个点的时间戳\n {\n given_offset_time = false;\n memset(is_first, true, sizeof(is_first));\n double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;\n double yaw_end = yaw_first;\n int layer_first = pl_orig.points[0].ring;\n for (uint i = plsize - 1; i > 0; i--)\n {\n if (pl_orig.points[i].ring == layer_first)\n {\n yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; //寻找终止yaw角\n break;\n }\n }\n }\n\n if (feature_enabled)\n {\n for (int i = 0; i < N_SCANS; i++)\n {\n pl_buff[i].clear();\n pl_buff[i].reserve(plsize);\n }\n\n for (int i = 0; i < plsize; i++)\n {\n PointType added_pt;\n added_pt.normal_x = 0;\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\n int layer = pl_orig.points[i].ring;\n if (layer >= N_SCANS)\n continue;\n added_pt.x = pl_orig.points[i].x;\n added_pt.y = pl_orig.points[i].y;\n added_pt.z = pl_orig.points[i].z;\n added_pt.intensity = pl_orig.points[i].intensity;\n added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms\n\n if (!given_offset_time) //假如没有提供时间\n {\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;\n if (is_first[layer]) //给每条线第一个赋予yaw角 时间归0\n {\n // printf(\"layer: %d; is first: %d\", layer, is_first[layer]);\n yaw_fp[layer] = yaw_angle;\n is_first[layer] = false;\n added_pt.curvature = 0.0;\n yaw_last[layer] = yaw_angle; //对每一条线最后一个点的时间赋值\n time_last[layer] = added_pt.curvature; //对每一条线最后一个点的时间赋值\n continue;\n }\n //对每个点计算时间\n if (yaw_angle <= yaw_fp[layer])\n {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;\n }\n else\n {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;\n }\n //新得到的点的时间不能早于之前的最后一个点的时间\n if (added_pt.curvature < time_last[layer])\n added_pt.curvature += 360.0 / omega_l;\n\n yaw_last[layer] = yaw_angle;\n time_last[layer] = added_pt.curvature;\n }\n\n pl_buff[layer].points.push_back(added_pt);\n }\n\n for (int j = 0; j < N_SCANS; j++)\n {\n PointCloudXYZI &pl = pl_buff[j];\n int linesize = pl.size();\n if (linesize < 2)\n continue; //点太少跳过\n vector &types = typess[j];\n types.clear();\n types.resize(linesize);\n linesize--;\n for (uint i = 0; i < linesize; i++)\n {\n types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);\n vx = pl[i].x - pl[i + 1].x;\n vy = pl[i].y - pl[i + 1].y;\n vz = pl[i].z - pl[i + 1].z;\n types[i].dista = vx * vx + vy * vy + vz * vz;\n }\n types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);\n give_feature(pl, types);\n }\n }\n else\n {\n for (int i = 0; i < plsize; i++)\n {\n PointType added_pt;\n // cout<<\"!!!!!!\"< blind)\n {\n pl_surf.points.push_back(added_pt);\n // printf(\"time mode: %d time: %d \\n\", given_offset_time, pl_orig.points[i].t);\n }\n }\n }\n }\n\n // pub_func(pl_surf, pub_full, msg->header.stamp);\n // pub_func(pl_surf, pub_surf, msg->header.stamp);\n // pub_func(pl_surf, pub_corn, msg->header.stamp);\n}\n/**\n * @brief 对于每条line的点云提取特征\n *\n * @param pl pcl格式的点云 输入进来一条扫描线上的点\n * @param types 点云的其他属性\n */\nvoid Preprocess::give_feature(pcl::PointCloud &pl, vector &types)\n{\n int plsize = pl.size(); //单条线的点数\n int plsize2;\n if (plsize == 0)\n {\n printf(\"something wrong\\n\");\n return;\n }\n uint head = 0;\n //不能在盲区 从这条线非盲区的点开始\n while (types[head].range < blind)\n {\n head++;\n }\n\n // Surf\n // group_size默认等于8\n plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; //判断当前点后面是否还有8个点 够的话就逐渐减少\n\n Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); //当前平面的法向量\n Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); //上一个平面的法向量\n\n uint i_nex = 0, i2; // i2为当前点的下一个点\n uint last_i = 0; // last_i为上一个点的保存的索引\n uint last_i_nex = 0; // last_i_nex为上一个点的下一个点的索引\n int last_state = 0; //为1代表上个状态为平面 否则为0\n\n //判断面点\n int plane_type;\n\n // 拿到8个点用于判断平面\n for (uint i = head; i < plsize2; i++)\n {\n if (types[i].range < blind) // 在盲区范围内的点不做处理\n {\n continue;\n }\n\n i2 = i; //更新i2\n\n plane_type = plane_judge(pl, types, i, i_nex, curr_direct); //求得平面，并返回类型0 1 2\n\n if (plane_type == 1) //返回1一般默认是平面\n {\n //设置确定的平面点和可能的平面点\n for (uint j = i; j <= i_nex; j++)\n {\n if (j != i && j != i_nex)\n {\n //把起始点和终止点之间的所有点设置为确定的平面点\n types[j].ftype = Real_Plane;\n }\n else\n {\n //把起始点和终止点设置为可能的平面点\n types[j].ftype = Poss_Plane;\n }\n }\n // if(last_state==1 && fabs(last_direct.sum())>0.5)\n\n //最开始last_state=0直接跳过\n //之后last_state=1\n //如果之前状态是平面则判断当前点是处于两平面边缘的点还是较为平坦的平面的点\n if (last_state == 1 && last_direct.norm() > 0.1)\n {\n double mod = last_direct.transpose() * curr_direct;\n if (mod > -0.707 && mod < 0.707)\n {\n //修改ftype，两个面法向量夹角在45度和135度之间 认为是两平面边缘上的点\n types[i].ftype = Edge_Plane;\n }\n else\n {\n //否则认为是真正的平面点\n types[i].ftype = Real_Plane;\n }\n }\n\n i = i_nex - 1;\n last_state = 1;\n }\n else // if(plane_type == 2)\n {\n // plane_type=0或2的时候\n i = i_nex;\n last_state = 0; //设置为不是平面点\n }\n // else if(plane_type == 0)\n // {\n // if(last_state == 1)\n // {\n // uint i_nex_tem;\n // uint j;\n // for(j=last_i+1; j<=last_i_nex; j++)\n // {\n // uint i_nex_tem2 = i_nex_tem;\n // Eigen::Vector3d curr_direct2;\n\n // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);\n\n // if(ttem != 1)\n // {\n // i_nex_tem = i_nex_tem2;\n // break;\n // }\n // curr_direct = curr_direct2;\n // }\n\n // if(j == last_i+1)\n // {\n // last_state = 0;\n // }\n // else\n // {\n // for(uint k=last_i_nex; k<=i_nex_tem; k++)\n // {\n // if(k != i_nex_tem)\n // {\n // types[k].ftype = Real_Plane;\n // }\n // else\n // {\n // types[k].ftype = Poss_Plane;\n // }\n // }\n // i = i_nex_tem-1;\n // i_nex = i_nex_tem;\n // i2 = j-1;\n // last_state = 1;\n // }\n\n // }\n // }\n\n last_i = i2; //更新last_i\n last_i_nex = i_nex; //更新last_i_nex\n last_direct = curr_direct; //更新last_direct\n }\n\n //判断边缘点\n plsize2 = plsize > 3 ? plsize - 3 : 0; //如果剩下的点数小于3则不判断边缘点，否则计算哪些点是边缘点\n for (uint i = head + 3; i < plsize2; i++)\n {\n //点不能在盲区 或者 点必须属于正常点和可能的平面点\n if (types[i].range < blind || types[i].ftype >= Real_Plane)\n {\n continue;\n }\n //该点与前后点的距离不能挨的太近\n if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16)\n {\n continue;\n }\n\n Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); //当前点组成的向量\n Eigen::Vector3d vecs[2];\n\n for (int j = 0; j < 2; j++)\n {\n int m = -1;\n if (j == 1)\n {\n m = 1;\n }\n\n //若当前的前/后一个点在盲区内（4m)\n if (types[i + m].range < blind)\n {\n if (types[i].range > inf_bound) //若其大于10m\n {\n types[i].edj[j] = Nr_inf; //赋予该点Nr_inf(跳变较远)\n }\n else\n {\n types[i].edj[j] = Nr_blind; //赋予该点Nr_blind(在盲区)\n }\n continue;\n }\n\n vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z);\n vecs[j] = vecs[j] - vec_a; //前/后点指向当前点的向量\n //若雷达坐标系原点为O 当前点为A 前/后一点为M和N\n //则下面OA点乘MA/（|OA|*|MA|）\n //得到的是cos角OAM的大小\n\n types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); //角MAN的cos值\n\n //前一个点是正常点 && 下一个点在激光线上 && 当前点与后一个点的距离大于0.0225m && 当前点与后一个点的距离大于当前点与前一个点距离的四倍\n //这种边缘点像是7字形这种的边缘？\n if (types[i].angle[j] < jump_up_limit) // cos(170)\n {\n types[i].edj[j] = Nr_180; // M在OA延长线上\n }\n else if (types[i].angle[j] > jump_down_limit) // cos(8)\n {\n types[i].edj[j] = Nr_zero; // M在OA上\n }\n }\n\n types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();\n 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)\n {\n if (types[i].intersect > cos160) //角MAN要小于160度 不然就平行于激光了\n {\n if (edge_jump_judge(pl, types, i, Prev))\n {\n types[i].ftype = Edge_Jump;\n }\n }\n }\n //与上面类似\n //前一个点在激光束上 && 后一个点正常 && 前一个点与当前点的距离大于0.0225m && 前一个点与当前点的距离大于当前点与后一个点距离的四倍\n 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)\n {\n if (types[i].intersect > cos160)\n {\n if (edge_jump_judge(pl, types, i, Next))\n {\n types[i].ftype = Edge_Jump;\n }\n }\n }\n //前面的是正常点 && (当前点到中心距离>10m并且后点在盲区)\n else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf)\n {\n if (edge_jump_judge(pl, types, i, Prev))\n {\n types[i].ftype = Edge_Jump;\n }\n }\n //(当前点到中心距离>10m并且前点在盲区) && 后面的是正常点\n else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor)\n {\n if (edge_jump_judge(pl, types, i, Next))\n {\n types[i].ftype = Edge_Jump;\n }\n }\n //前后点都不是正常点\n else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor)\n {\n if (types[i].ftype == Nor)\n {\n types[i].ftype = Wire; //程序中应该没使用 当成空间中的小线段或者无用点了\n }\n }\n }\n\n plsize2 = plsize - 1;\n double ratio;\n //继续找平面点\n for (uint i = head + 1; i < plsize2; i++)\n {\n //前面、当前、之后三个点都需要不在盲区内\n if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind)\n {\n continue;\n }\n //前面和当前 当前和之后的点与点之间距离都不能太近\n if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8)\n {\n continue;\n }\n //还剩下来一些正常点继续找平面点\n\n if (types[i].ftype == Nor)\n {\n //求点与点间距的比例 大间距/小间距\n if (types[i - 1].dista > types[i].dista)\n {\n ratio = types[i - 1].dista / types[i].dista;\n }\n else\n {\n ratio = types[i].dista / types[i - 1].dista;\n }\n\n //如果夹角大于172.5度 && 间距比例<1.2\n if (types[i].intersect < smallp_intersect && ratio < smallp_ratio)\n {\n //前后三个点认为是平面点\n if (types[i - 1].ftype == Nor)\n {\n types[i - 1].ftype = Real_Plane;\n }\n if (types[i + 1].ftype == Nor)\n {\n types[i + 1].ftype = Real_Plane;\n }\n types[i].ftype = Real_Plane;\n }\n }\n }\n //存储平面点\n int last_surface = -1;\n for (uint j = head; j < plsize; j++)\n {\n //可能的平面点和确定的平面点\n if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane)\n {\n if (last_surface == -1)\n {\n last_surface = j;\n }\n //通常连着好几个都是面点\n //必须在采样间隔上的平面点才使用（这里的是无差别滤波 从每次新找到面点开始每几个点才取一个）\n if (j == uint(last_surface + point_filter_num - 1))\n {\n PointType ap;\n ap.x = pl[j].x;\n ap.y = pl[j].y;\n ap.z = pl[j].z;\n ap.intensity = pl[j].intensity;\n ap.curvature = pl[j].curvature;\n pl_surf.push_back(ap);\n\n last_surface = -1;\n }\n }\n else\n {\n //跳变较大的边缘边的点 位于平面边缘的点\n if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane)\n {\n pl_corn.push_back(pl[j]);\n }\n //假如上次找到的面点被无差别滤波掉了，而此时已经到了边缘\n if (last_surface != -1)\n {\n PointType ap;\n //取上次面点到此次边缘线之间的所有点的重心当作一个面点存储进去\n for (uint k = last_surface; k < j; k++)\n {\n ap.x += pl[k].x;\n ap.y += pl[k].y;\n ap.z += pl[k].z;\n ap.intensity += pl[k].intensity;\n ap.curvature += pl[k].curvature;\n }\n ap.x /= (j - last_surface);\n ap.y /= (j - last_surface);\n ap.z /= (j - last_surface);\n ap.intensity /= (j - last_surface);\n ap.curvature /= (j - last_surface);\n pl_surf.push_back(ap);\n }\n last_surface = -1;\n }\n }\n}\n\n// 发布topic指令，该函数暂时没用到\nvoid Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct)\n{\n pl.height = 1;\n pl.width = pl.size();\n sensor_msgs::PointCloud2 output;\n pcl::toROSMsg(pl, output);\n output.header.frame_id = \"livox\";\n output.header.stamp = ct;\n}\n\n//平面判断\nint Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct)\n{\n double group_dis = disA * types[i_cur].range + disB; // 0.01*sqrt(x^2+y^2)+0.1 基本上可以近似看成是0.1 100m的时候才到0.2\n group_dis = group_dis * group_dis;\n // i_nex = i_cur;\n\n double two_dis;\n vector disarr; //前后点距离数组\n disarr.reserve(20);\n\n //距离小 点与点之间较近 先取够8个点\n for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++)\n {\n if (types[i_nex].range < blind)\n {\n curr_direct.setZero(); //距离雷达原点太小了将法向量设置为零向量\n return 2;\n }\n disarr.push_back(types[i_nex].dista); //存储当前点与后一个点的距离\n }\n\n //看看后续的点有没有满足条件的\n for (;;)\n {\n if ((i_cur >= pl.size()) || (i_nex >= pl.size())) //索引超出所有点的个数直接BREAK\n break;\n\n if (types[i_nex].range < blind)\n {\n curr_direct.setZero(); //距离雷达原点太小了将法向量设置为零向量\n return 2;\n }\n //最后的i_nex点到i_cur点的距离\n vx = pl[i_nex].x - pl[i_cur].x;\n vy = pl[i_nex].y - pl[i_cur].y;\n vz = pl[i_nex].z - pl[i_cur].z;\n two_dis = vx * vx + vy * vy + vz * vz;\n if (two_dis >= group_dis) //距离i_cur点太远了就直接break\n {\n break;\n }\n disarr.push_back(types[i_nex].dista); //存储当前点与后一个点的距离\n i_nex++; // i_nex点加一\n }\n\n double leng_wid = 0;\n double v1[3], v2[3];\n for (uint j = i_cur + 1; j < i_nex; j++)\n {\n if ((j >= pl.size()) || (i_cur >= pl.size()))\n break;\n //假设i_cur点为A j点为B i_nex点为C\n //向量AB\n v1[0] = pl[j].x - pl[i_cur].x;\n v1[1] = pl[j].y - pl[i_cur].y;\n v1[2] = pl[j].z - pl[i_cur].z;\n //向量AB叉乘向量AC\n v2[0] = v1[1] * vz - vy * v1[2];\n v2[1] = v1[2] * vx - v1[0] * vz;\n v2[2] = v1[0] * vy - vx * v1[1];\n //物理意义是组成的ABC组成的平行四边形面积的平方(为|AC|*h，其中为B到线AC的距离)\n\n double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2];\n if (lw > leng_wid)\n {\n leng_wid = lw; //寻找最大面积的平方(也就是寻找距离AC最远的B)\n }\n }\n\n //|AC|*|AC|/(|AC|*|AC|*h*h)<225\n //也就是h>1/15 B点到AC的距离要大于0.06667m\n //太近了不好拟合一个平面\n if ((two_dis * two_dis / leng_wid) < p2l_ratio)\n {\n curr_direct.setZero(); //太近了法向量直接设置为0\n return 0;\n }\n\n //把两点之间的距离 按从大到小排个顺序\n uint disarrsize = disarr.size();\n for (uint j = 0; j < disarrsize - 1; j++)\n {\n for (uint k = j + 1; k < disarrsize; k++)\n {\n if (disarr[j] < disarr[k])\n {\n leng_wid = disarr[j];\n disarr[j] = disarr[k];\n disarr[k] = leng_wid;\n }\n }\n }\n //这里可能最近的点还是太近了\n if (disarr[disarr.size() - 2] < 1e-16)\n {\n curr_direct.setZero();\n return 0;\n }\n\n //目前还不太懂为什么给AVIA单独弄了一种，其实我觉得全都可以采用上面这种判定方式（虽然FAST-LIO默认不开特征提取）\n if (lidar_type == AVIA)\n {\n //点与点之间距离变化太大的时候 可能与激光束是平行的 就也舍弃了\n double dismax_mid = disarr[0] / disarr[disarrsize / 2];\n double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2];\n\n if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin)\n {\n curr_direct.setZero();\n return 0;\n }\n }\n else\n {\n double dismax_min = disarr[0] / disarr[disarrsize - 2];\n if (dismax_min >= limit_maxmin)\n {\n curr_direct.setZero();\n return 0;\n }\n }\n\n curr_direct << vx, vy, vz;\n curr_direct.normalize(); //法向量归一化\n return 1;\n}\n//边缘判断\nbool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir)\n{\n if (nor_dir == 0)\n {\n if (types[i - 1].range < blind || types[i - 2].range < blind) //前两个点不能在盲区\n {\n return false;\n }\n }\n else if (nor_dir == 1)\n {\n if (types[i + 1].range < blind || types[i + 2].range < blind) //后两个点不能在盲区\n {\n return false;\n }\n }\n //下面分别对i-2 i-1和i i+1两种情况时点与点间距进行了判断\n double d1 = types[i + nor_dir - 1].dista;\n double d2 = types[i + 3 * nor_dir - 2].dista;\n double d;\n //将大小间距进行调换 大在前 小在后\n if (d1 < d2)\n {\n d = d1;\n d1 = d2;\n d2 = d;\n }\n\n d1 = sqrt(d1);\n d2 = sqrt(d2);\n\n if (d1 > edgea * d2 || (d1 - d2) > edgeb)\n {\n //假如间距太大 可能是被遮挡，就不把它当作边缘点\n return false;\n }\n\n return true;\n}\n" }, { "path": "FAST-LIO/src/preprocess.h", "content": "#include \n#include \n#include \n#include \n\nusing namespace std;\n\n#define IS_VALID(a) ((abs(a) > 1e8) ? true : false)\n\ntypedef pcl::PointXYZINormal PointType;\ntypedef pcl::PointCloud PointCloudXYZI;\n\n// 枚举类型：表示支持的雷达类型\nenum LID_TYPE\n{\n AVIA = 1,\n VELO16,\n OUST64\n}; //{1, 2, 3}\n\n// 枚举类型：表示特征点的类型\nenum Feature\n{\n Nor, // 正常点\n Poss_Plane, // 可能的平面点\n Real_Plane, // 确定的平面点\n Edge_Jump, // 有跨越的边\n Edge_Plane, // 边上的平面点\n Wire, // 线段 这个也许当了无效点？也就是空间中的小线段？\n ZeroPoint // 无效点 程序中未使用\n};\n// 枚举类型：位置标识\nenum Surround\n{\n Prev, // 前一个\n Next // 后一个\n};\n\n// 枚举类型：表示有跨越边的类型\nenum E_jump\n{\n Nr_nor, // 正常\n Nr_zero, // 0\n Nr_180, // 180\n Nr_inf, // 无穷大 跳变较远？\n Nr_blind // 在盲区？\n};\n// orgtype类：用于存储激光雷达点的一些其他属性\nstruct orgtype\n{\n double range; // 点云在xy平面离雷达中心的距离\n double dista; // 当前点与后一个点之间的距离\n //假设雷达原点为O 前一个点为M 当前点为A 后一个点为N\n double angle[2]; // 这个是角OAM和角OAN的cos值\n double intersect; // 这个是角MAN的cos值\n E_jump edj[2]; // 前后两点的类型\n Feature ftype; // 点类型\n\n // 构造函数\n orgtype()\n {\n range = 0;\n edj[Prev] = Nr_nor;\n edj[Next] = Nr_nor;\n ftype = Nor; //默认为正常点\n intersect = 2;\n }\n};\n\n// velodyne数据结构\nnamespace velodyne_ros\n{\n struct EIGEN_ALIGN16 Point\n {\n PCL_ADD_POINT4D; // 4D点坐标类型\n float intensity; // 强度\n float time; // 时间\n uint16_t ring; // 点所属的圈数\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐\n };\n} // namespace velodyne_ros\n\n// 注册velodyne_ros的Point类型\nPOINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,\n (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(uint16_t, ring, ring))\n\n// ouster数据结构\nnamespace ouster_ros\n{\n struct EIGEN_ALIGN16 Point\n {\n PCL_ADD_POINT4D; // 4D点坐标类型\n float intensity; // 强度\n uint32_t t; // 时间\n uint16_t reflectivity; // 反射率\n uint8_t ring; // 点所属的圈数\n uint16_t ambient; // 没用到\n uint32_t range; // 距离\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐\n };\n} // namespace ouster_ros\n\n// clang-format off\n// 注册ouster的Point类型\nPOINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,\n (float, x, x)\n (float, y, y)\n (float, z, z)\n (float, intensity, intensity)\n // use std::uint32_t to avoid conflicting with pcl::uint32_t\n (std::uint32_t, t, t)\n (std::uint16_t, reflectivity, reflectivity)\n (std::uint8_t, ring, ring)\n (std::uint16_t, ambient, ambient)\n (std::uint32_t, range, range)\n)\n\n// Preproscess类：用于对激光雷达点云数据进行预处理\nclass Preprocess\n{\n public:\n// EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Preprocess(); // 构造函数\n ~Preprocess(); // 析构函数\n \n void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对Livox自定义Msg格式的激光雷达数据进行处理\n void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对ros的Msg格式的激光雷达数据进行处理\n void set(bool feat_en, int lid_type, double bld, int pfilt_num);\n\n // sensor_msgs::PointCloud2::ConstPtr pointcloud;\n PointCloudXYZI pl_full, pl_corn, pl_surf; // 全部点、边缘点、平面点\n PointCloudXYZI pl_buff[128]; //maximum 128 line lidar\n vector typess[128]; //maximum 128 line lidar\n int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; // 雷达类型、采样间隔、扫描线数、扫描频率\n double blind; // 最小距离阈值(盲区)\n bool feature_enabled, given_offset_time; // 是否提取特征、是否进行时间偏移\n ros::Publisher pub_full, pub_surf, pub_corn; // 发布全部点、发布平面点、发布边缘点\n \n\n private:\n void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); // 用于对Livox激光雷达数据进行处理\n void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对ouster激光雷达数据进行处理\n void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对velodyne激光雷达数据进行处理\n void give_feature(PointCloudXYZI &pl, vector &types);\n void pub_func(PointCloudXYZI &pl, const ros::Time &ct);\n int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);\n bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); // 没有用到\n bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir);\n \n int group_size;\n double disA, disB, inf_bound;\n double limit_maxmid, limit_midmin, limit_maxmin;\n double p2l_ratio;\n double jump_up_limit, jump_down_limit;\n double cos160;\n double edgea, edgeb;\n double smallp_intersect, smallp_ratio;\n double vx, vy, vz;\n};\n" }, { "path": "README.md", "content": "# FAST_LIO2_Noted\n\n## 中文注释说明\n\n本项目是基于[FAST_LIO_SLAM](https://github.com/GDUT-Kyle/FAST_LIO_SLAM)项目扩展来的，主要讲述了Fast-LIO2主程序代码、esikf更新代码和ikd-tree算法，因此基于自己的理解和思考，在代码里做了相关的中文注释。注释部分难免有误，欢迎大家批评指正。\n\n## News\n\n- ``Aug 2021``: The Livox-lidar tests and corresponding launch files will be uploaded soon. Currenty only Ouster lidar tutorial videos had been made. \n\n## What is FAST_LIO_SLAM?\nIntegration of \n1. [FAST-LIO2](https://github.com/hku-mars/FAST_LIO) (Odometry): A computationally efficient and robust LiDAR-inertial odometry (LIO) package\n2. [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\n\n## Features\n- An easy-to-use plug-and-play LiDAR SLAM \n - FAST-LIO2 and SC-PGO run separately (see below How to use? tab).\n - SC-PGO takes odometry and lidar point cloud topics from the FAST-LIO2 node. \n - Finally, an optimized map is made within the SC-PGO node. \n\n## How to use?\n- 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).\n```\n # terminal 1: run FAST-LIO2 \n mkdir -p ~/catkin_fastlio_slam/src\n cd ~/catkin_fastlio_slam/src\n git clone https://github.com/gisbi-kim/FAST_LIO_SLAM.git\n git clone https://github.com/Livox-SDK/livox_ros_driver\n cd .. \n catkin_make\n source devel/setup.bash\n roslaunch fast_lio mapping_ouster64_mulran.launch # setting for MulRan dataset \n\n # open the other terminal tab: run SC-PGO\n cd ~/catkin_fastlio_slam\n source devel/setup.bash\n roslaunch aloam_velodyne fastlio_ouster64.launch # setting for MulRan dataset \n\n # open the other terminal tab\n # run file_player_mulran (for the details, refer here https://github.com/irapkaist/file_player_mulran)\n```\n\n## Utility\n- 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).\n\n## Example results \n- [Tutorial video 1](https://youtu.be/nu8j4yaBMnw) (using KAIST 03 sequence of [MulRan dataset](https://sites.google.com/view/mulran-pr/dataset))\n - Example result captures \n \n

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

\n - [download the Riverisde02 pcd map](https://www.dropbox.com/s/1aolth7ry4odxo4/Riverside02.pcd?dl=0) made by FAST-LIO-SLAM, 400MB\n\n## Acknowledgements \n- Thanks for [FAST_LIO](https://github.com/hku-mars/FAST_LIO) authors.\n\n\n\n" }, { "path": "SC-PGO/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(aloam_velodyne)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# set(CMAKE_CXX_FLAGS \"-std=c++11\")\nset(CMAKE_CXX_STANDARD 17)\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g\")\n\nfind_package(catkin REQUIRED COMPONENTS\n geometry_msgs\n nav_msgs\n sensor_msgs\n roscpp\n rospy\n rosbag\n std_msgs\n image_transport\n cv_bridge\n tf\n)\n\n#find_package(Eigen3 REQUIRED)\nfind_package(PCL REQUIRED)\nfind_package(OpenCV REQUIRED)\nfind_package(Ceres REQUIRED)\n\nfind_package(OpenMP REQUIRED)\nfind_package(GTSAM REQUIRED QUIET)\n\ninclude_directories(\n include\n\t${catkin_INCLUDE_DIRS} \n\t${PCL_INCLUDE_DIRS}\n ${CERES_INCLUDE_DIRS}\n ${OpenCV_INCLUDE_DIRS}\n ${GTSAM_INCLUDE_DIR}\n)\n\ncatkin_package(\n CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs\n DEPENDS EIGEN3 PCL \n INCLUDE_DIRS include\n)\n\n\nadd_executable(ascanRegistration src/scanRegistration.cpp)\ntarget_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES})\n\nadd_executable(alaserOdometry src/laserOdometry.cpp)\ntarget_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})\n\nadd_executable(alaserMapping src/laserMapping.cpp)\ntarget_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})\n\nadd_executable(alaserPGO \n src/laserPosegraphOptimization.cpp\n include/scancontext/Scancontext.cpp\n)\ntarget_compile_options(alaserPGO \n PRIVATE ${OpenMP_CXX_FLAGS}\n)\ntarget_link_libraries(alaserPGO \n ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}\n ${OpenMP_CXX_FLAGS}\n gtsam\n)\n\nadd_executable(kittiHelper src/kittiHelper.cpp)\ntarget_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})\n\n\n\n\n" }, { "path": "SC-PGO/README.md", "content": "# SC-A-LOAM\n\n## What is SC-A-LOAM? \n- A real-time LiDAR SLAM package that integrates A-LOAM and ScanContext. \n - **A-LOAM** for odometry (i.e., consecutive motion estimation)\n - **ScanContext** for coarse global localization that can deal with big drifts (i.e., place recognition as kidnapped robot problem without initial pose)\n - and iSAM2 of GTSAM is used for pose-graph optimization. \n- This package aims to show ScanContext's handy applicability. \n - The only things a user should do is just to include `Scancontext.h`, call `makeAndSaveScancontextAndKeys` and `detectLoopClosureID`. \n\n## Features \n1. A strong place recognition and loop closing \n - 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)\n2. A modular implementation \n - 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). \n -

\n3. (optional) Altitude stabilization using consumer-level GPS \n - 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. \n\n## Prerequisites (dependencies)\n- We mainly depend on ROS, Ceres (for A-LOAM), and GTSAM (for pose-graph optimization). \n - For the details to install the prerequisites, please follow the A-LOAM and LIO-SAM repositiory. \n- The below examples are done under ROS melodic (ubuntu 18) and GTSAM version 4.x. \n\n## How to use? \n- First, install the abovementioned dependencies, and follow below lines. \n```\n mkdir -p ~/catkin_scaloam_ws/src\n cd ~/catkin_scaloam_ws/src\n git clone https://github.com/gisbi-kim/SC-A-LOAM.git\n cd ../\n catkin_make\n source ~/catkin_scaloam_ws/devel/setup.bash\n roslaunch aloam_velodyne aloam_mulran.launch # for MulRan dataset setting \n```\n\n## Example Results \n\n### Riverside 01, MulRan dataset \n- The MulRan dataset provides lidar scans (Ouster OS1-64, horizontally mounted, 10Hz) and consumer level gps (U-Blox EVK-7P, 4Hz) data.\n - About how to use (publishing data) data: see here https://github.com/irapkaist/file_player_mulran\n- example videos on Riverside 01 sequence. \n 1. with consumer level GPS-based altitude stabilization: https://youtu.be/FwAVX5TVm04\n 2. without the z stabilization: https://youtu.be/okML_zNadhY \n- example result:\n\n

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

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

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

\n\n### For Navtech Radar \n- Scan Context also works for Navtech Radar data!\n- For the details, please see \n - https://github.com/gisbi-kim/navtech-radar-slam\n - used the pose-graph optimization node of this repository (SC-A-LOAM)\n - [example video](https://www.youtube.com/watch?v=avtIQ8fesgU&t=128s)\n\n## Utilities\n\n### Data saver and Map construction \n- 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). \n- 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)). \n\n

\n\n- 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).\n\n## Acknowledgements\n- Thanks to LOAM, A-LOAM, and LIO-SAM code authors. The major codes in this repository are borrowed from their efforts.\n\n## Maintainer \n- please contact me through `paulgkim@kaist.ac.kr` \n\n## TODO\n- Delayed RS loop closings \n- SLAM with multi-session localization \n- More examples on other datasets (KITTI, complex urban dataset, etc.)\n" }, { "path": "SC-PGO/docker/Dockerfile", "content": "FROM ros:kinetic-perception\n\nENV CERES_VERSION=\"1.12.0\"\nENV PCL_VERSION=\"1.8.0\"\nENV CATKIN_WS=/root/catkin_ws\n\n # setup processors number used to compile library\nRUN if [ \"x$(nproc)\" = \"x1\" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi && \\\n # Install dependencies\n apt-get update && apt-get install -y \\\n cmake \\\n libatlas-base-dev \\\n libeigen3-dev \\\n libgoogle-glog-dev \\\n libsuitesparse-dev \\\n python-catkin-tools \\\n ros-${ROS_DISTRO}-cv-bridge \\\n ros-${ROS_DISTRO}-image-transport \\\n ros-${ROS_DISTRO}-message-filters \\\n ros-${ROS_DISTRO}-tf && \\\n rm -rf /var/lib/apt/lists/* && \\\n # Build and install Ceres\n git clone https://ceres-solver.googlesource.com/ceres-solver && \\\n cd ceres-solver && \\\n git checkout tags/${CERES_VERSION} && \\\n mkdir build && cd build && \\\n cmake .. && \\\n make -j${USE_PROC} install && \\\n cd ../.. && \\\n rm -rf ./ceres-solver && \\\n # Build and install pcl\n git clone https://github.com/PointCloudLibrary/pcl.git && \\\n cd pcl && \\\n git checkout tags/pcl-${PCL_VERSION} && \\\n mkdir build && cd build && \\\n cmake .. && \\\n make -j${USE_PROC} install && \\\n cd ../.. && \\\n rm -rf ./pcl && \\\n # Setup catkin workspace\n mkdir -p $CATKIN_WS/src/A-LOAM/\n \n# WORKDIR $CATKIN_WS/src\n\n# Copy A-LOAM\nCOPY ./ $CATKIN_WS/src/A-LOAM/\n# use the following line if you only have this dockerfile\n# RUN git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git\n\n# Build A-LOAM\nWORKDIR $CATKIN_WS\nENV TERM xterm\nENV PYTHONIOENCODING UTF-8\nRUN catkin config \\\n --extend /opt/ros/$ROS_DISTRO \\\n --cmake-args \\\n -DCMAKE_BUILD_TYPE=Release && \\\n catkin build && \\\n sed -i '/exec \"$@\"/i \\\n source \"/root/catkin_ws/devel/setup.bash\"' /ros_entrypoint.sh\n" }, { "path": "SC-PGO/docker/Makefile", "content": "all: help\n\nhelp:\n\t@echo \"\"\n\t@echo \"-- Help Menu\"\n\t@echo \"\"\n\t@echo \" 1. make build - build all images\"\n\t# @echo \" 1. make pull - pull all images\"\n\t@echo \" 1. make clean - remove all images\"\n\t@echo \"\"\n\nbuild:\n\t@docker build --tag ros:aloam -f ./Dockerfile ..\n\nclean:\n\t@docker rmi -f ros:aloam\n" }, { "path": "SC-PGO/docker/run.sh", "content": "#!/bin/bash\ntrap : SIGTERM SIGINT\n\nfunction abspath() {\n # generate absolute path from relative path\n # $1 : relative filename\n # return : absolute path\n if [ -d \"$1\" ]; then\n # dir\n (cd \"$1\"; pwd)\n elif [ -f \"$1\" ]; then\n # file\n if [[ $1 = /* ]]; then\n echo \"$1\"\n elif [[ $1 == */* ]]; then\n echo \"$(cd \"${1%/*}\"; pwd)/${1##*/}\"\n else\n echo \"$(pwd)/$1\"\n fi\n fi\n}\n\nif [ \"$#\" -ne 1 ]; then\n echo \"Usage: $0 LIDAR_SCAN_NUMBER\" >&2\n exit 1\nfi\n\nroscore &\nROSCORE_PID=$!\nsleep 1\n\nrviz -d ../rviz_cfg/aloam_velodyne.rviz &\nRVIZ_PID=$!\n\nA_LOAM_DIR=$(abspath \"..\")\n\nif [ \"$1\" -eq 16 ]; then\n docker run \\\n -it \\\n --rm \\\n --net=host \\\n -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \\\n ros:aloam \\\n /bin/bash -c \\\n \"cd /root/catkin_ws/; \\\n catkin config \\\n --cmake-args \\\n -DCMAKE_BUILD_TYPE=Release; \\\n catkin build; \\\n source devel/setup.bash; \\\n roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch rviz:=false\"\nelif [ \"$1\" -eq \"32\" ]; then\n docker run \\\n -it \\\n --rm \\\n --net=host \\\n -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \\\n ros:aloam \\\n /bin/bash -c \\\n \"cd /root/catkin_ws/; \\\n catkin config \\\n --cmake-args \\\n -DCMAKE_BUILD_TYPE=Release; \\\n catkin build; \\\n source devel/setup.bash; \\\n roslaunch aloam_velodyne aloam_velodyne_HDL_32.launch rviz:=false\"\nelif [ \"$1\" -eq \"64\" ]; then\n docker run \\\n -it \\\n --rm \\\n --net=host \\\n -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \\\n ros:aloam \\\n /bin/bash -c \\\n \"cd /root/catkin_ws/; \\\n catkin config \\\n --cmake-args \\\n -DCMAKE_BUILD_TYPE=Release; \\\n catkin build; \\\n source devel/setup.bash; \\\n roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch rviz:=false\"\nfi\n\nwait $ROSCORE_PID\nwait $RVIZ_PID\n\nif [[ $? -gt 128 ]]\nthen\n kill $ROSCORE_PID\n kill $RVIZ_PID\nfi" }, { "path": "SC-PGO/include/aloam_velodyne/common.h", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. \n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#pragma once\n\n#include \n\n#include \n\ntypedef pcl::PointXYZI PointType;\n\ninline double rad2deg(double radians)\n{\n return radians * 180.0 / M_PI;\n}\n\ninline double deg2rad(double degrees)\n{\n return degrees * M_PI / 180.0;\n}\n\nstruct Pose6D {\n double x;\n double y;\n double z;\n double roll;\n double pitch;\n double yaw;\n};" }, { "path": "SC-PGO/include/aloam_velodyne/tic_toc.h", "content": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#pragma once\n\n#include \n#include \n#include \n\nclass TicToc\n{\n public:\n TicToc()\n {\n tic();\n }\n\n void tic()\n {\n start = std::chrono::system_clock::now();\n }\n\n double toc()\n {\n end = std::chrono::system_clock::now();\n std::chrono::duration elapsed_seconds = end - start;\n return elapsed_seconds.count() * 1000;\n }\n\n private:\n std::chrono::time_point start, end;\n};\n\nclass TicTocV2\n{\npublic:\n TicTocV2()\n {\n tic();\n }\n\n TicTocV2( bool _disp )\n {\n disp_ = _disp;\n tic();\n }\n\n void tic()\n {\n start = std::chrono::system_clock::now();\n }\n\n void toc( std::string _about_task )\n {\n end = std::chrono::system_clock::now();\n std::chrono::duration elapsed_seconds = end - start;\n double elapsed_ms = elapsed_seconds.count() * 1000;\n\n if( disp_ )\n {\n std::cout.precision(3); // 10 for sec, 3 for ms \n std::cout << _about_task << \": \" << elapsed_ms << \" msec.\" << std::endl;\n }\n }\n\nprivate: \n std::chrono::time_point start, end;\n bool disp_ = false;\n};" }, { "path": "SC-PGO/include/scancontext/KDTreeVectorOfVectorsAdaptor.h", "content": "/***********************************************************************\n * Software License Agreement (BSD License)\n *\n * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n *\n * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR\n * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES\n * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.\n * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\n * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\n * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\n * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF\n * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n *************************************************************************/\n\n#pragma once\n\n#include \"scancontext/nanoflann.hpp\"\n\n#include \n\n// ===== This example shows how to use nanoflann with these types of containers: =======\n//typedef std::vector > my_vector_of_vectors_t;\n//typedef std::vector my_vector_of_vectors_t; // This requires #include \n// =====================================================================================\n\n\n/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.\n * The i'th vector represents a point in the state space.\n *\n * \\tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.\n * \\tparam num_t The type of the point coordinates (typically, double or float).\n * \\tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.\n * \\tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)\n */\ntemplate \nstruct KDTreeVectorOfVectorsAdaptor\n{\n\ttypedef KDTreeVectorOfVectorsAdaptor self_t;\n\ttypedef typename Distance::template traits::distance_t metric_t;\n\ttypedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t;\n\n\tindex_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.\n\n\t/// Constructor: takes a const ref to the vector of vectors object with the data points\n\tKDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat)\n\t{\n\t\tassert(mat.size() != 0 && mat[0].size() != 0);\n\t\tconst size_t dims = mat[0].size();\n\t\tif (DIM>0 && static_cast(dims) != DIM)\n\t\t\tthrow std::runtime_error(\"Data set dimensionality does not match the 'DIM' template argument\");\n\t\tindex = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );\n\t\tindex->buildIndex();\n\t}\n\n\t~KDTreeVectorOfVectorsAdaptor() {\n\t\tdelete index;\n\t}\n\n\tconst VectorOfVectorsType &m_data;\n\n\t/** Query for the \\a num_closest closest points to a given point (entered as query_point[0:dim-1]).\n\t * Note that this is a short-cut method for index->findNeighbors().\n\t * The user can also call index->... methods as desired.\n\t * \\note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.\n\t */\n\tinline 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\n\t{\n\t\tnanoflann::KNNResultSet resultSet(num_closest);\n\t\tresultSet.init(out_indices, out_distances_sq);\n\t\tindex->findNeighbors(resultSet, query_point, nanoflann::SearchParams());\n\t}\n\n\t/** @name Interface expected by KDTreeSingleIndexAdaptor\n\t * @{ */\n\n\tconst self_t & derived() const {\n\t\treturn *this;\n\t}\n\tself_t & derived() {\n\t\treturn *this;\n\t}\n\n\t// Must return the number of data points\n\tinline size_t kdtree_get_point_count() const {\n\t\treturn m_data.size();\n\t}\n\n\t// Returns the dim'th component of the idx'th point in the class:\n\tinline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {\n\t\treturn m_data[idx][dim];\n\t}\n\n\t// Optional bounding-box computation: return false to default to a standard bbox computation loop.\n\t// Return true if the BBOX was already computed by the class and returned in \"bb\" so it can be avoided to redo it again.\n\t// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)\n\ttemplate \n\tbool kdtree_get_bbox(BBOX & /*bb*/) const {\n\t\treturn false;\n\t}\n\n\t/** @} */\n\n}; // end of KDTreeVectorOfVectorsAdaptor\n" }, { "path": "SC-PGO/include/scancontext/Scancontext.cpp", "content": "#include \"scancontext/Scancontext.h\"\n\n// namespace SC2\n// {\n\nvoid coreImportTest (void)\n{\n cout << \"scancontext lib is successfully imported.\" << endl;\n} // coreImportTest\n\n\nfloat rad2deg(float radians)\n{\n return radians * 180.0 / M_PI;\n}\n\nfloat deg2rad(float degrees)\n{\n return degrees * M_PI / 180.0;\n}\n\n\nfloat xy2theta( const float & _x, const float & _y )\n{\n if ( (_x >= 0) & (_y >= 0)) \n return (180/M_PI) * atan(_y / _x);\n\n if ( (_x < 0) & (_y >= 0)) \n return 180 - ( (180/M_PI) * atan(_y / (-_x)) );\n\n if ( (_x < 0) & (_y < 0)) \n return 180 + ( (180/M_PI) * atan(_y / _x) );\n\n if ( (_x >= 0) & (_y < 0))\n return 360 - ( (180/M_PI) * atan((-_y) / _x) );\n} // xy2theta\n\n\nMatrixXd circshift( MatrixXd &_mat, int _num_shift )\n{\n // shift columns to right direction \n assert(_num_shift >= 0);\n\n if( _num_shift == 0 )\n {\n MatrixXd shifted_mat( _mat );\n return shifted_mat; // Early return \n }\n\n MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() );\n for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ )\n {\n int new_location = (col_idx + _num_shift) % _mat.cols();\n shifted_mat.col(new_location) = _mat.col(col_idx);\n }\n\n return shifted_mat;\n\n} // circshift\n\n\nstd::vector eig2stdvec( MatrixXd _eigmat )\n{\n std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() );\n return vec;\n} // eig2stdvec\n\n\ndouble SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 )\n{\n int num_eff_cols = 0; // i.e., to exclude all-nonzero sector\n double sum_sector_similarity = 0;\n for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ )\n {\n VectorXd col_sc1 = _sc1.col(col_idx);\n VectorXd col_sc2 = _sc2.col(col_idx);\n \n if( (col_sc1.norm() == 0) | (col_sc2.norm() == 0) )\n continue; // don't count this sector pair. \n\n double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());\n\n sum_sector_similarity = sum_sector_similarity + sector_similarity;\n num_eff_cols = num_eff_cols + 1;\n }\n \n double sc_sim = sum_sector_similarity / num_eff_cols;\n return 1.0 - sc_sim;\n\n} // distDirectSC\n\n\nint SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)\n{\n int argmin_vkey_shift = 0;\n double min_veky_diff_norm = 10000000;\n for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )\n {\n MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);\n\n MatrixXd vkey_diff = _vkey1 - vkey2_shifted;\n\n double cur_diff_norm = vkey_diff.norm();\n if( cur_diff_norm < min_veky_diff_norm )\n {\n argmin_vkey_shift = shift_idx;\n min_veky_diff_norm = cur_diff_norm;\n }\n }\n\n return argmin_vkey_shift;\n\n} // fastAlignUsingVkey\n\n\nstd::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )\n{\n // 1. fast align using variant key (not in original IROS18)\n MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );\n MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );\n int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );\n\n const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range \n std::vector shift_idx_search_space { argmin_vkey_shift };\n for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )\n {\n shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );\n shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );\n }\n std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());\n\n // 2. fast columnwise diff \n int argmin_shift = 0;\n double min_sc_dist = 10000000;\n for ( int num_shift: shift_idx_search_space )\n {\n MatrixXd sc2_shifted = circshift(_sc2, num_shift);\n double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );\n if( cur_sc_dist < min_sc_dist )\n {\n argmin_shift = num_shift;\n min_sc_dist = cur_sc_dist;\n }\n }\n\n return make_pair(min_sc_dist, argmin_shift);\n\n} // distanceBtnScanContext\n\n\nMatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down )\n{\n TicTocV2 t_making_desc;\n\n int num_pts_scan_down = _scan_down.points.size();\n\n // main\n const int NO_POINT = -1000;\n MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);\n\n SCPointType pt;\n float azim_angle, azim_range; // wihtin 2d plane\n int ring_idx, sctor_idx;\n for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)\n {\n pt.x = _scan_down.points[pt_idx].x; \n pt.y = _scan_down.points[pt_idx].y;\n pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).\n\n // xyz to ring, sector\n azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);\n azim_angle = xy2theta(pt.x, pt.y);\n\n // if range is out of roi, pass\n if( azim_range > PC_MAX_RADIUS )\n continue;\n\n ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );\n sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );\n\n // taking maximum z \n if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0\n desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin\n }\n\n // reset no points to zero (for cosine dist later)\n for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )\n for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )\n if( desc(row_idx, col_idx) == NO_POINT )\n desc(row_idx, col_idx) = 0;\n\n t_making_desc.toc(\"PolarContext making\");\n\n return desc;\n} // SCManager::makeScancontext\n\n\nMatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )\n{\n /* \n * summary: rowwise mean vector\n */\n Eigen::MatrixXd invariant_key(_desc.rows(), 1);\n for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )\n {\n Eigen::MatrixXd curr_row = _desc.row(row_idx);\n invariant_key(row_idx, 0) = curr_row.mean();\n }\n\n return invariant_key;\n} // SCManager::makeRingkeyFromScancontext\n\n\nMatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )\n{\n /* \n * summary: columnwise mean vector\n */\n Eigen::MatrixXd variant_key(1, _desc.cols());\n for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ )\n {\n Eigen::MatrixXd curr_col = _desc.col(col_idx);\n variant_key(0, col_idx) = curr_col.mean();\n }\n\n return variant_key;\n} // SCManager::makeSectorkeyFromScancontext\n\n\nconst Eigen::MatrixXd& SCManager::getConstRefRecentSCD(void)\n{\n return polarcontexts_.back();\n}\n\n\nvoid SCManager::saveScancontextAndKeys( Eigen::MatrixXd _scd )\n{\n Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( _scd );\n Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( _scd );\n std::vector polarcontext_invkey_vec = eig2stdvec( ringkey );\n\n polarcontexts_.push_back( _scd ); \n polarcontext_invkeys_.push_back( ringkey );\n polarcontext_vkeys_.push_back( sectorkey );\n polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );\n} // SCManager::makeAndSaveScancontextAndKeys\n\n\nvoid SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down )\n{\n Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 \n Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );\n Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );\n std::vector polarcontext_invkey_vec = eig2stdvec( ringkey );\n\n polarcontexts_.push_back( sc ); \n polarcontext_invkeys_.push_back( ringkey );\n polarcontext_vkeys_.push_back( sectorkey );\n polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );\n} // SCManager::makeAndSaveScancontextAndKeys\n\nvoid SCManager::setSCdistThres(double _new_thres)\n{\n SC_DIST_THRES = _new_thres;\n} // SCManager::setThres\n\nvoid SCManager::setMaximumRadius(double _max_r)\n{\n PC_MAX_RADIUS = _max_r;\n} // SCManager::setMaximumRadius\n\nstd::pair SCManager::detectLoopClosureIDBetweenSession (std::vector& _curr_key, Eigen::MatrixXd& _curr_desc)\n{\n int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable \"closestHistoryFrameID\")\n\n auto& curr_key = _curr_key;\n auto& curr_desc = _curr_desc; // current observation (query)\n\n // step 0: if first, construct the tree in batch\n if( ! is_tree_batch_made ) // run only once\n {\n polarcontext_invkeys_to_search_.clear();\n polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() ) ;\n\n polarcontext_tree_batch_.reset(); \n polarcontext_tree_batch_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );\n\n is_tree_batch_made = true; // for running this block only once\n }\n \n double min_dist = 10000000; // init with somthing large\n int nn_align = 0;\n int nn_idx = 0;\n\n // step 1: knn search\n std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); \n std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE );\n\n nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE );\n knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );\n polarcontext_tree_batch_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); // error here\n\n // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)\n TicTocV2 t_calc_dist; \n for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )\n {\n MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];\n std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); \n \n double candidate_dist = sc_dist_result.first;\n int candidate_align = sc_dist_result.second;\n\n if( candidate_dist < min_dist )\n {\n min_dist = candidate_dist;\n nn_align = candidate_align;\n\n nn_idx = candidate_indexes[candidate_iter_idx];\n }\n }\n t_calc_dist.toc(\"Distance calc\");\n\n // step 3: similarity threshold\n if( min_dist < SC_DIST_THRES )\n loop_id = nn_idx; \n\n // To do: return also nn_align (i.e., yaw diff)\n float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);\n std::pair result {loop_id, yaw_diff_rad};\n\n return result;\n\n} // SCManager::detectLoopClosureIDBetweenSession\n\n\nstd::pair SCManager::detectLoopClosureID ( void )\n{\n int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable \"closestHistoryFrameID\")\n\n auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)\n auto curr_desc = polarcontexts_.back(); // current observation (query)\n\n /* \n * step 1: candidates from ringkey tree_\n */\n if( (int)polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1)\n {\n std::pair result {loop_id, 0.0};\n return result; // Early return \n }\n\n // tree_ reconstruction (not mandatory to make everytime)\n if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost\n {\n TicTocV2 t_tree_construction;\n\n polarcontext_invkeys_to_search_.clear();\n polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;\n\n polarcontext_tree_.reset(); \n polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );\n // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)\n t_tree_construction.toc(\"Tree construction\");\n }\n tree_making_period_conter = tree_making_period_conter + 1;\n \n double min_dist = 10000000; // init with somthing large\n int nn_align = 0;\n int nn_idx = 0;\n\n // knn search\n std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); \n std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE );\n\n TicTocV2 t_tree_search;\n nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE );\n knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );\n polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); \n t_tree_search.toc(\"Tree search\");\n\n /* \n * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)\n */\n TicTocV2 t_calc_dist; \n for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )\n {\n MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];\n std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); \n \n double candidate_dist = sc_dist_result.first;\n int candidate_align = sc_dist_result.second;\n\n if( candidate_dist < min_dist )\n {\n min_dist = candidate_dist;\n nn_align = candidate_align;\n\n nn_idx = candidate_indexes[candidate_iter_idx];\n }\n }\n t_calc_dist.toc(\"Distance calc\");\n\n /* \n * loop threshold check\n */\n if( min_dist < SC_DIST_THRES )\n {\n loop_id = nn_idx; \n \n // std::cout.precision(3); \n cout << \"[Loop found] Nearest distance: \" << min_dist << \" btn \" << polarcontexts_.size()-1 << \" and \" << nn_idx << \".\" << endl;\n // cout << \"[Loop found] yaw diff: \" << nn_align * PC_UNIT_SECTORANGLE << \" deg.\" << endl;\n }\n else\n {\n std::cout.precision(3); \n cout << \"[Not loop] Nearest distance: \" << min_dist << \" btn \" << polarcontexts_.size()-1 << \" and \" << nn_idx << \".\" << endl;\n // cout << \"[Not loop] yaw diff: \" << nn_align * PC_UNIT_SECTORANGLE << \" deg.\" << endl;\n }\n\n // To do: return also nn_align (i.e., yaw diff)\n float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);\n std::pair result {loop_id, yaw_diff_rad};\n\n return result;\n\n} // SCManager::detectLoopClosureID\n\n// } // namespace SC2" }, { "path": "SC-PGO/include/scancontext/Scancontext.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n#include \"scancontext/nanoflann.hpp\"\n#include \"scancontext/KDTreeVectorOfVectorsAdaptor.h\"\n#include \"aloam_velodyne/tic_toc.h\"\n\nusing namespace Eigen;\nusing namespace nanoflann;\n\nusing std::cout;\nusing std::endl;\nusing std::make_pair;\n\nusing std::atan2;\nusing std::cos;\nusing std::sin;\n\nusing 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)\nusing KeyMat = std::vector >;\nusing InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >;\n\n\n// namespace SC2\n// {\n\nvoid coreImportTest ( void );\n\n\n// sc param-independent helper functions \nfloat xy2theta( const float & _x, const float & _y );\nMatrixXd circshift( MatrixXd &_mat, int _num_shift );\nstd::vector eig2stdvec( MatrixXd _eigmat );\n\n\nclass SCManager\n{\npublic: \n SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care.\n\n Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down );\n Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc );\n Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc );\n\n int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); \n double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // \"d\" (eq 5) in the original paper (IROS 18)\n std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // \"D\" (eq 6) in the original paper (IROS 18)\n\n // User-side API\n void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down );\n std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw \n\n // for ltslam \n // User-side API for multi-session\n void saveScancontextAndKeys( Eigen::MatrixXd _scd );\n std::pair detectLoopClosureIDBetweenSession ( std::vector& curr_key, Eigen::MatrixXd& curr_desc);\n\n const Eigen::MatrixXd& getConstRefRecentSCD(void);\n\npublic:\n // hyper parameters ()\n 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.\n\n const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18)\n const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)\n double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)\n const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);\n const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING);\n\n // tree\n const int NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. \n const int NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper)\n\n // loop thres\n 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.\n // 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)\n\n 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\n // 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\n\n // config \n 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.\n int tree_making_period_conter = 0;\n\n // setter\n void setSCdistThres(double _new_thres);\n void setMaximumRadius(double _max_r);\n\n // data \n std::vector polarcontexts_timestamp_; // optional.\n std::vector polarcontexts_;\n std::vector polarcontext_invkeys_;\n std::vector polarcontext_vkeys_;\n\n KeyMat polarcontext_invkeys_mat_;\n KeyMat polarcontext_invkeys_to_search_;\n std::unique_ptr polarcontext_tree_;\n\n bool is_tree_batch_made = false;\n std::unique_ptr polarcontext_tree_batch_;\n\n}; // SCManager\n\n// } // namespace SC2\n" }, { "path": "SC-PGO/include/scancontext/nanoflann.hpp", "content": "/***********************************************************************\n * Software License Agreement (BSD License)\n *\n * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.\n * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.\n * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com).\n * All rights reserved.\n *\n * THE BSD LICENSE\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions\n * are met:\n *\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n *\n * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR\n * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES\n * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.\n * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,\n * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\n * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,\n * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY\n * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF\n * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n *************************************************************************/\n\n/** \\mainpage nanoflann C++ API documentation\n * nanoflann is a C++ header-only library for building KD-Trees, mostly\n * optimized for 2D or 3D point clouds.\n *\n * nanoflann does not require compiling or installing, just an\n * #include in your code.\n *\n * See:\n * - C++ API organized by modules\n * - Online README\n * - Doxygen\n * documentation\n */\n\n#ifndef NANOFLANN_HPP_\n#define NANOFLANN_HPP_\n\n#include \n#include \n#include \n#include // for abs()\n#include // for fwrite()\n#include // for abs()\n#include \n#include // std::reference_wrapper\n#include \n#include \n\n/** Library version: 0xMmP (M=Major,m=minor,P=patch) */\n#define NANOFLANN_VERSION 0x132\n\n// Avoid conflicting declaration of min/max macros in windows headers\n#if !defined(NOMINMAX) && \\\n (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))\n#define NOMINMAX\n#ifdef max\n#undef max\n#undef min\n#endif\n#endif\n\nnamespace nanoflann {\n/** @addtogroup nanoflann_grp nanoflann C++ library for ANN\n * @{ */\n\n/** the PI constant (required to avoid MSVC missing symbols) */\ntemplate T pi_const() {\n return static_cast(3.14159265358979323846);\n}\n\n/**\n * Traits if object is resizable and assignable (typically has a resize | assign\n * method)\n */\ntemplate struct has_resize : std::false_type {};\n\ntemplate \nstruct has_resize().resize(1), 0)>\n : std::true_type {};\n\ntemplate struct has_assign : std::false_type {};\n\ntemplate \nstruct has_assign().assign(1, 0), 0)>\n : std::true_type {};\n\n/**\n * Free function to resize a resizable object\n */\ntemplate \ninline typename std::enable_if::value, void>::type\nresize(Container &c, const size_t nElements) {\n c.resize(nElements);\n}\n\n/**\n * Free function that has no effects on non resizable containers (e.g.\n * std::array) It raises an exception if the expected size does not match\n */\ntemplate \ninline typename std::enable_if::value, void>::type\nresize(Container &c, const size_t nElements) {\n if (nElements != c.size())\n throw std::logic_error(\"Try to change the size of a std::array.\");\n}\n\n/**\n * Free function to assign to a container\n */\ntemplate \ninline typename std::enable_if::value, void>::type\nassign(Container &c, const size_t nElements, const T &value) {\n c.assign(nElements, value);\n}\n\n/**\n * Free function to assign to a std::array\n */\ntemplate \ninline typename std::enable_if::value, void>::type\nassign(Container &c, const size_t nElements, const T &value) {\n for (size_t i = 0; i < nElements; i++)\n c[i] = value;\n}\n\n/** @addtogroup result_sets_grp Result set classes\n * @{ */\ntemplate \nclass KNNResultSet {\npublic:\n typedef _DistanceType DistanceType;\n typedef _IndexType IndexType;\n typedef _CountType CountType;\n\nprivate:\n IndexType *indices;\n DistanceType *dists;\n CountType capacity;\n CountType count;\n\npublic:\n inline KNNResultSet(CountType capacity_)\n : indices(0), dists(0), capacity(capacity_), count(0) {}\n\n inline void init(IndexType *indices_, DistanceType *dists_) {\n indices = indices_;\n dists = dists_;\n count = 0;\n if (capacity)\n dists[capacity - 1] = (std::numeric_limits::max)();\n }\n\n inline CountType size() const { return count; }\n\n inline bool full() const { return count == capacity; }\n\n /**\n * Called during search to add an element matching the criteria.\n * @return true if the search should be continued, false if the results are\n * sufficient\n */\n inline bool addPoint(DistanceType dist, IndexType index) {\n CountType i;\n for (i = count; i > 0; --i) {\n#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same\n // distance, the one with the lowest-index will be\n // returned first.\n if ((dists[i - 1] > dist) ||\n ((dist == dists[i - 1]) && (indices[i - 1] > index))) {\n#else\n if (dists[i - 1] > dist) {\n#endif\n if (i < capacity) {\n dists[i] = dists[i - 1];\n indices[i] = indices[i - 1];\n }\n } else\n break;\n }\n if (i < capacity) {\n dists[i] = dist;\n indices[i] = index;\n }\n if (count < capacity)\n count++;\n\n // tell caller that the search shall continue\n return true;\n }\n\n inline DistanceType worstDist() const { return dists[capacity - 1]; }\n};\n\n/** operator \"<\" for std::sort() */\nstruct IndexDist_Sorter {\n /** PairType will be typically: std::pair */\n template \n inline bool operator()(const PairType &p1, const PairType &p2) const {\n return p1.second < p2.second;\n }\n};\n\n/**\n * A result-set class used when performing a radius based search.\n */\ntemplate \nclass RadiusResultSet {\npublic:\n typedef _DistanceType DistanceType;\n typedef _IndexType IndexType;\n\npublic:\n const DistanceType radius;\n\n std::vector> &m_indices_dists;\n\n inline RadiusResultSet(\n DistanceType radius_,\n std::vector> &indices_dists)\n : radius(radius_), m_indices_dists(indices_dists) {\n init();\n }\n\n inline void init() { clear(); }\n inline void clear() { m_indices_dists.clear(); }\n\n inline size_t size() const { return m_indices_dists.size(); }\n\n inline bool full() const { return true; }\n\n /**\n * Called during search to add an element matching the criteria.\n * @return true if the search should be continued, false if the results are\n * sufficient\n */\n inline bool addPoint(DistanceType dist, IndexType index) {\n if (dist < radius)\n m_indices_dists.push_back(std::make_pair(index, dist));\n return true;\n }\n\n inline DistanceType worstDist() const { return radius; }\n\n /**\n * Find the worst result (furtherest neighbor) without copying or sorting\n * Pre-conditions: size() > 0\n */\n std::pair worst_item() const {\n if (m_indices_dists.empty())\n throw std::runtime_error(\"Cannot invoke RadiusResultSet::worst_item() on \"\n \"an empty list of results.\");\n typedef\n typename std::vector>::const_iterator\n DistIt;\n DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(),\n IndexDist_Sorter());\n return *it;\n }\n};\n\n/** @} */\n\n/** @addtogroup loadsave_grp Load/save auxiliary functions\n * @{ */\ntemplate \nvoid save_value(FILE *stream, const T &value, size_t count = 1) {\n fwrite(&value, sizeof(value), count, stream);\n}\n\ntemplate \nvoid save_value(FILE *stream, const std::vector &value) {\n size_t size = value.size();\n fwrite(&size, sizeof(size_t), 1, stream);\n fwrite(&value[0], sizeof(T), size, stream);\n}\n\ntemplate \nvoid load_value(FILE *stream, T &value, size_t count = 1) {\n size_t read_cnt = fread(&value, sizeof(value), count, stream);\n if (read_cnt != count) {\n throw std::runtime_error(\"Cannot read from file\");\n }\n}\n\ntemplate void load_value(FILE *stream, std::vector &value) {\n size_t size;\n size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);\n if (read_cnt != 1) {\n throw std::runtime_error(\"Cannot read from file\");\n }\n value.resize(size);\n read_cnt = fread(&value[0], sizeof(T), size, stream);\n if (read_cnt != size) {\n throw std::runtime_error(\"Cannot read from file\");\n }\n}\n/** @} */\n\n/** @addtogroup metric_grp Metric (distance) classes\n * @{ */\n\nstruct Metric {};\n\n/** Manhattan distance functor (generic version, optimized for\n * high-dimensionality data sets). Corresponding distance traits:\n * nanoflann::metric_L1 \\tparam T Type of the elements (e.g. double, float,\n * uint8_t) \\tparam _DistanceType Type of distance variables (must be signed)\n * (e.g. float, double, int64_t)\n */\ntemplate \nstruct L1_Adaptor {\n typedef T ElementType;\n typedef _DistanceType DistanceType;\n\n const DataSource &data_source;\n\n L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}\n\n inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,\n DistanceType worst_dist = -1) const {\n DistanceType result = DistanceType();\n const T *last = a + size;\n const T *lastgroup = last - 3;\n size_t d = 0;\n\n /* Process 4 items with each loop for efficiency. */\n while (a < lastgroup) {\n const DistanceType diff0 =\n std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));\n const DistanceType diff1 =\n std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));\n const DistanceType diff2 =\n std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));\n const DistanceType diff3 =\n std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));\n result += diff0 + diff1 + diff2 + diff3;\n a += 4;\n if ((worst_dist > 0) && (result > worst_dist)) {\n return result;\n }\n }\n /* Process last 0-3 components. Not needed for standard vector lengths. */\n while (a < last) {\n result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));\n }\n return result;\n }\n\n template \n inline DistanceType accum_dist(const U a, const V b, const size_t) const {\n return std::abs(a - b);\n }\n};\n\n/** Squared Euclidean distance functor (generic version, optimized for\n * high-dimensionality data sets). Corresponding distance traits:\n * nanoflann::metric_L2 \\tparam T Type of the elements (e.g. double, float,\n * uint8_t) \\tparam _DistanceType Type of distance variables (must be signed)\n * (e.g. float, double, int64_t)\n */\ntemplate \nstruct L2_Adaptor {\n typedef T ElementType;\n typedef _DistanceType DistanceType;\n\n const DataSource &data_source;\n\n L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}\n\n inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,\n DistanceType worst_dist = -1) const {\n DistanceType result = DistanceType();\n const T *last = a + size;\n const T *lastgroup = last - 3;\n size_t d = 0;\n\n /* Process 4 items with each loop for efficiency. */\n while (a < lastgroup) {\n const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);\n const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);\n const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);\n const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);\n result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;\n a += 4;\n if ((worst_dist > 0) && (result > worst_dist)) {\n return result;\n }\n }\n /* Process last 0-3 components. Not needed for standard vector lengths. */\n while (a < last) {\n const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);\n result += diff0 * diff0;\n }\n return result;\n }\n\n template \n inline DistanceType accum_dist(const U a, const V b, const size_t) const {\n return (a - b) * (a - b);\n }\n};\n\n/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality\n * datasets, like 2D or 3D point clouds) Corresponding distance traits:\n * nanoflann::metric_L2_Simple \\tparam T Type of the elements (e.g. double,\n * float, uint8_t) \\tparam _DistanceType Type of distance variables (must be\n * signed) (e.g. float, double, int64_t)\n */\ntemplate \nstruct L2_Simple_Adaptor {\n typedef T ElementType;\n typedef _DistanceType DistanceType;\n\n const DataSource &data_source;\n\n L2_Simple_Adaptor(const DataSource &_data_source)\n : data_source(_data_source) {}\n\n inline DistanceType evalMetric(const T *a, const size_t b_idx,\n size_t size) const {\n DistanceType result = DistanceType();\n for (size_t i = 0; i < size; ++i) {\n const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i);\n result += diff * diff;\n }\n return result;\n }\n\n template \n inline DistanceType accum_dist(const U a, const V b, const size_t) const {\n return (a - b) * (a - b);\n }\n};\n\n/** SO2 distance functor\n * Corresponding distance traits: nanoflann::metric_SO2\n * \\tparam T Type of the elements (e.g. double, float)\n * \\tparam _DistanceType Type of distance variables (must be signed) (e.g.\n * float, double) orientation is constrained to be in [-pi, pi]\n */\ntemplate \nstruct SO2_Adaptor {\n typedef T ElementType;\n typedef _DistanceType DistanceType;\n\n const DataSource &data_source;\n\n SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}\n\n inline DistanceType evalMetric(const T *a, const size_t b_idx,\n size_t size) const {\n return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1),\n size - 1);\n }\n\n /** Note: this assumes that input angles are already in the range [-pi,pi] */\n template \n inline DistanceType accum_dist(const U a, const V b, const size_t) const {\n DistanceType result = DistanceType();\n DistanceType PI = pi_const();\n result = b - a;\n if (result > PI)\n result -= 2 * PI;\n else if (result < -PI)\n result += 2 * PI;\n return result;\n }\n};\n\n/** SO3 distance functor (Uses L2_Simple)\n * Corresponding distance traits: nanoflann::metric_SO3\n * \\tparam T Type of the elements (e.g. double, float)\n * \\tparam _DistanceType Type of distance variables (must be signed) (e.g.\n * float, double)\n */\ntemplate \nstruct SO3_Adaptor {\n typedef T ElementType;\n typedef _DistanceType DistanceType;\n\n L2_Simple_Adaptor distance_L2_Simple;\n\n SO3_Adaptor(const DataSource &_data_source)\n : distance_L2_Simple(_data_source) {}\n\n inline DistanceType evalMetric(const T *a, const size_t b_idx,\n size_t size) const {\n return distance_L2_Simple.evalMetric(a, b_idx, size);\n }\n\n template \n inline DistanceType accum_dist(const U a, const V b, const size_t idx) const {\n return distance_L2_Simple.accum_dist(a, b, idx);\n }\n};\n\n/** Metaprogramming helper traits class for the L1 (Manhattan) metric */\nstruct metric_L1 : public Metric {\n template struct traits {\n typedef L1_Adaptor distance_t;\n };\n};\n/** Metaprogramming helper traits class for the L2 (Euclidean) metric */\nstruct metric_L2 : public Metric {\n template struct traits {\n typedef L2_Adaptor distance_t;\n };\n};\n/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */\nstruct metric_L2_Simple : public Metric {\n template struct traits {\n typedef L2_Simple_Adaptor distance_t;\n };\n};\n/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */\nstruct metric_SO2 : public Metric {\n template struct traits {\n typedef SO2_Adaptor distance_t;\n };\n};\n/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */\nstruct metric_SO3 : public Metric {\n template struct traits {\n typedef SO3_Adaptor distance_t;\n };\n};\n\n/** @} */\n\n/** @addtogroup param_grp Parameter structs\n * @{ */\n\n/** Parameters (see README.md) */\nstruct KDTreeSingleIndexAdaptorParams {\n KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)\n : leaf_max_size(_leaf_max_size) {}\n\n size_t leaf_max_size;\n};\n\n/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */\nstruct SearchParams {\n /** Note: The first argument (checks_IGNORED_) is ignored, but kept for\n * compatibility with the FLANN interface */\n SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)\n : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}\n\n int checks; //!< Ignored parameter (Kept for compatibility with the FLANN\n //!< interface).\n float eps; //!< search for eps-approximate neighbours (default: 0)\n bool sorted; //!< only for radius search, require neighbours sorted by\n //!< distance (default: true)\n};\n/** @} */\n\n/** @addtogroup memalloc_grp Memory allocation\n * @{ */\n\n/**\n * Allocates (using C's malloc) a generic type T.\n *\n * Params:\n * count = number of instances to allocate.\n * Returns: pointer (of type T*) to memory buffer\n */\ntemplate inline T *allocate(size_t count = 1) {\n T *mem = static_cast(::malloc(sizeof(T) * count));\n return mem;\n}\n\n/**\n * Pooled storage allocator\n *\n * The following routines allow for the efficient allocation of storage in\n * small chunks from a specified pool. Rather than allowing each structure\n * to be freed individually, an entire pool of storage is freed at once.\n * This method has two advantages over just using malloc() and free(). First,\n * it is far more efficient for allocating small objects, as there is\n * no overhead for remembering all the information needed to free each\n * object or consolidating fragmented memory. Second, the decision about\n * how long to keep an object is made at the time of allocation, and there\n * is no need to track down all the objects to free them.\n *\n */\n\nconst size_t WORDSIZE = 16;\nconst size_t BLOCKSIZE = 8192;\n\nclass PooledAllocator {\n /* We maintain memory alignment to word boundaries by requiring that all\n allocations be in multiples of the machine wordsize. */\n /* Size of machine word in bytes. Must be power of 2. */\n /* Minimum number of bytes requested at a time from\tthe system. Must be\n * multiple of WORDSIZE. */\n\n size_t remaining; /* Number of bytes left in current block of storage. */\n void *base; /* Pointer to base of current block of storage. */\n void *loc; /* Current location in block to next allocate memory. */\n\n void internal_init() {\n remaining = 0;\n base = NULL;\n usedMemory = 0;\n wastedMemory = 0;\n }\n\npublic:\n size_t usedMemory;\n size_t wastedMemory;\n\n /**\n Default constructor. Initializes a new pool.\n */\n PooledAllocator() { internal_init(); }\n\n /**\n * Destructor. Frees all the memory allocated in this pool.\n */\n ~PooledAllocator() { free_all(); }\n\n /** Frees all allocated memory chunks */\n void free_all() {\n while (base != NULL) {\n void *prev =\n *(static_cast(base)); /* Get pointer to prev block. */\n ::free(base);\n base = prev;\n }\n internal_init();\n }\n\n /**\n * Returns a pointer to a piece of new memory of the given size in bytes\n * allocated from the pool.\n */\n void *malloc(const size_t req_size) {\n /* Round size up to a multiple of wordsize. The following expression\n only works for WORDSIZE that is a power of 2, by masking last bits of\n incremented size to zero.\n */\n const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);\n\n /* Check whether a new block must be allocated. Note that the first word\n of a block is reserved for a pointer to the previous block.\n */\n if (size > remaining) {\n\n wastedMemory += remaining;\n\n /* Allocate new storage. */\n const size_t blocksize =\n (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE)\n ? size + sizeof(void *) + (WORDSIZE - 1)\n : BLOCKSIZE;\n\n // use the standard C malloc to allocate memory\n void *m = ::malloc(blocksize);\n if (!m) {\n fprintf(stderr, \"Failed to allocate memory.\\n\");\n return NULL;\n }\n\n /* Fill first word of new block with pointer to previous block. */\n static_cast(m)[0] = base;\n base = m;\n\n size_t shift = 0;\n // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) &\n // (WORDSIZE-1))) & (WORDSIZE-1);\n\n remaining = blocksize - sizeof(void *) - shift;\n loc = (static_cast(m) + sizeof(void *) + shift);\n }\n void *rloc = loc;\n loc = static_cast(loc) + size;\n remaining -= size;\n\n usedMemory += size;\n\n return rloc;\n }\n\n /**\n * Allocates (using this pool) a generic type T.\n *\n * Params:\n * count = number of instances to allocate.\n * Returns: pointer (of type T*) to memory buffer\n */\n template T *allocate(const size_t count = 1) {\n T *mem = static_cast(this->malloc(sizeof(T) * count));\n return mem;\n }\n};\n/** @} */\n\n/** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff\n * @{ */\n\n/** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors\n * when DIM=-1. Fixed size version for a generic DIM:\n */\ntemplate struct array_or_vector_selector {\n typedef std::array container_t;\n};\n/** Dynamic size version */\ntemplate struct array_or_vector_selector<-1, T> {\n typedef std::vector container_t;\n};\n\n/** @} */\n\n/** kd-tree base-class\n *\n * Contains the member functions common to the classes KDTreeSingleIndexAdaptor\n * and KDTreeSingleIndexDynamicAdaptor_.\n *\n * \\tparam Derived The name of the class which inherits this class.\n * \\tparam DatasetAdaptor The user-provided adaptor (see comments above).\n * \\tparam Distance The distance metric to use, these are all classes derived\n * from nanoflann::Metric \\tparam DIM Dimensionality of data points (e.g. 3 for\n * 3D points) \\tparam IndexType Will be typically size_t or int\n */\n\ntemplate \nclass KDTreeBaseClass {\n\npublic:\n /** Frees the previously-built index. Automatically called within\n * buildIndex(). */\n void freeIndex(Derived &obj) {\n obj.pool.free_all();\n obj.root_node = NULL;\n obj.m_size_at_index_build = 0;\n }\n\n typedef typename Distance::ElementType ElementType;\n typedef typename Distance::DistanceType DistanceType;\n\n /*--------------------- Internal Data Structures --------------------------*/\n struct Node {\n /** Union used because a node can be either a LEAF node or a non-leaf node,\n * so both data fields are never used simultaneously */\n union {\n struct leaf {\n IndexType left, right; //!< Indices of points in leaf node\n } lr;\n struct nonleaf {\n int divfeat; //!< Dimension used for subdivision.\n DistanceType divlow, divhigh; //!< The values used for subdivision.\n } sub;\n } node_type;\n Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node)\n };\n\n typedef Node *NodePtr;\n\n struct Interval {\n ElementType low, high;\n };\n\n /**\n * Array of indices to vectors in the dataset.\n */\n std::vector vind;\n\n NodePtr root_node;\n\n size_t m_leaf_max_size;\n\n size_t m_size; //!< Number of current points in the dataset\n size_t m_size_at_index_build; //!< Number of points in the dataset when the\n //!< index was built\n int dim; //!< Dimensionality of each data point\n\n /** Define \"BoundingBox\" as a fixed-size or variable-size container depending\n * on \"DIM\" */\n typedef\n typename array_or_vector_selector::container_t BoundingBox;\n\n /** Define \"distance_vector_t\" as a fixed-size or variable-size container\n * depending on \"DIM\" */\n typedef typename array_or_vector_selector::container_t\n distance_vector_t;\n\n /** The KD-tree used to find neighbours */\n\n BoundingBox root_bbox;\n\n /**\n * Pooled memory allocator.\n *\n * Using a pooled memory allocator is more efficient\n * than allocating memory directly when there is a large\n * number small of memory allocations.\n */\n PooledAllocator pool;\n\n /** Returns number of points in dataset */\n size_t size(const Derived &obj) const { return obj.m_size; }\n\n /** Returns the length of each point in the dataset */\n size_t veclen(const Derived &obj) {\n return static_cast(DIM > 0 ? DIM : obj.dim);\n }\n\n /// Helper accessor to the dataset points:\n inline ElementType dataset_get(const Derived &obj, size_t idx,\n int component) const {\n return obj.dataset.kdtree_get_pt(idx, component);\n }\n\n /**\n * Computes the inde memory usage\n * Returns: memory used by the index\n */\n size_t usedMemory(Derived &obj) {\n return obj.pool.usedMemory + obj.pool.wastedMemory +\n obj.dataset.kdtree_get_point_count() *\n sizeof(IndexType); // pool memory and vind array memory\n }\n\n void computeMinMax(const Derived &obj, IndexType *ind, IndexType count,\n int element, ElementType &min_elem,\n ElementType &max_elem) {\n min_elem = dataset_get(obj, ind[0], element);\n max_elem = dataset_get(obj, ind[0], element);\n for (IndexType i = 1; i < count; ++i) {\n ElementType val = dataset_get(obj, ind[i], element);\n if (val < min_elem)\n min_elem = val;\n if (val > max_elem)\n max_elem = val;\n }\n }\n\n /**\n * Create a tree node that subdivides the list of vecs from vind[first]\n * to vind[last]. The routine is called recursively on each sublist.\n *\n * @param left index of the first vector\n * @param right index of the last vector\n */\n NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right,\n BoundingBox &bbox) {\n NodePtr node = obj.pool.template allocate(); // allocate memory\n\n /* If too few exemplars remain, then make this a leaf node. */\n if ((right - left) <= static_cast(obj.m_leaf_max_size)) {\n node->child1 = node->child2 = NULL; /* Mark as leaf node. */\n node->node_type.lr.left = left;\n node->node_type.lr.right = right;\n\n // compute bounding-box of leaf points\n for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {\n bbox[i].low = dataset_get(obj, obj.vind[left], i);\n bbox[i].high = dataset_get(obj, obj.vind[left], i);\n }\n for (IndexType k = left + 1; k < right; ++k) {\n for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {\n if (bbox[i].low > dataset_get(obj, obj.vind[k], i))\n bbox[i].low = dataset_get(obj, obj.vind[k], i);\n if (bbox[i].high < dataset_get(obj, obj.vind[k], i))\n bbox[i].high = dataset_get(obj, obj.vind[k], i);\n }\n }\n } else {\n IndexType idx;\n int cutfeat;\n DistanceType cutval;\n middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval,\n bbox);\n\n node->node_type.sub.divfeat = cutfeat;\n\n BoundingBox left_bbox(bbox);\n left_bbox[cutfeat].high = cutval;\n node->child1 = divideTree(obj, left, left + idx, left_bbox);\n\n BoundingBox right_bbox(bbox);\n right_bbox[cutfeat].low = cutval;\n node->child2 = divideTree(obj, left + idx, right, right_bbox);\n\n node->node_type.sub.divlow = left_bbox[cutfeat].high;\n node->node_type.sub.divhigh = right_bbox[cutfeat].low;\n\n for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {\n bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);\n bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);\n }\n }\n\n return node;\n }\n\n void middleSplit_(Derived &obj, IndexType *ind, IndexType count,\n IndexType &index, int &cutfeat, DistanceType &cutval,\n const BoundingBox &bbox) {\n const DistanceType EPS = static_cast(0.00001);\n ElementType max_span = bbox[0].high - bbox[0].low;\n for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) {\n ElementType span = bbox[i].high - bbox[i].low;\n if (span > max_span) {\n max_span = span;\n }\n }\n ElementType max_spread = -1;\n cutfeat = 0;\n for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {\n ElementType span = bbox[i].high - bbox[i].low;\n if (span > (1 - EPS) * max_span) {\n ElementType min_elem, max_elem;\n computeMinMax(obj, ind, count, i, min_elem, max_elem);\n ElementType spread = max_elem - min_elem;\n ;\n if (spread > max_spread) {\n cutfeat = i;\n max_spread = spread;\n }\n }\n }\n // split in the middle\n DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;\n ElementType min_elem, max_elem;\n computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem);\n\n if (split_val < min_elem)\n cutval = min_elem;\n else if (split_val > max_elem)\n cutval = max_elem;\n else\n cutval = split_val;\n\n IndexType lim1, lim2;\n planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2);\n\n if (lim1 > count / 2)\n index = lim1;\n else if (lim2 < count / 2)\n index = lim2;\n else\n index = count / 2;\n }\n\n /**\n * Subdivide the list of points by a plane perpendicular on axe corresponding\n * to the 'cutfeat' dimension at 'cutval' position.\n *\n * On return:\n * dataset[ind[0..lim1-1]][cutfeat]cutval\n */\n void planeSplit(Derived &obj, IndexType *ind, const IndexType count,\n int cutfeat, DistanceType &cutval, IndexType &lim1,\n IndexType &lim2) {\n /* Move vector indices for left subtree to front of list. */\n IndexType left = 0;\n IndexType right = count - 1;\n for (;;) {\n while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval)\n ++left;\n while (right && left <= right &&\n dataset_get(obj, ind[right], cutfeat) >= cutval)\n --right;\n if (left > right || !right)\n break; // \"!right\" was added to support unsigned Index types\n std::swap(ind[left], ind[right]);\n ++left;\n --right;\n }\n /* If either list is empty, it means that all remaining features\n * are identical. Split in the middle to maintain a balanced tree.\n */\n lim1 = left;\n right = count - 1;\n for (;;) {\n while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval)\n ++left;\n while (right && left <= right &&\n dataset_get(obj, ind[right], cutfeat) > cutval)\n --right;\n if (left > right || !right)\n break; // \"!right\" was added to support unsigned Index types\n std::swap(ind[left], ind[right]);\n ++left;\n --right;\n }\n lim2 = left;\n }\n\n DistanceType computeInitialDistances(const Derived &obj,\n const ElementType *vec,\n distance_vector_t &dists) const {\n assert(vec);\n DistanceType distsq = DistanceType();\n\n for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {\n if (vec[i] < obj.root_bbox[i].low) {\n dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i);\n distsq += dists[i];\n }\n if (vec[i] > obj.root_bbox[i].high) {\n dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i);\n distsq += dists[i];\n }\n }\n return distsq;\n }\n\n void save_tree(Derived &obj, FILE *stream, NodePtr tree) {\n save_value(stream, *tree);\n if (tree->child1 != NULL) {\n save_tree(obj, stream, tree->child1);\n }\n if (tree->child2 != NULL) {\n save_tree(obj, stream, tree->child2);\n }\n }\n\n void load_tree(Derived &obj, FILE *stream, NodePtr &tree) {\n tree = obj.pool.template allocate();\n load_value(stream, *tree);\n if (tree->child1 != NULL) {\n load_tree(obj, stream, tree->child1);\n }\n if (tree->child2 != NULL) {\n load_tree(obj, stream, tree->child2);\n }\n }\n\n /** Stores the index in a binary file.\n * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when\n * loading the index object it must be constructed associated to the same\n * source of data points used while building it. See the example:\n * examples/saveload_example.cpp \\sa loadIndex */\n void saveIndex_(Derived &obj, FILE *stream) {\n save_value(stream, obj.m_size);\n save_value(stream, obj.dim);\n save_value(stream, obj.root_bbox);\n save_value(stream, obj.m_leaf_max_size);\n save_value(stream, obj.vind);\n save_tree(obj, stream, obj.root_node);\n }\n\n /** Loads a previous index from a binary file.\n * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the\n * index object must be constructed associated to the same source of data\n * points used while building the index. See the example:\n * examples/saveload_example.cpp \\sa loadIndex */\n void loadIndex_(Derived &obj, FILE *stream) {\n load_value(stream, obj.m_size);\n load_value(stream, obj.dim);\n load_value(stream, obj.root_bbox);\n load_value(stream, obj.m_leaf_max_size);\n load_value(stream, obj.vind);\n load_tree(obj, stream, obj.root_node);\n }\n};\n\n/** @addtogroup kdtrees_grp KD-tree classes and adaptors\n * @{ */\n\n/** kd-tree static index\n *\n * Contains the k-d trees and other information for indexing a set of points\n * for nearest-neighbor matching.\n *\n * The class \"DatasetAdaptor\" must provide the following interface (can be\n * non-virtual, inlined methods):\n *\n * \\code\n * // Must return the number of data poins\n * inline size_t kdtree_get_point_count() const { ... }\n *\n *\n * // Must return the dim'th component of the idx'th point in the class:\n * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }\n *\n * // Optional bounding-box computation: return false to default to a standard\n * bbox computation loop.\n * // Return true if the BBOX was already computed by the class and returned\n * in \"bb\" so it can be avoided to redo it again.\n * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3\n * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const\n * {\n * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits\n * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits\n * ...\n * return true;\n * }\n *\n * \\endcode\n *\n * \\tparam DatasetAdaptor The user-provided adaptor (see comments above).\n * \\tparam Distance The distance metric to use: nanoflann::metric_L1,\n * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \\tparam DIM\n * Dimensionality of data points (e.g. 3 for 3D points) \\tparam IndexType Will\n * be typically size_t or int\n */\ntemplate \nclass KDTreeSingleIndexAdaptor\n : public KDTreeBaseClass<\n KDTreeSingleIndexAdaptor,\n Distance, DatasetAdaptor, DIM, IndexType> {\npublic:\n /** Deleted copy constructor*/\n KDTreeSingleIndexAdaptor(\n const KDTreeSingleIndexAdaptor\n &) = delete;\n\n /**\n * The dataset used by this index\n */\n const DatasetAdaptor &dataset; //!< The source of our data\n\n const KDTreeSingleIndexAdaptorParams index_params;\n\n Distance distance;\n\n typedef typename nanoflann::KDTreeBaseClass<\n nanoflann::KDTreeSingleIndexAdaptor,\n Distance, DatasetAdaptor, DIM, IndexType>\n BaseClassRef;\n\n typedef typename BaseClassRef::ElementType ElementType;\n typedef typename BaseClassRef::DistanceType DistanceType;\n\n typedef typename BaseClassRef::Node Node;\n typedef Node *NodePtr;\n\n typedef typename BaseClassRef::Interval Interval;\n /** Define \"BoundingBox\" as a fixed-size or variable-size container depending\n * on \"DIM\" */\n typedef typename BaseClassRef::BoundingBox BoundingBox;\n\n /** Define \"distance_vector_t\" as a fixed-size or variable-size container\n * depending on \"DIM\" */\n typedef typename BaseClassRef::distance_vector_t distance_vector_t;\n\n /**\n * KDTree constructor\n *\n * Refer to docs in README.md or online in\n * https://github.com/jlblancoc/nanoflann\n *\n * The KD-Tree point dimension (the length of each point in the datase, e.g. 3\n * for 3D points) is determined by means of:\n * - The \\a DIM template parameter if >0 (highest priority)\n * - Otherwise, the \\a dimensionality parameter of this constructor.\n *\n * @param inputData Dataset with the input features\n * @param params Basically, the maximum leaf node size\n */\n KDTreeSingleIndexAdaptor(const int dimensionality,\n const DatasetAdaptor &inputData,\n const KDTreeSingleIndexAdaptorParams ¶ms =\n KDTreeSingleIndexAdaptorParams())\n : dataset(inputData), index_params(params), distance(inputData) {\n BaseClassRef::root_node = NULL;\n BaseClassRef::m_size = dataset.kdtree_get_point_count();\n BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;\n BaseClassRef::dim = dimensionality;\n if (DIM > 0)\n BaseClassRef::dim = DIM;\n BaseClassRef::m_leaf_max_size = params.leaf_max_size;\n\n // Create a permutable array of indices to the input vectors.\n init_vind();\n }\n\n /**\n * Builds the index\n */\n void buildIndex() {\n BaseClassRef::m_size = dataset.kdtree_get_point_count();\n BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;\n init_vind();\n this->freeIndex(*this);\n BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;\n if (BaseClassRef::m_size == 0)\n return;\n computeBoundingBox(BaseClassRef::root_bbox);\n BaseClassRef::root_node =\n this->divideTree(*this, 0, BaseClassRef::m_size,\n BaseClassRef::root_bbox); // construct the tree\n }\n\n /** \\name Query methods\n * @{ */\n\n /**\n * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored\n * inside the result object.\n *\n * Params:\n * result = the result object in which the indices of the\n * nearest-neighbors are stored vec = the vector for which to search the\n * nearest neighbors\n *\n * \\tparam RESULTSET Should be any ResultSet\n * \\return True if the requested neighbors could be found.\n * \\sa knnSearch, radiusSearch\n */\n template \n bool findNeighbors(RESULTSET &result, const ElementType *vec,\n const SearchParams &searchParams) const {\n assert(vec);\n if (this->size(*this) == 0)\n return false;\n if (!BaseClassRef::root_node)\n throw std::runtime_error(\n \"[nanoflann] findNeighbors() called before building the index.\");\n float epsError = 1 + searchParams.eps;\n\n distance_vector_t\n dists; // fixed or variable-sized container (depending on DIM)\n auto zero = static_cast(0);\n assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),\n zero); // Fill it with zeros.\n DistanceType distsq = this->computeInitialDistances(*this, vec, dists);\n searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,\n epsError); // \"count_leaf\" parameter removed since was neither\n // used nor returned to the user.\n return result.full();\n }\n\n /**\n * Find the \"num_closest\" nearest neighbors to the \\a query_point[0:dim-1].\n * Their indices are stored inside the result object. \\sa radiusSearch,\n * findNeighbors \\note nChecks_IGNORED is ignored but kept for compatibility\n * with the original FLANN interface. \\return Number `N` of valid points in\n * the result set. Only the first `N` entries in `out_indices` and\n * `out_distances_sq` will be valid. Return may be less than `num_closest`\n * only if the number of elements in the tree is less than `num_closest`.\n */\n size_t knnSearch(const ElementType *query_point, const size_t num_closest,\n IndexType *out_indices, DistanceType *out_distances_sq,\n const int /* nChecks_IGNORED */ = 10) const {\n nanoflann::KNNResultSet resultSet(num_closest);\n resultSet.init(out_indices, out_distances_sq);\n this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());\n return resultSet.size();\n }\n\n /**\n * Find all the neighbors to \\a query_point[0:dim-1] within a maximum radius.\n * The output is given as a vector of pairs, of which the first element is a\n * point index and the second the corresponding distance. Previous contents of\n * \\a IndicesDists are cleared.\n *\n * If searchParams.sorted==true, the output list is sorted by ascending\n * distances.\n *\n * For a better performance, it is advisable to do a .reserve() on the vector\n * if you have any wild guess about the number of expected matches.\n *\n * \\sa knnSearch, findNeighbors, radiusSearchCustomCallback\n * \\return The number of points within the given radius (i.e. indices.size()\n * or dists.size() )\n */\n size_t\n radiusSearch(const ElementType *query_point, const DistanceType &radius,\n std::vector> &IndicesDists,\n const SearchParams &searchParams) const {\n RadiusResultSet resultSet(radius, IndicesDists);\n const size_t nFound =\n radiusSearchCustomCallback(query_point, resultSet, searchParams);\n if (searchParams.sorted)\n std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());\n return nFound;\n }\n\n /**\n * Just like radiusSearch() but with a custom callback class for each point\n * found in the radius of the query. See the source of RadiusResultSet<> as a\n * start point for your own classes. \\sa radiusSearch\n */\n template \n size_t radiusSearchCustomCallback(\n const ElementType *query_point, SEARCH_CALLBACK &resultSet,\n const SearchParams &searchParams = SearchParams()) const {\n this->findNeighbors(resultSet, query_point, searchParams);\n return resultSet.size();\n }\n\n /** @} */\n\npublic:\n /** Make sure the auxiliary list \\a vind has the same size than the current\n * dataset, and re-generate if size has changed. */\n void init_vind() {\n // Create a permutable array of indices to the input vectors.\n BaseClassRef::m_size = dataset.kdtree_get_point_count();\n if (BaseClassRef::vind.size() != BaseClassRef::m_size)\n BaseClassRef::vind.resize(BaseClassRef::m_size);\n for (size_t i = 0; i < BaseClassRef::m_size; i++)\n BaseClassRef::vind[i] = i;\n }\n\n void computeBoundingBox(BoundingBox &bbox) {\n resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));\n if (dataset.kdtree_get_bbox(bbox)) {\n // Done! It was implemented in derived class\n } else {\n const size_t N = dataset.kdtree_get_point_count();\n if (!N)\n throw std::runtime_error(\"[nanoflann] computeBoundingBox() called but \"\n \"no data points found.\");\n for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {\n bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i);\n }\n for (size_t k = 1; k < N; ++k) {\n for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {\n if (this->dataset_get(*this, k, i) < bbox[i].low)\n bbox[i].low = this->dataset_get(*this, k, i);\n if (this->dataset_get(*this, k, i) > bbox[i].high)\n bbox[i].high = this->dataset_get(*this, k, i);\n }\n }\n }\n }\n\n /**\n * Performs an exact search in the tree starting from a node.\n * \\tparam RESULTSET Should be any ResultSet\n * \\return true if the search should be continued, false if the results are\n * sufficient\n */\n template \n bool searchLevel(RESULTSET &result_set, const ElementType *vec,\n const NodePtr node, DistanceType mindistsq,\n distance_vector_t &dists, const float epsError) const {\n /* If this is a leaf node, then do check and return. */\n if ((node->child1 == NULL) && (node->child2 == NULL)) {\n // count_leaf += (node->lr.right-node->lr.left); // Removed since was\n // neither used nor returned to the user.\n DistanceType worst_dist = result_set.worstDist();\n for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;\n ++i) {\n const IndexType index = BaseClassRef::vind[i]; // reorder... : i;\n DistanceType dist = distance.evalMetric(\n vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));\n if (dist < worst_dist) {\n if (!result_set.addPoint(dist, BaseClassRef::vind[i])) {\n // the resultset doesn't want to receive any more points, we're done\n // searching!\n return false;\n }\n }\n }\n return true;\n }\n\n /* Which child branch should be taken first? */\n int idx = node->node_type.sub.divfeat;\n ElementType val = vec[idx];\n DistanceType diff1 = val - node->node_type.sub.divlow;\n DistanceType diff2 = val - node->node_type.sub.divhigh;\n\n NodePtr bestChild;\n NodePtr otherChild;\n DistanceType cut_dist;\n if ((diff1 + diff2) < 0) {\n bestChild = node->child1;\n otherChild = node->child2;\n cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);\n } else {\n bestChild = node->child2;\n otherChild = node->child1;\n cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);\n }\n\n /* Call recursively to search next level down. */\n if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) {\n // the resultset doesn't want to receive any more points, we're done\n // searching!\n return false;\n }\n\n DistanceType dst = dists[idx];\n mindistsq = mindistsq + cut_dist - dst;\n dists[idx] = cut_dist;\n if (mindistsq * epsError <= result_set.worstDist()) {\n if (!searchLevel(result_set, vec, otherChild, mindistsq, dists,\n epsError)) {\n // the resultset doesn't want to receive any more points, we're done\n // searching!\n return false;\n }\n }\n dists[idx] = dst;\n return true;\n }\n\npublic:\n /** Stores the index in a binary file.\n * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when\n * loading the index object it must be constructed associated to the same\n * source of data points used while building it. See the example:\n * examples/saveload_example.cpp \\sa loadIndex */\n void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }\n\n /** Loads a previous index from a binary file.\n * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the\n * index object must be constructed associated to the same source of data\n * points used while building the index. See the example:\n * examples/saveload_example.cpp \\sa loadIndex */\n void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }\n\n}; // class KDTree\n\n/** kd-tree dynamic index\n *\n * Contains the k-d trees and other information for indexing a set of points\n * for nearest-neighbor matching.\n *\n * The class \"DatasetAdaptor\" must provide the following interface (can be\n * non-virtual, inlined methods):\n *\n * \\code\n * // Must return the number of data poins\n * inline size_t kdtree_get_point_count() const { ... }\n *\n * // Must return the dim'th component of the idx'th point in the class:\n * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }\n *\n * // Optional bounding-box computation: return false to default to a standard\n * bbox computation loop.\n * // Return true if the BBOX was already computed by the class and returned\n * in \"bb\" so it can be avoided to redo it again.\n * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3\n * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const\n * {\n * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits\n * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits\n * ...\n * return true;\n * }\n *\n * \\endcode\n *\n * \\tparam DatasetAdaptor The user-provided adaptor (see comments above).\n * \\tparam Distance The distance metric to use: nanoflann::metric_L1,\n * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \\tparam DIM\n * Dimensionality of data points (e.g. 3 for 3D points) \\tparam IndexType Will\n * be typically size_t or int\n */\ntemplate \nclass KDTreeSingleIndexDynamicAdaptor_\n : public KDTreeBaseClass,\n Distance, DatasetAdaptor, DIM, IndexType> {\npublic:\n /**\n * The dataset used by this index\n */\n const DatasetAdaptor &dataset; //!< The source of our data\n\n KDTreeSingleIndexAdaptorParams index_params;\n\n std::vector &treeIndex;\n\n Distance distance;\n\n typedef typename nanoflann::KDTreeBaseClass<\n nanoflann::KDTreeSingleIndexDynamicAdaptor_,\n Distance, DatasetAdaptor, DIM, IndexType>\n BaseClassRef;\n\n typedef typename BaseClassRef::ElementType ElementType;\n typedef typename BaseClassRef::DistanceType DistanceType;\n\n typedef typename BaseClassRef::Node Node;\n typedef Node *NodePtr;\n\n typedef typename BaseClassRef::Interval Interval;\n /** Define \"BoundingBox\" as a fixed-size or variable-size container depending\n * on \"DIM\" */\n typedef typename BaseClassRef::BoundingBox BoundingBox;\n\n /** Define \"distance_vector_t\" as a fixed-size or variable-size container\n * depending on \"DIM\" */\n typedef typename BaseClassRef::distance_vector_t distance_vector_t;\n\n /**\n * KDTree constructor\n *\n * Refer to docs in README.md or online in\n * https://github.com/jlblancoc/nanoflann\n *\n * The KD-Tree point dimension (the length of each point in the datase, e.g. 3\n * for 3D points) is determined by means of:\n * - The \\a DIM template parameter if >0 (highest priority)\n * - Otherwise, the \\a dimensionality parameter of this constructor.\n *\n * @param inputData Dataset with the input features\n * @param params Basically, the maximum leaf node size\n */\n KDTreeSingleIndexDynamicAdaptor_(\n const int dimensionality, const DatasetAdaptor &inputData,\n std::vector &treeIndex_,\n const KDTreeSingleIndexAdaptorParams ¶ms =\n KDTreeSingleIndexAdaptorParams())\n : dataset(inputData), index_params(params), treeIndex(treeIndex_),\n distance(inputData) {\n BaseClassRef::root_node = NULL;\n BaseClassRef::m_size = 0;\n BaseClassRef::m_size_at_index_build = 0;\n BaseClassRef::dim = dimensionality;\n if (DIM > 0)\n BaseClassRef::dim = DIM;\n BaseClassRef::m_leaf_max_size = params.leaf_max_size;\n }\n\n /** Assignment operator definiton */\n KDTreeSingleIndexDynamicAdaptor_\n operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) {\n KDTreeSingleIndexDynamicAdaptor_ tmp(rhs);\n std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind);\n std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size);\n std::swap(index_params, tmp.index_params);\n std::swap(treeIndex, tmp.treeIndex);\n std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size);\n std::swap(BaseClassRef::m_size_at_index_build,\n tmp.BaseClassRef::m_size_at_index_build);\n std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node);\n std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox);\n std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool);\n return *this;\n }\n\n /**\n * Builds the index\n */\n void buildIndex() {\n BaseClassRef::m_size = BaseClassRef::vind.size();\n this->freeIndex(*this);\n BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;\n if (BaseClassRef::m_size == 0)\n return;\n computeBoundingBox(BaseClassRef::root_bbox);\n BaseClassRef::root_node =\n this->divideTree(*this, 0, BaseClassRef::m_size,\n BaseClassRef::root_bbox); // construct the tree\n }\n\n /** \\name Query methods\n * @{ */\n\n /**\n * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored\n * inside the result object.\n *\n * Params:\n * result = the result object in which the indices of the\n * nearest-neighbors are stored vec = the vector for which to search the\n * nearest neighbors\n *\n * \\tparam RESULTSET Should be any ResultSet\n * \\return True if the requested neighbors could be found.\n * \\sa knnSearch, radiusSearch\n */\n template \n bool findNeighbors(RESULTSET &result, const ElementType *vec,\n const SearchParams &searchParams) const {\n assert(vec);\n if (this->size(*this) == 0)\n return false;\n if (!BaseClassRef::root_node)\n return false;\n float epsError = 1 + searchParams.eps;\n\n // fixed or variable-sized container (depending on DIM)\n distance_vector_t dists;\n // Fill it with zeros.\n assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),\n static_cast(0));\n DistanceType distsq = this->computeInitialDistances(*this, vec, dists);\n searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,\n epsError); // \"count_leaf\" parameter removed since was neither\n // used nor returned to the user.\n return result.full();\n }\n\n /**\n * Find the \"num_closest\" nearest neighbors to the \\a query_point[0:dim-1].\n * Their indices are stored inside the result object. \\sa radiusSearch,\n * findNeighbors \\note nChecks_IGNORED is ignored but kept for compatibility\n * with the original FLANN interface. \\return Number `N` of valid points in\n * the result set. Only the first `N` entries in `out_indices` and\n * `out_distances_sq` will be valid. Return may be less than `num_closest`\n * only if the number of elements in the tree is less than `num_closest`.\n */\n size_t knnSearch(const ElementType *query_point, const size_t num_closest,\n IndexType *out_indices, DistanceType *out_distances_sq,\n const int /* nChecks_IGNORED */ = 10) const {\n nanoflann::KNNResultSet resultSet(num_closest);\n resultSet.init(out_indices, out_distances_sq);\n this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());\n return resultSet.size();\n }\n\n /**\n * Find all the neighbors to \\a query_point[0:dim-1] within a maximum radius.\n * The output is given as a vector of pairs, of which the first element is a\n * point index and the second the corresponding distance. Previous contents of\n * \\a IndicesDists are cleared.\n *\n * If searchParams.sorted==true, the output list is sorted by ascending\n * distances.\n *\n * For a better performance, it is advisable to do a .reserve() on the vector\n * if you have any wild guess about the number of expected matches.\n *\n * \\sa knnSearch, findNeighbors, radiusSearchCustomCallback\n * \\return The number of points within the given radius (i.e. indices.size()\n * or dists.size() )\n */\n size_t\n radiusSearch(const ElementType *query_point, const DistanceType &radius,\n std::vector> &IndicesDists,\n const SearchParams &searchParams) const {\n RadiusResultSet resultSet(radius, IndicesDists);\n const size_t nFound =\n radiusSearchCustomCallback(query_point, resultSet, searchParams);\n if (searchParams.sorted)\n std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());\n return nFound;\n }\n\n /**\n * Just like radiusSearch() but with a custom callback class for each point\n * found in the radius of the query. See the source of RadiusResultSet<> as a\n * start point for your own classes. \\sa radiusSearch\n */\n template \n size_t radiusSearchCustomCallback(\n const ElementType *query_point, SEARCH_CALLBACK &resultSet,\n const SearchParams &searchParams = SearchParams()) const {\n this->findNeighbors(resultSet, query_point, searchParams);\n return resultSet.size();\n }\n\n /** @} */\n\npublic:\n void computeBoundingBox(BoundingBox &bbox) {\n resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));\n\n if (dataset.kdtree_get_bbox(bbox)) {\n // Done! It was implemented in derived class\n } else {\n const size_t N = BaseClassRef::m_size;\n if (!N)\n throw std::runtime_error(\"[nanoflann] computeBoundingBox() called but \"\n \"no data points found.\");\n for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {\n bbox[i].low = bbox[i].high =\n this->dataset_get(*this, BaseClassRef::vind[0], i);\n }\n for (size_t k = 1; k < N; ++k) {\n for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {\n if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low)\n bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i);\n if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high)\n bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i);\n }\n }\n }\n }\n\n /**\n * Performs an exact search in the tree starting from a node.\n * \\tparam RESULTSET Should be any ResultSet\n */\n template \n void searchLevel(RESULTSET &result_set, const ElementType *vec,\n const NodePtr node, DistanceType mindistsq,\n distance_vector_t &dists, const float epsError) const {\n /* If this is a leaf node, then do check and return. */\n if ((node->child1 == NULL) && (node->child2 == NULL)) {\n // count_leaf += (node->lr.right-node->lr.left); // Removed since was\n // neither used nor returned to the user.\n DistanceType worst_dist = result_set.worstDist();\n for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;\n ++i) {\n const IndexType index = BaseClassRef::vind[i]; // reorder... : i;\n if (treeIndex[index] == -1)\n continue;\n DistanceType dist = distance.evalMetric(\n vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));\n if (dist < worst_dist) {\n if (!result_set.addPoint(\n static_cast(dist),\n static_cast(\n BaseClassRef::vind[i]))) {\n // the resultset doesn't want to receive any more points, we're done\n // searching!\n return; // false;\n }\n }\n }\n return;\n }\n\n /* Which child branch should be taken first? */\n int idx = node->node_type.sub.divfeat;\n ElementType val = vec[idx];\n DistanceType diff1 = val - node->node_type.sub.divlow;\n DistanceType diff2 = val - node->node_type.sub.divhigh;\n\n NodePtr bestChild;\n NodePtr otherChild;\n DistanceType cut_dist;\n if ((diff1 + diff2) < 0) {\n bestChild = node->child1;\n otherChild = node->child2;\n cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);\n } else {\n bestChild = node->child2;\n otherChild = node->child1;\n cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);\n }\n\n /* Call recursively to search next level down. */\n searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);\n\n DistanceType dst = dists[idx];\n mindistsq = mindistsq + cut_dist - dst;\n dists[idx] = cut_dist;\n if (mindistsq * epsError <= result_set.worstDist()) {\n searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);\n }\n dists[idx] = dst;\n }\n\npublic:\n /** Stores the index in a binary file.\n * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when\n * loading the index object it must be constructed associated to the same\n * source of data points used while building it. See the example:\n * examples/saveload_example.cpp \\sa loadIndex */\n void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }\n\n /** Loads a previous index from a binary file.\n * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the\n * index object must be constructed associated to the same source of data\n * points used while building the index. See the example:\n * examples/saveload_example.cpp \\sa loadIndex */\n void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }\n};\n\n/** kd-tree dynaimic index\n *\n * class to create multiple static index and merge their results to behave as\n * single dynamic index as proposed in Logarithmic Approach.\n *\n * Example of usage:\n * examples/dynamic_pointcloud_example.cpp\n *\n * \\tparam DatasetAdaptor The user-provided adaptor (see comments above).\n * \\tparam Distance The distance metric to use: nanoflann::metric_L1,\n * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \\tparam DIM\n * Dimensionality of data points (e.g. 3 for 3D points) \\tparam IndexType Will\n * be typically size_t or int\n */\ntemplate \nclass KDTreeSingleIndexDynamicAdaptor {\npublic:\n typedef typename Distance::ElementType ElementType;\n typedef typename Distance::DistanceType DistanceType;\n\nprotected:\n size_t m_leaf_max_size;\n size_t treeCount;\n size_t pointCount;\n\n /**\n * The dataset used by this index\n */\n const DatasetAdaptor &dataset; //!< The source of our data\n\n std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which\n //!< point at idx is stored. treeIndex[idx]=-1\n //!< means that point has been removed.\n\n KDTreeSingleIndexAdaptorParams index_params;\n\n int dim; //!< Dimensionality of each data point\n\n typedef KDTreeSingleIndexDynamicAdaptor_\n index_container_t;\n std::vector index;\n\npublic:\n /** Get a const ref to the internal list of indices; the number of indices is\n * adapted dynamically as the dataset grows in size. */\n const std::vector &getAllIndices() const { return index; }\n\nprivate:\n /** finds position of least significant unset bit */\n int First0Bit(IndexType num) {\n int pos = 0;\n while (num & 1) {\n num = num >> 1;\n pos++;\n }\n return pos;\n }\n\n /** Creates multiple empty trees to handle dynamic support */\n void init() {\n typedef KDTreeSingleIndexDynamicAdaptor_\n my_kd_tree_t;\n std::vector index_(\n treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params));\n index = index_;\n }\n\npublic:\n Distance distance;\n\n /**\n * KDTree constructor\n *\n * Refer to docs in README.md or online in\n * https://github.com/jlblancoc/nanoflann\n *\n * The KD-Tree point dimension (the length of each point in the datase, e.g. 3\n * for 3D points) is determined by means of:\n * - The \\a DIM template parameter if >0 (highest priority)\n * - Otherwise, the \\a dimensionality parameter of this constructor.\n *\n * @param inputData Dataset with the input features\n * @param params Basically, the maximum leaf node size\n */\n KDTreeSingleIndexDynamicAdaptor(const int dimensionality,\n const DatasetAdaptor &inputData,\n const KDTreeSingleIndexAdaptorParams ¶ms =\n KDTreeSingleIndexAdaptorParams(),\n const size_t maximumPointCount = 1000000000U)\n : dataset(inputData), index_params(params), distance(inputData) {\n treeCount = static_cast(std::log2(maximumPointCount));\n pointCount = 0U;\n dim = dimensionality;\n treeIndex.clear();\n if (DIM > 0)\n dim = DIM;\n m_leaf_max_size = params.leaf_max_size;\n init();\n const size_t num_initial_points = dataset.kdtree_get_point_count();\n if (num_initial_points > 0) {\n addPoints(0, num_initial_points - 1);\n }\n }\n\n /** Deleted copy constructor*/\n KDTreeSingleIndexDynamicAdaptor(\n const KDTreeSingleIndexDynamicAdaptor &) = delete;\n\n /** Add points to the set, Inserts all points from [start, end] */\n void addPoints(IndexType start, IndexType end) {\n size_t count = end - start + 1;\n treeIndex.resize(treeIndex.size() + count);\n for (IndexType idx = start; idx <= end; idx++) {\n int pos = First0Bit(pointCount);\n index[pos].vind.clear();\n treeIndex[pointCount] = pos;\n for (int i = 0; i < pos; i++) {\n for (int j = 0; j < static_cast(index[i].vind.size()); j++) {\n index[pos].vind.push_back(index[i].vind[j]);\n if (treeIndex[index[i].vind[j]] != -1)\n treeIndex[index[i].vind[j]] = pos;\n }\n index[i].vind.clear();\n index[i].freeIndex(index[i]);\n }\n index[pos].vind.push_back(idx);\n index[pos].buildIndex();\n pointCount++;\n }\n }\n\n /** Remove a point from the set (Lazy Deletion) */\n void removePoint(size_t idx) {\n if (idx >= pointCount)\n return;\n treeIndex[idx] = -1;\n }\n\n /**\n * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored\n * inside the result object.\n *\n * Params:\n * result = the result object in which the indices of the\n * nearest-neighbors are stored vec = the vector for which to search the\n * nearest neighbors\n *\n * \\tparam RESULTSET Should be any ResultSet\n * \\return True if the requested neighbors could be found.\n * \\sa knnSearch, radiusSearch\n */\n template \n bool findNeighbors(RESULTSET &result, const ElementType *vec,\n const SearchParams &searchParams) const {\n for (size_t i = 0; i < treeCount; i++) {\n index[i].findNeighbors(result, &vec[0], searchParams);\n }\n return result.full();\n }\n};\n\n/** An L2-metric KD-tree adaptor for working with data directly stored in an\n * Eigen Matrix, without duplicating the data storage. Each row in the matrix\n * represents a point in the state space.\n *\n * Example of usage:\n * \\code\n * \tEigen::Matrix mat;\n * \t// Fill out \"mat\"...\n *\n * \ttypedef KDTreeEigenMatrixAdaptor< Eigen::Matrix >\n * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf\n * ); mat_index.index->buildIndex(); mat_index.index->... \\endcode\n *\n * \\tparam DIM If set to >0, it specifies a compile-time fixed dimensionality\n * for the points in the data set, allowing more compiler optimizations. \\tparam\n * Distance The distance metric to use: nanoflann::metric_L1,\n * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.\n */\ntemplate \nstruct KDTreeEigenMatrixAdaptor {\n typedef KDTreeEigenMatrixAdaptor self_t;\n typedef typename MatrixType::Scalar num_t;\n typedef typename MatrixType::Index IndexType;\n typedef\n typename Distance::template traits::distance_t metric_t;\n typedef KDTreeSingleIndexAdaptor\n index_t;\n\n index_t *index; //! The kd-tree index for the user to call its methods as\n //! usual with any other FLANN index.\n\n /// Constructor: takes a const ref to the matrix object with the data points\n KDTreeEigenMatrixAdaptor(const size_t dimensionality,\n const std::reference_wrapper &mat,\n const int leaf_max_size = 10)\n : m_data_matrix(mat) {\n const auto dims = mat.get().cols();\n if (size_t(dims) != dimensionality)\n throw std::runtime_error(\n \"Error: 'dimensionality' must match column count in data matrix\");\n if (DIM > 0 && int(dims) != DIM)\n throw std::runtime_error(\n \"Data set dimensionality does not match the 'DIM' template argument\");\n index =\n new index_t(static_cast(dims), *this /* adaptor */,\n nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));\n index->buildIndex();\n }\n\npublic:\n /** Deleted copy constructor */\n KDTreeEigenMatrixAdaptor(const self_t &) = delete;\n\n ~KDTreeEigenMatrixAdaptor() { delete index; }\n\n const std::reference_wrapper m_data_matrix;\n\n /** Query for the \\a num_closest closest points to a given point (entered as\n * query_point[0:dim-1]). Note that this is a short-cut method for\n * index->findNeighbors(). The user can also call index->... methods as\n * desired. \\note nChecks_IGNORED is ignored but kept for compatibility with\n * the original FLANN interface.\n */\n inline void query(const num_t *query_point, const size_t num_closest,\n IndexType *out_indices, num_t *out_distances_sq,\n const int /* nChecks_IGNORED */ = 10) const {\n nanoflann::KNNResultSet resultSet(num_closest);\n resultSet.init(out_indices, out_distances_sq);\n index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());\n }\n\n /** @name Interface expected by KDTreeSingleIndexAdaptor\n * @{ */\n\n const self_t &derived() const { return *this; }\n self_t &derived() { return *this; }\n\n // Must return the number of data points\n inline size_t kdtree_get_point_count() const {\n return m_data_matrix.get().rows();\n }\n\n // Returns the dim'th component of the idx'th point in the class:\n inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const {\n return m_data_matrix.get().coeff(idx, IndexType(dim));\n }\n\n // Optional bounding-box computation: return false to default to a standard\n // bbox computation loop.\n // Return true if the BBOX was already computed by the class and returned in\n // \"bb\" so it can be avoided to redo it again. Look at bb.size() to find out\n // the expected dimensionality (e.g. 2 or 3 for point clouds)\n template bool kdtree_get_bbox(BBOX & /*bb*/) const {\n return false;\n }\n\n /** @} */\n\n}; // end of KDTreeEigenMatrixAdaptor\n /** @} */\n\n/** @} */ // end of grouping\n} // namespace nanoflann\n\n#endif /* NANOFLANN_HPP_ */\n" }, { "path": "SC-PGO/launch/aloam_mulran.launch", "content": "\n \n \n\n \n \n\n \n \n\n \n \n\n \n \n\n \n \n \n \n\n \n \n \n\n \n \n\n \n \n \n \n \n\n \n \n \n \n \n\n\n" }, { "path": "SC-PGO/launch/aloam_velodyne_HDL_32.launch", "content": "\n \n \n\n \n \n\n \n \n\n \n \n\n \n \n\n \n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n \n\n\n" }, { "path": "SC-PGO/launch/aloam_velodyne_HDL_64.launch", "content": "\n \n \n\n \n \n\n \n \n\n \n \n\n \n \n\n \n \n \n \n\n \n \n \n\n \n \n \n \n\n \n \n \n \n\n\n" }, { "path": "SC-PGO/launch/aloam_velodyne_VLP_16.launch", "content": "\n \n \n\n \n \n\n \n \n\n \n \n\n \n\n \n \n \n\n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n\n\n" }, { "path": "SC-PGO/launch/fastlio_ouster64.launch", "content": "\n\n \n\n \n \n\n \n \n\n \n \n\n \n\n \n \n\n \n \n \n \n\n \n \n\n \n \n \n \n\n \n \n\n \n \n\n \n \n \n \n \n\n\n" }, { "path": "SC-PGO/launch/kitti_helper.launch", "content": "\n \n\n \n \n \n \n \n \n \n" }, { "path": "SC-PGO/package.xml", "content": "\n\n aloam_velodyne\n 0.0.0\n\n \n This is an advanced implentation of LOAM.\n LOAM is described in the following paper:\n J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n \n\n qintong\n\n BSD\n\n Ji Zhang\n \n catkin\n geometry_msgs\n nav_msgs\n roscpp\n rospy\n std_msgs \n rosbag\n sensor_msgs\n tf\n image_transport\n \n geometry_msgs\n nav_msgs\n sensor_msgs\n roscpp\n rospy\n std_msgs\n rosbag\n tf\n image_transport\n\n \n \n\n" }, { "path": "SC-PGO/rviz_cfg/aloam_velodyne.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /odometry1/odomPath1\n - /pgoOdom1/Shape1\n - /pgoMap1\n Splitter Ratio: 0.440559446811676\n Tree Height: 787\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: pgoMap\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: pgo Views\n Splitter Ratio: 0.5\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 160\n Reference Frame: \n Value: false\n - Class: rviz/Axes\n Enabled: true\n Length: 1\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: camera_init\n Value: true\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: gtPathlc\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /path_gt\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: gtPathLidar\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /path_gt_lidar\n Unreliable: false\n Value: true\n Enabled: false\n Name: GT\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 204; 0; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.20000000298023224\n Name: odomPath\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /laser_odom_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 63.10047912597656\n Min Color: 0; 0; 0\n Min Intensity: -0.0067862942814826965\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Flat Squares\n Topic: /velodyne_cloud_2\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n Enabled: true\n Name: odometry\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 4; 69; 246\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.30000001192092896\n Name: mappingPath\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /aft_mapped_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 50.14332962036133\n Min Color: 0; 0; 0\n Min Intensity: -0.04392780363559723\n Name: allMapCloud\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 4\n Size (m): 0.20000000298023224\n Style: Points\n Topic: /laser_cloud_map\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: false\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 15\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: surround\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.05000000074505806\n Style: Squares\n Topic: /laser_cloud_surround\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 50.141441345214844\n Min Color: 0; 0; 0\n Min Intensity: -0.024373823776841164\n Name: currPoints\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 1.5\n Size (m): 0.10000000149011612\n Style: Squares\n Topic: /velodyne_cloud_registered\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: false\n Keep: 1\n Name: Odometry\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1.5\n Axes Radius: 0.5\n Color: 255; 25; 0\n Head Length: 0.10000000149011612\n Head Radius: 2\n Shaft Length: 0.10000000149011612\n Shaft Radius: 20\n Value: Axes\n Topic: /aft_mapped_to_init\n Unreliable: false\n Value: false\n - Class: rviz/TF\n Enabled: false\n Frame Timeout: 15\n Frames:\n All Enabled: true\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n {}\n Update Interval: 0\n Value: false\n Enabled: true\n Name: mapping\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.4000000059604645\n Name: pgoPath\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: Axes\n Radius: 0.10000000149011612\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /aft_pgo_path\n Unreliable: false\n Value: true\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: true\n Keep: 1\n Name: pgoOdom\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 2\n Axes Radius: 0.699999988079071\n Color: 255; 25; 0\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /aft_pgo_odom\n Unreliable: false\n Value: true\n - Alpha: 0.10000000149011612\n Autocompute Intensity Bounds: false\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: true\n Max Color: 255; 255; 255\n Max Intensity: 300\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: pgoMap\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 2\n Size (m): 0.4000000059604645\n Style: Points\n Topic: /aft_pgo_map\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 20.06364631652832\n Min Color: 0; 0; 0\n Min Intensity: 3.003571033477783\n Name: loop scan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.5\n Style: Spheres\n Topic: /loop_scan_local\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 32.84956741333008\n Min Value: -16.756437301635742\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 20.100271224975586\n Min Color: 0; 0; 0\n Min Intensity: 3.0008726119995117\n Name: loop submap\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 2\n Size (m): 0.20000000298023224\n Style: Spheres\n Topic: /loop_submap_local\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n Enabled: true\n Global Options:\n Background Color: 255; 255; 255\n Default Light: true\n Fixed Frame: camera_init\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 638.897705078125\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 191.27947998046875\n Y: 248.57839965820312\n Z: -12.774103164672852\n Focal Shape Fixed Size: false\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.5697963237762451\n Target Frame: aft_pgo\n Value: Orbit (rviz)\n Yaw: 3.1635923385620117\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1016\n Hide Left Dock: false\n Hide Right Dock: true\n QMainWindow State: 000000ff00000000fd0000000400000000000001af0000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002ef000000c30000000000000000fb0000001200700067006f00200056006900650077007300000002db000000d7000000a400ffffff000000010000010f00000267fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d000002670000000000fffffffaffffffff0100000002fb0000000a005600690065007700730000000000ffffffff0000010000fffffffb0000000a0056006900650077007300000006710000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007530000003efc0100000002fb0000000800540069006d0065000000000000000753000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000059e0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1875\n X: 1965\n Y: 27\n pgo Views:\n collapsed: false\n" }, { "path": "SC-PGO/src/kittiHelper.cpp", "content": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nstd::vector read_lidar_data(const std::string lidar_data_path)\n{\n std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);\n lidar_data_file.seekg(0, std::ios::end);\n const size_t num_elements = lidar_data_file.tellg() / sizeof(float);\n lidar_data_file.seekg(0, std::ios::beg);\n\n std::vector lidar_data_buffer(num_elements);\n lidar_data_file.read(reinterpret_cast(&lidar_data_buffer[0]), num_elements*sizeof(float));\n return lidar_data_buffer;\n}\n\nint main(int argc, char** argv)\n{\n ros::init(argc, argv, \"kitti_helper\");\n ros::NodeHandle n(\"~\");\n std::string dataset_folder, sequence_number, output_bag_file;\n n.getParam(\"dataset_folder\", dataset_folder);\n n.getParam(\"sequence_number\", sequence_number);\n std::cout << \"Reading sequence \" << sequence_number << \" from \" << dataset_folder << '\\n';\n bool to_bag;\n n.getParam(\"to_bag\", to_bag);\n if (to_bag)\n n.getParam(\"output_bag_file\", output_bag_file);\n int publish_delay;\n n.getParam(\"publish_delay\", publish_delay);\n publish_delay = publish_delay <= 0 ? 1 : publish_delay;\n\n ros::Publisher pub_laser_cloud = n.advertise(\"/velodyne_points\", 2);\n\n image_transport::ImageTransport it(n);\n image_transport::Publisher pub_image_left = it.advertise(\"/image_left\", 2);\n image_transport::Publisher pub_image_right = it.advertise(\"/image_right\", 2);\n\n ros::Publisher pubOdomGT = n.advertise (\"/odometry_gt\", 5);\n nav_msgs::Odometry odomGT;\n odomGT.header.frame_id = \"camera_init\";\n odomGT.child_frame_id = \"/ground_truth\";\n\n ros::Publisher pubPathGT = n.advertise (\"/path_gt\", 5);\n nav_msgs::Path pathGT;\n pathGT.header.frame_id = \"camera_init\";\n\n std::string timestamp_path = \"sequences/\" + sequence_number + \"/times.txt\";\n std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);\n\n std::string ground_truth_path = \"results/\" + sequence_number + \".txt\";\n std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);\n\n rosbag::Bag bag_out;\n if (to_bag)\n bag_out.open(output_bag_file, rosbag::bagmode::Write);\n \n Eigen::Matrix3d R_transform;\n R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;\n Eigen::Quaterniond q_transform(R_transform);\n\n std::string line;\n std::size_t line_num = 0;\n\n ros::Rate r(10.0 / publish_delay);\n while (std::getline(timestamp_file, line) && ros::ok())\n {\n float timestamp = stof(line);\n std::stringstream left_image_path, right_image_path;\n left_image_path << dataset_folder << \"sequences/\" + sequence_number + \"/image_0/\" << std::setfill('0') << std::setw(6) << line_num << \".png\";\n cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);\n right_image_path << dataset_folder << \"sequences/\" + sequence_number + \"/image_1/\" << std::setfill('0') << std::setw(6) << line_num << \".png\";\n cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);\n\n std::getline(ground_truth_file, line);\n std::stringstream pose_stream(line);\n std::string s;\n Eigen::Matrix gt_pose;\n for (std::size_t i = 0; i < 3; ++i)\n {\n for (std::size_t j = 0; j < 4; ++j)\n {\n std::getline(pose_stream, s, ' ');\n gt_pose(i, j) = stof(s);\n }\n }\n\n Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());\n Eigen::Quaterniond q = q_transform * q_w_i;\n q.normalize();\n Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();\n\n odomGT.header.stamp = ros::Time().fromSec(timestamp);\n odomGT.pose.pose.orientation.x = q.x();\n odomGT.pose.pose.orientation.y = q.y();\n odomGT.pose.pose.orientation.z = q.z();\n odomGT.pose.pose.orientation.w = q.w();\n odomGT.pose.pose.position.x = t(0);\n odomGT.pose.pose.position.y = t(1);\n odomGT.pose.pose.position.z = t(2);\n pubOdomGT.publish(odomGT);\n\n geometry_msgs::PoseStamped poseGT;\n poseGT.header = odomGT.header;\n poseGT.pose = odomGT.pose.pose;\n pathGT.header.stamp = odomGT.header.stamp;\n pathGT.poses.push_back(poseGT);\n pubPathGT.publish(pathGT);\n\n // read lidar point cloud\n std::stringstream lidar_data_path;\n lidar_data_path << dataset_folder << \"velodyne/sequences/\" + sequence_number + \"/velodyne/\" \n << std::setfill('0') << std::setw(6) << line_num << \".bin\";\n std::vector lidar_data = read_lidar_data(lidar_data_path.str());\n std::cout << \"totally \" << lidar_data.size() / 4.0 << \" points in this lidar frame \\n\";\n\n std::vector lidar_points;\n std::vector lidar_intensities;\n pcl::PointCloud laser_cloud;\n for (std::size_t i = 0; i < lidar_data.size(); i += 4)\n {\n lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);\n lidar_intensities.push_back(lidar_data[i+3]);\n\n pcl::PointXYZI point;\n point.x = lidar_data[i];\n point.y = lidar_data[i + 1];\n point.z = lidar_data[i + 2];\n point.intensity = lidar_data[i + 3];\n laser_cloud.push_back(point);\n }\n\n sensor_msgs::PointCloud2 laser_cloud_msg;\n pcl::toROSMsg(laser_cloud, laser_cloud_msg);\n laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);\n laser_cloud_msg.header.frame_id = \"camera_init\";\n pub_laser_cloud.publish(laser_cloud_msg);\n\n sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, \"mono8\", left_image).toImageMsg();\n sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, \"mono8\", right_image).toImageMsg();\n pub_image_left.publish(image_left_msg);\n pub_image_right.publish(image_right_msg);\n\n if (to_bag)\n {\n bag_out.write(\"/image_left\", ros::Time::now(), image_left_msg);\n bag_out.write(\"/image_right\", ros::Time::now(), image_right_msg);\n bag_out.write(\"/velodyne_points\", ros::Time::now(), laser_cloud_msg);\n bag_out.write(\"/path_gt\", ros::Time::now(), pathGT);\n bag_out.write(\"/odometry_gt\", ros::Time::now(), odomGT);\n }\n\n line_num ++;\n r.sleep();\n }\n bag_out.close();\n std::cout << \"Done \\n\";\n\n\n return 0;\n}" }, { "path": "SC-PGO/src/laserMapping.cpp", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. \n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"lidarFactor.hpp\"\n#include \"aloam_velodyne/common.h\"\n#include \"aloam_velodyne/tic_toc.h\"\n\n\nint frameCount = 0;\n\ndouble timeLaserCloudCornerLast = 0;\ndouble timeLaserCloudSurfLast = 0;\ndouble timeLaserCloudFullRes = 0;\ndouble timeLaserOdometry = 0;\n\n\nint laserCloudCenWidth = 10;\nint laserCloudCenHeight = 10;\nint laserCloudCenDepth = 5;\nconst int laserCloudWidth = 21;\nconst int laserCloudHeight = 21;\nconst int laserCloudDepth = 11;\n\n\nconst int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851\n\n\nint laserCloudValidInd[125];\nint laserCloudSurroundInd[125];\n\n// input: from odom\npcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud());\npcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud());\n\n// ouput: all visualble cube points\npcl::PointCloud::Ptr laserCloudSurround(new pcl::PointCloud());\n\n// surround points in map to build tree\npcl::PointCloud::Ptr laserCloudCornerFromMap(new pcl::PointCloud());\npcl::PointCloud::Ptr laserCloudSurfFromMap(new pcl::PointCloud());\n\n//input & output: points in one frame. local --> global\npcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());\n\n// points in every cube\npcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum];\npcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum];\n\n//kd-tree\npcl::KdTreeFLANN::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN());\npcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN());\n\ndouble parameters[7] = {0, 0, 0, 1, 0, 0, 0};\nEigen::Map q_w_curr(parameters);\nEigen::Map t_w_curr(parameters + 4);\n\n// wmap_T_odom * odom_T_curr = wmap_T_curr;\n// transformation between odom's world and map's world frame\nEigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);\nEigen::Vector3d t_wmap_wodom(0, 0, 0);\n\nEigen::Quaterniond q_wodom_curr(1, 0, 0, 0);\nEigen::Vector3d t_wodom_curr(0, 0, 0);\n\n\nstd::queue cornerLastBuf;\nstd::queue surfLastBuf;\nstd::queue fullResBuf;\nstd::queue odometryBuf;\nstd::mutex mBuf;\n\npcl::VoxelGrid downSizeFilterCorner;\npcl::VoxelGrid downSizeFilterSurf;\n\nstd::vector pointSearchInd;\nstd::vector pointSearchSqDis;\n\nPointType pointOri, pointSel;\n\nros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped, pubOdomAftMappedHighFrec, pubLaserAfterMappedPath;\nros::Publisher pubLaserCloudFullResLocal;\n\nnav_msgs::Path laserAfterMappedPath;\n\n// set initial guess\nvoid transformAssociateToMap()\n{\n\tq_w_curr = q_wmap_wodom * q_wodom_curr;\n\tt_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;\n}\n\nvoid transformUpdate()\n{\n\tq_wmap_wodom = q_w_curr * q_wodom_curr.inverse();\n\tt_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;\n}\n\nvoid pointAssociateToMap(PointType const *const pi, PointType *const po)\n{\n\tEigen::Vector3d point_curr(pi->x, pi->y, pi->z);\n\tEigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;\n\tpo->x = point_w.x();\n\tpo->y = point_w.y();\n\tpo->z = point_w.z();\n\tpo->intensity = pi->intensity;\n\t//po->intensity = 1.0;\n}\n\nvoid pointAssociateTobeMapped(PointType const *const pi, PointType *const po)\n{\n\tEigen::Vector3d point_w(pi->x, pi->y, pi->z);\n\tEigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);\n\tpo->x = point_curr.x();\n\tpo->y = point_curr.y();\n\tpo->z = point_curr.z();\n\tpo->intensity = pi->intensity;\n}\n\nvoid laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)\n{\n\tmBuf.lock();\n\tcornerLastBuf.push(laserCloudCornerLast2);\n\tmBuf.unlock();\n}\n\nvoid laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)\n{\n\tmBuf.lock();\n\tsurfLastBuf.push(laserCloudSurfLast2);\n\tmBuf.unlock();\n}\n\nvoid laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)\n{\n\tmBuf.lock();\n\tfullResBuf.push(laserCloudFullRes2);\n\tmBuf.unlock();\n}\n\n//receive odomtry\nvoid laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)\n{\n\tmBuf.lock();\n\todometryBuf.push(laserOdometry);\n\tmBuf.unlock();\n\n\t// high frequence publish\n\tEigen::Quaterniond q_wodom_curr;\n\tEigen::Vector3d t_wodom_curr;\n\tq_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;\n\tq_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;\n\tq_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;\n\tq_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;\n\tt_wodom_curr.x() = laserOdometry->pose.pose.position.x;\n\tt_wodom_curr.y() = laserOdometry->pose.pose.position.y;\n\tt_wodom_curr.z() = laserOdometry->pose.pose.position.z;\n\n\tEigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;\n\tEigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; \n\n\tnav_msgs::Odometry odomAftMapped;\n\todomAftMapped.header.frame_id = \"camera_init\";\n\todomAftMapped.child_frame_id = \"/aft_mapped\";\n\todomAftMapped.header.stamp = laserOdometry->header.stamp;\n\todomAftMapped.pose.pose.orientation.x = q_w_curr.x();\n\todomAftMapped.pose.pose.orientation.y = q_w_curr.y();\n\todomAftMapped.pose.pose.orientation.z = q_w_curr.z();\n\todomAftMapped.pose.pose.orientation.w = q_w_curr.w();\n\todomAftMapped.pose.pose.position.x = t_w_curr.x();\n\todomAftMapped.pose.pose.position.y = t_w_curr.y();\n\todomAftMapped.pose.pose.position.z = t_w_curr.z();\n\tpubOdomAftMappedHighFrec.publish(odomAftMapped);\n}\n\nvoid process()\n{\n\twhile(1)\n\t{\n\t\twhile (!cornerLastBuf.empty() && !surfLastBuf.empty() &&\n\t\t\t!fullResBuf.empty() && !odometryBuf.empty())\n\t\t{\n\t\t\tmBuf.lock();\n\t\t\twhile (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())\n\t\t\t\todometryBuf.pop();\n\t\t\tif (odometryBuf.empty())\n\t\t\t{\n\t\t\t\tmBuf.unlock();\n\t\t\t\tbreak;\n\t\t\t}\n\n\t\t\twhile (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())\n\t\t\t\tsurfLastBuf.pop();\n\t\t\tif (surfLastBuf.empty())\n\t\t\t{\n\t\t\t\tmBuf.unlock();\n\t\t\t\tbreak;\n\t\t\t}\n\n\t\t\twhile (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())\n\t\t\t\tfullResBuf.pop();\n\t\t\tif (fullResBuf.empty())\n\t\t\t{\n\t\t\t\tmBuf.unlock();\n\t\t\t\tbreak;\n\t\t\t}\n\n\t\t\ttimeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();\n\t\t\ttimeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();\n\t\t\ttimeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();\n\t\t\ttimeLaserOdometry = odometryBuf.front()->header.stamp.toSec();\n\n\t\t\tif (timeLaserCloudCornerLast != timeLaserOdometry ||\n\t\t\t\ttimeLaserCloudSurfLast != timeLaserOdometry ||\n\t\t\t\ttimeLaserCloudFullRes != timeLaserOdometry)\n\t\t\t{\n\t\t\t\t//printf(\"time corner %f surf %f full %f odom %f \\n\", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);\n\t\t\t\t//printf(\"unsync messeage!\");\n\t\t\t\tmBuf.unlock();\n\t\t\t\tbreak;\n\t\t\t}\n\n\t\t\tlaserCloudCornerLast->clear();\n\t\t\tpcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);\n\t\t\tcornerLastBuf.pop();\n\n\t\t\tlaserCloudSurfLast->clear();\n\t\t\tpcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);\n\t\t\tsurfLastBuf.pop();\n\n\t\t\tlaserCloudFullRes->clear();\n\t\t\tpcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);\n\t\t\tfullResBuf.pop();\n\n\t\t\tq_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;\n\t\t\tq_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;\n\t\t\tq_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;\n\t\t\tq_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;\n\t\t\tt_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;\n\t\t\tt_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;\n\t\t\tt_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;\n\t\t\todometryBuf.pop();\n\n\t\t\twhile(!cornerLastBuf.empty())\n\t\t\t{\n\t\t\t\tcornerLastBuf.pop();\n\t\t\t\t//printf(\"drop lidar frame in mapping for real time performance \\n\");\n\t\t\t}\n\n\t\t\tmBuf.unlock();\n\n\t\t\tTicToc t_whole;\n\n\t\t\ttransformAssociateToMap();\n\n\t\t\tTicToc t_shift;\n\t\t\tint centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;\n\t\t\tint centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;\n\t\t\tint centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;\n\n\t\t\tif (t_w_curr.x() + 25.0 < 0)\n\t\t\t\tcenterCubeI--;\n\t\t\tif (t_w_curr.y() + 25.0 < 0)\n\t\t\t\tcenterCubeJ--;\n\t\t\tif (t_w_curr.z() + 25.0 < 0)\n\t\t\t\tcenterCubeK--;\n\n\t\t\twhile (centerCubeI < 3)\n\t\t\t{\n\t\t\t\tfor (int j = 0; j < laserCloudHeight; j++)\n\t\t\t\t{\n\t\t\t\t\tfor (int k = 0; k < laserCloudDepth; k++)\n\t\t\t\t\t{ \n\t\t\t\t\t\tint i = laserCloudWidth - 1;\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; \n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tfor (; i >= 1; i--)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t}\n\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeCornerPointer;\n\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeSurfPointer;\n\t\t\t\t\t\tlaserCloudCubeCornerPointer->clear();\n\t\t\t\t\t\tlaserCloudCubeSurfPointer->clear();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tcenterCubeI++;\n\t\t\t\tlaserCloudCenWidth++;\n\t\t\t}\n\n\t\t\twhile (centerCubeI >= laserCloudWidth - 3)\n\t\t\t{ \n\t\t\t\tfor (int j = 0; j < laserCloudHeight; j++)\n\t\t\t\t{\n\t\t\t\t\tfor (int k = 0; k < laserCloudDepth; k++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint i = 0;\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tfor (; i < laserCloudWidth - 1; i++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t}\n\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeCornerPointer;\n\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeSurfPointer;\n\t\t\t\t\t\tlaserCloudCubeCornerPointer->clear();\n\t\t\t\t\t\tlaserCloudCubeSurfPointer->clear();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tcenterCubeI--;\n\t\t\t\tlaserCloudCenWidth--;\n\t\t\t}\n\n\t\t\twhile (centerCubeJ < 3)\n\t\t\t{\n\t\t\t\tfor (int i = 0; i < laserCloudWidth; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int k = 0; k < laserCloudDepth; k++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint j = laserCloudHeight - 1;\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tfor (; j >= 1; j--)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t}\n\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeCornerPointer;\n\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeSurfPointer;\n\t\t\t\t\t\tlaserCloudCubeCornerPointer->clear();\n\t\t\t\t\t\tlaserCloudCubeSurfPointer->clear();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tcenterCubeJ++;\n\t\t\t\tlaserCloudCenHeight++;\n\t\t\t}\n\n\t\t\twhile (centerCubeJ >= laserCloudHeight - 3)\n\t\t\t{\n\t\t\t\tfor (int i = 0; i < laserCloudWidth; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int k = 0; k < laserCloudDepth; k++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint j = 0;\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tfor (; j < laserCloudHeight - 1; j++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\t}\n\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeCornerPointer;\n\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeSurfPointer;\n\t\t\t\t\t\tlaserCloudCubeCornerPointer->clear();\n\t\t\t\t\t\tlaserCloudCubeSurfPointer->clear();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tcenterCubeJ--;\n\t\t\t\tlaserCloudCenHeight--;\n\t\t\t}\n\n\t\t\twhile (centerCubeK < 3)\n\t\t\t{\n\t\t\t\tfor (int i = 0; i < laserCloudWidth; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int j = 0; j < laserCloudHeight; j++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint k = laserCloudDepth - 1;\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tfor (; k >= 1; k--)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];\n\t\t\t\t\t\t}\n\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeCornerPointer;\n\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeSurfPointer;\n\t\t\t\t\t\tlaserCloudCubeCornerPointer->clear();\n\t\t\t\t\t\tlaserCloudCubeSurfPointer->clear();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tcenterCubeK++;\n\t\t\t\tlaserCloudCenDepth++;\n\t\t\t}\n\n\t\t\twhile (centerCubeK >= laserCloudDepth - 3)\n\t\t\t{\n\t\t\t\tfor (int i = 0; i < laserCloudWidth; i++)\n\t\t\t\t{\n\t\t\t\t\tfor (int j = 0; j < laserCloudHeight; j++)\n\t\t\t\t\t{\n\t\t\t\t\t\tint k = 0;\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tpcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n\t\t\t\t\t\tfor (; k < laserCloudDepth - 1; k++)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];\n\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];\n\t\t\t\t\t\t}\n\t\t\t\t\t\tlaserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeCornerPointer;\n\t\t\t\t\t\tlaserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n\t\t\t\t\t\t\tlaserCloudCubeSurfPointer;\n\t\t\t\t\t\tlaserCloudCubeCornerPointer->clear();\n\t\t\t\t\t\tlaserCloudCubeSurfPointer->clear();\n\t\t\t\t\t}\n\t\t\t\t}\n\n\t\t\t\tcenterCubeK--;\n\t\t\t\tlaserCloudCenDepth--;\n\t\t\t}\n\n\t\t\tint laserCloudValidNum = 0;\n\t\t\tint laserCloudSurroundNum = 0;\n\n\t\t\tfor (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)\n\t\t\t{\n\t\t\t\tfor (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)\n\t\t\t\t{\n\t\t\t\t\tfor (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)\n\t\t\t\t\t{\n\t\t\t\t\t\tif (i >= 0 && i < laserCloudWidth &&\n\t\t\t\t\t\t\tj >= 0 && j < laserCloudHeight &&\n\t\t\t\t\t\t\tk >= 0 && k < laserCloudDepth)\n\t\t\t\t\t\t{ \n\t\t\t\t\t\t\tlaserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;\n\t\t\t\t\t\t\tlaserCloudValidNum++;\n\t\t\t\t\t\t\tlaserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;\n\t\t\t\t\t\t\tlaserCloudSurroundNum++;\n\t\t\t\t\t\t}\n\t\t\t\t\t}\n\t\t\t\t}\n\t\t\t}\n\n\t\t\tlaserCloudCornerFromMap->clear();\n\t\t\tlaserCloudSurfFromMap->clear();\n\t\t\tfor (int i = 0; i < laserCloudValidNum; i++)\n\t\t\t{\n\t\t\t\t*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];\n\t\t\t\t*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];\n\t\t\t}\n\t\t\tint laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();\n\t\t\tint laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();\n\n\n\t\t\tpcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud());\n\t\t\tdownSizeFilterCorner.setInputCloud(laserCloudCornerLast);\n\t\t\tdownSizeFilterCorner.filter(*laserCloudCornerStack);\n\t\t\tint laserCloudCornerStackNum = laserCloudCornerStack->points.size();\n\n\t\t\tpcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud());\n\t\t\tdownSizeFilterSurf.setInputCloud(laserCloudSurfLast);\n\t\t\tdownSizeFilterSurf.filter(*laserCloudSurfStack);\n\t\t\tint laserCloudSurfStackNum = laserCloudSurfStack->points.size();\n\n\t\t\t//printf(\"map prepare time %f ms\\n\", t_shift.toc());\n\t\t\t//printf(\"map corner num %d surf num %d \\n\", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);\n\t\t\tif (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)\n\t\t\t{\n\t\t\t\tTicToc t_opt;\n\t\t\t\tTicToc t_tree;\n\t\t\t\tkdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);\n\t\t\t\tkdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);\n\t\t\t\t//printf(\"build tree time %f ms \\n\", t_tree.toc());\n\n\t\t\t\tfor (int iterCount = 0; iterCount < 2; iterCount++)\n\t\t\t\t{\n\t\t\t\t\t//ceres::LossFunction *loss_function = NULL;\n\t\t\t\t\tceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);\n\t\t\t\t\tceres::LocalParameterization *q_parameterization =\n\t\t\t\t\t\tnew ceres::EigenQuaternionParameterization();\n\t\t\t\t\tceres::Problem::Options problem_options;\n\n\t\t\t\t\tceres::Problem problem(problem_options);\n\t\t\t\t\tproblem.AddParameterBlock(parameters, 4, q_parameterization);\n\t\t\t\t\tproblem.AddParameterBlock(parameters + 4, 3);\n\n\t\t\t\t\tTicToc t_data;\n\t\t\t\t\tint corner_num = 0;\n\n\t\t\t\t\tfor (int i = 0; i < laserCloudCornerStackNum; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tpointOri = laserCloudCornerStack->points[i];\n\t\t\t\t\t\t//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;\n\t\t\t\t\t\tpointAssociateToMap(&pointOri, &pointSel);\n\t\t\t\t\t\tkdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); \n\n\t\t\t\t\t\tif (pointSearchSqDis[4] < 1.0)\n\t\t\t\t\t\t{ \n\t\t\t\t\t\t\tstd::vector nearCorners;\n\t\t\t\t\t\t\tEigen::Vector3d center(0, 0, 0);\n\t\t\t\t\t\t\tfor (int j = 0; j < 5; j++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tEigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,\n\t\t\t\t\t\t\t\t\t\t\t\t\tlaserCloudCornerFromMap->points[pointSearchInd[j]].y,\n\t\t\t\t\t\t\t\t\t\t\t\t\tlaserCloudCornerFromMap->points[pointSearchInd[j]].z);\n\t\t\t\t\t\t\t\tcenter = center + tmp;\n\t\t\t\t\t\t\t\tnearCorners.push_back(tmp);\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t\tcenter = center / 5.0;\n\n\t\t\t\t\t\t\tEigen::Matrix3d covMat = Eigen::Matrix3d::Zero();\n\t\t\t\t\t\t\tfor (int j = 0; j < 5; j++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tEigen::Matrix tmpZeroMean = nearCorners[j] - center;\n\t\t\t\t\t\t\t\tcovMat = covMat + tmpZeroMean * tmpZeroMean.transpose();\n\t\t\t\t\t\t\t}\n\n\t\t\t\t\t\t\tEigen::SelfAdjointEigenSolver saes(covMat);\n\n\t\t\t\t\t\t\t// if is indeed line feature\n\t\t\t\t\t\t\t// note Eigen library sort eigenvalues in increasing order\n\t\t\t\t\t\t\tEigen::Vector3d unit_direction = saes.eigenvectors().col(2);\n\t\t\t\t\t\t\tEigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n\t\t\t\t\t\t\tif (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])\n\t\t\t\t\t\t\t{ \n\t\t\t\t\t\t\t\tEigen::Vector3d point_on_line = center;\n\t\t\t\t\t\t\t\tEigen::Vector3d point_a, point_b;\n\t\t\t\t\t\t\t\tpoint_a = 0.1 * unit_direction + point_on_line;\n\t\t\t\t\t\t\t\tpoint_b = -0.1 * unit_direction + point_on_line;\n\n\t\t\t\t\t\t\t\tceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);\n\t\t\t\t\t\t\t\tproblem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n\t\t\t\t\t\t\t\tcorner_num++;\t\n\t\t\t\t\t\t\t}\t\t\t\t\t\t\t\n\t\t\t\t\t\t}\n\t\t\t\t\t\t/*\n\t\t\t\t\t\telse if(pointSearchSqDis[4] < 0.01 * sqrtDis)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tEigen::Vector3d center(0, 0, 0);\n\t\t\t\t\t\t\tfor (int j = 0; j < 5; j++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tEigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,\n\t\t\t\t\t\t\t\t\t\t\t\t\tlaserCloudCornerFromMap->points[pointSearchInd[j]].y,\n\t\t\t\t\t\t\t\t\t\t\t\t\tlaserCloudCornerFromMap->points[pointSearchInd[j]].z);\n\t\t\t\t\t\t\t\tcenter = center + tmp;\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t\tcenter = center / 5.0;\t\n\t\t\t\t\t\t\tEigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n\t\t\t\t\t\t\tceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);\n\t\t\t\t\t\t\tproblem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n\t\t\t\t\t\t}\n\t\t\t\t\t\t*/\n\t\t\t\t\t}\n\n\t\t\t\t\tint surf_num = 0;\n\t\t\t\t\tfor (int i = 0; i < laserCloudSurfStackNum; i++)\n\t\t\t\t\t{\n\t\t\t\t\t\tpointOri = laserCloudSurfStack->points[i];\n\t\t\t\t\t\t//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;\n\t\t\t\t\t\tpointAssociateToMap(&pointOri, &pointSel);\n\t\t\t\t\t\tkdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);\n\n\t\t\t\t\t\tEigen::Matrix matA0;\n\t\t\t\t\t\tEigen::Matrix matB0 = -1 * Eigen::Matrix::Ones();\n\t\t\t\t\t\tif (pointSearchSqDis[4] < 1.0)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\n\t\t\t\t\t\t\tfor (int j = 0; j < 5; j++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tmatA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;\n\t\t\t\t\t\t\t\tmatA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;\n\t\t\t\t\t\t\t\tmatA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;\n\t\t\t\t\t\t\t\t////printf(\" pts %f %f %f \", matA0(j, 0), matA0(j, 1), matA0(j, 2));\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t\t// find the norm of plane\n\t\t\t\t\t\t\tEigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);\n\t\t\t\t\t\t\tdouble negative_OA_dot_norm = 1 / norm.norm();\n\t\t\t\t\t\t\tnorm.normalize();\n\n\t\t\t\t\t\t\t// Here n(pa, pb, pc) is unit norm of plane\n\t\t\t\t\t\t\tbool planeValid = true;\n\t\t\t\t\t\t\tfor (int j = 0; j < 5; j++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\t// if OX * n > 0.2, then plane is not fit well\n\t\t\t\t\t\t\t\tif (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +\n\t\t\t\t\t\t\t\t\t\t norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +\n\t\t\t\t\t\t\t\t\t\t norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)\n\t\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\t\tplaneValid = false;\n\t\t\t\t\t\t\t\t\tbreak;\n\t\t\t\t\t\t\t\t}\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t\tEigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n\t\t\t\t\t\t\tif (planeValid)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);\n\t\t\t\t\t\t\t\tproblem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n\t\t\t\t\t\t\t\tsurf_num++;\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t}\n\t\t\t\t\t\t/*\n\t\t\t\t\t\telse if(pointSearchSqDis[4] < 0.01 * sqrtDis)\n\t\t\t\t\t\t{\n\t\t\t\t\t\t\tEigen::Vector3d center(0, 0, 0);\n\t\t\t\t\t\t\tfor (int j = 0; j < 5; j++)\n\t\t\t\t\t\t\t{\n\t\t\t\t\t\t\t\tEigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,\n\t\t\t\t\t\t\t\t\t\t\t\t\tlaserCloudSurfFromMap->points[pointSearchInd[j]].y,\n\t\t\t\t\t\t\t\t\t\t\t\t\tlaserCloudSurfFromMap->points[pointSearchInd[j]].z);\n\t\t\t\t\t\t\t\tcenter = center + tmp;\n\t\t\t\t\t\t\t}\n\t\t\t\t\t\t\tcenter = center / 5.0;\t\n\t\t\t\t\t\t\tEigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n\t\t\t\t\t\t\tceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);\n\t\t\t\t\t\t\tproblem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n\t\t\t\t\t\t}\n\t\t\t\t\t\t*/\n\t\t\t\t\t}\n\n\t\t\t\t\t////printf(\"corner num %d used corner num %d \\n\", laserCloudCornerStackNum, corner_num);\n\t\t\t\t\t////printf(\"surf num %d used surf num %d \\n\", laserCloudSurfStackNum, surf_num);\n\n\t\t\t\t\t//printf(\"mapping data assosiation time %f ms \\n\", t_data.toc());\n\n\t\t\t\t\tTicToc t_solver;\n\t\t\t\t\tceres::Solver::Options options;\n\t\t\t\t\toptions.linear_solver_type = ceres::DENSE_QR;\n\t\t\t\t\toptions.max_num_iterations = 4;\n\t\t\t\t\toptions.minimizer_progress_to_stdout = false;\n\t\t\t\t\toptions.check_gradients = false;\n\t\t\t\t\toptions.gradient_check_relative_precision = 1e-4;\n\t\t\t\t\tceres::Solver::Summary summary;\n\t\t\t\t\tceres::Solve(options, &problem, &summary);\n\t\t\t\t\t//printf(\"mapping solver time %f ms \\n\", t_solver.toc());\n\n\t\t\t\t\t////printf(\"time %f \\n\", timeLaserOdometry);\n\t\t\t\t\t////printf(\"corner factor num %d surf factor num %d\\n\", corner_num, surf_num);\n\t\t\t\t\t////printf(\"result q %f %f %f %f result t %f %f %f\\n\", parameters[3], parameters[0], parameters[1], parameters[2],\n\t\t\t\t\t//\t parameters[4], parameters[5], parameters[6]);\n\t\t\t\t}\n\t\t\t\t//printf(\"mapping optimization time %f \\n\", t_opt.toc());\n\t\t\t}\n\t\t\telse\n\t\t\t{\n\t\t\t\tROS_WARN(\"time Map corner and surf num are not enough\");\n\t\t\t}\n\t\t\ttransformUpdate();\n\n\t\t\tTicToc t_add;\n\t\t\tfor (int i = 0; i < laserCloudCornerStackNum; i++)\n\t\t\t{\n\t\t\t\tpointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);\n\n\t\t\t\tint cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;\n\t\t\t\tint cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;\n\t\t\t\tint cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;\n\n\t\t\t\tif (pointSel.x + 25.0 < 0)\n\t\t\t\t\tcubeI--;\n\t\t\t\tif (pointSel.y + 25.0 < 0)\n\t\t\t\t\tcubeJ--;\n\t\t\t\tif (pointSel.z + 25.0 < 0)\n\t\t\t\t\tcubeK--;\n\n\t\t\t\tif (cubeI >= 0 && cubeI < laserCloudWidth &&\n\t\t\t\t\tcubeJ >= 0 && cubeJ < laserCloudHeight &&\n\t\t\t\t\tcubeK >= 0 && cubeK < laserCloudDepth)\n\t\t\t\t{\n\t\t\t\t\tint cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;\n\t\t\t\t\tlaserCloudCornerArray[cubeInd]->push_back(pointSel);\n\t\t\t\t}\n\t\t\t}\n\n\t\t\tfor (int i = 0; i < laserCloudSurfStackNum; i++)\n\t\t\t{\n\t\t\t\tpointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);\n\n\t\t\t\tint cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;\n\t\t\t\tint cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;\n\t\t\t\tint cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;\n\n\t\t\t\tif (pointSel.x + 25.0 < 0)\n\t\t\t\t\tcubeI--;\n\t\t\t\tif (pointSel.y + 25.0 < 0)\n\t\t\t\t\tcubeJ--;\n\t\t\t\tif (pointSel.z + 25.0 < 0)\n\t\t\t\t\tcubeK--;\n\n\t\t\t\tif (cubeI >= 0 && cubeI < laserCloudWidth &&\n\t\t\t\t\tcubeJ >= 0 && cubeJ < laserCloudHeight &&\n\t\t\t\t\tcubeK >= 0 && cubeK < laserCloudDepth)\n\t\t\t\t{\n\t\t\t\t\tint cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;\n\t\t\t\t\tlaserCloudSurfArray[cubeInd]->push_back(pointSel);\n\t\t\t\t}\n\t\t\t}\n\t\t\t//printf(\"add points time %f ms\\n\", t_add.toc());\n\n\t\t\t\n\t\t\tTicToc t_filter;\n\t\t\tfor (int i = 0; i < laserCloudValidNum; i++)\n\t\t\t{\n\t\t\t\tint ind = laserCloudValidInd[i];\n\n\t\t\t\tpcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud());\n\t\t\t\tdownSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);\n\t\t\t\tdownSizeFilterCorner.filter(*tmpCorner);\n\t\t\t\tlaserCloudCornerArray[ind] = tmpCorner;\n\n\t\t\t\tpcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud());\n\t\t\t\tdownSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);\n\t\t\t\tdownSizeFilterSurf.filter(*tmpSurf);\n\t\t\t\tlaserCloudSurfArray[ind] = tmpSurf;\n\t\t\t}\n\t\t\t//printf(\"filter time %f ms \\n\", t_filter.toc());\n\t\t\t\n\t\t\tTicToc t_pub;\n\t\t\t//publish surround map for every 5 frame\n\t\t\tif (frameCount % 5 == 0)\n\t\t\t{\n\t\t\t\tlaserCloudSurround->clear();\n\t\t\t\tfor (int i = 0; i < laserCloudSurroundNum; i++)\n\t\t\t\t{\n\t\t\t\t\tint ind = laserCloudSurroundInd[i];\n\t\t\t\t\t*laserCloudSurround += *laserCloudCornerArray[ind];\n\t\t\t\t\t*laserCloudSurround += *laserCloudSurfArray[ind];\n\t\t\t\t}\n\n\t\t\t\tsensor_msgs::PointCloud2 laserCloudSurround3;\n\t\t\t\tpcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);\n\t\t\t\tlaserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);\n\t\t\t\tlaserCloudSurround3.header.frame_id = \"camera_init\";\n\t\t\t\tpubLaserCloudSurround.publish(laserCloudSurround3);\n\t\t\t}\n\n\t\t\tif (frameCount % 20 == 0)\n\t\t\t{\n\t\t\t\tpcl::PointCloud laserCloudMap;\n\t\t\t\tfor (int i = 0; i < 4851; i++)\n\t\t\t\t{\n\t\t\t\t\tlaserCloudMap += *laserCloudCornerArray[i];\n\t\t\t\t\tlaserCloudMap += *laserCloudSurfArray[i];\n\t\t\t\t}\n\t\t\t\tsensor_msgs::PointCloud2 laserCloudMsg;\n\t\t\t\tpcl::toROSMsg(laserCloudMap, laserCloudMsg);\n\t\t\t\tlaserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);\n\t\t\t\tlaserCloudMsg.header.frame_id = \"camera_init\";\n\t\t\t\tpubLaserCloudMap.publish(laserCloudMsg);\n\t\t\t}\n\n\t\t\tsensor_msgs::PointCloud2 laserCloudFullRes3Local;\n\t\t\tpcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3Local);\n\t\t\tlaserCloudFullRes3Local.header.stamp = ros::Time().fromSec(timeLaserOdometry);\n\t\t\tlaserCloudFullRes3Local.header.frame_id = \"camera_init\";\n\t\t\tpubLaserCloudFullResLocal.publish(laserCloudFullRes3Local);\n\n\t\t\tint laserCloudFullResNum = laserCloudFullRes->points.size();\n\t\t\tfor (int i = 0; i < laserCloudFullResNum; i++)\n\t\t\t{\n\t\t\t\tpointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);\n\t\t\t}\n\n\t\t\tsensor_msgs::PointCloud2 laserCloudFullRes3;\n\t\t\tpcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);\n\t\t\tlaserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);\n\t\t\tlaserCloudFullRes3.header.frame_id = \"camera_init\";\n\t\t\tpubLaserCloudFullRes.publish(laserCloudFullRes3);\n\n\t\t\t//printf(\"mapping pub time %f ms \\n\", t_pub.toc());\n\n\t\t\t//printf(\"whole mapping time %f ms ++++++++++\\n\", t_whole.toc());\n\n\t\t\tnav_msgs::Odometry odomAftMapped;\n\t\t\todomAftMapped.header.frame_id = \"camera_init\";\n\t\t\todomAftMapped.child_frame_id = \"/aft_mapped\";\n\t\t\todomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);\n\t\t\todomAftMapped.pose.pose.orientation.x = q_w_curr.x();\n\t\t\todomAftMapped.pose.pose.orientation.y = q_w_curr.y();\n\t\t\todomAftMapped.pose.pose.orientation.z = q_w_curr.z();\n\t\t\todomAftMapped.pose.pose.orientation.w = q_w_curr.w();\n\t\t\todomAftMapped.pose.pose.position.x = t_w_curr.x();\n\t\t\todomAftMapped.pose.pose.position.y = t_w_curr.y();\n\t\t\todomAftMapped.pose.pose.position.z = t_w_curr.z();\n\t\t\tpubOdomAftMapped.publish(odomAftMapped);\n\n\t\t\t// double roll, pitch, yaw;\n\t\t\t// geometry_msgs::Quaternion quat = odomAftMapped.pose.pose.orientation;\n\t\t\t// tf::Matrix3x3(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(roll, pitch, yaw);\n\t\t\t// std::cout << \"pub - roll, pitch, yaw (deg): \" << rad2deg(roll) << \", \" << rad2deg(pitch) << \", \" << rad2deg(yaw) << std::endl;\n\n\t\t\tgeometry_msgs::PoseStamped laserAfterMappedPose;\n\t\t\tlaserAfterMappedPose.header = odomAftMapped.header;\n\t\t\tlaserAfterMappedPose.pose = odomAftMapped.pose.pose;\n\n\t\t\tlaserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;\n\t\t\tlaserAfterMappedPath.header.frame_id = \"camera_init\";\n\t\t\tlaserAfterMappedPath.poses.push_back(laserAfterMappedPose);\n\t\t\tpubLaserAfterMappedPath.publish(laserAfterMappedPath);\n\n\t\t\tstatic tf::TransformBroadcaster br;\n\t\t\ttf::Transform transform;\n\t\t\ttf::Quaternion q;\n\t\t\ttransform.setOrigin(tf::Vector3(t_w_curr(0),\n\t\t\t\t\t\t\t\t\t\t\tt_w_curr(1),\n\t\t\t\t\t\t\t\t\t\t\tt_w_curr(2)));\n\t\t\tq.setW(q_w_curr.w());\n\t\t\tq.setX(q_w_curr.x());\n\t\t\tq.setY(q_w_curr.y());\n\t\t\tq.setZ(q_w_curr.z());\n\t\t\ttransform.setRotation(q);\n\t\t\tbr.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, \"camera_init\", \"/aft_mapped\"));\n\n\t\t\tframeCount++;\n\t\t}\n\t\tstd::chrono::milliseconds dura(2);\n std::this_thread::sleep_for(dura);\n\t}\n}\n\nint main(int argc, char **argv)\n{\n\tros::init(argc, argv, \"laserMapping\");\n\tros::NodeHandle nh;\n\n\tfloat lineRes = 0;\n\tfloat planeRes = 0;\n\tnh.param(\"mapping_line_resolution\", lineRes, 0.4);\n\tnh.param(\"mapping_plane_resolution\", planeRes, 0.8);\n\t//printf(\"line resolution %f plane resolution %f \\n\", lineRes, planeRes);\n\tdownSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);\n\tdownSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);\n\n\tros::Subscriber subLaserCloudCornerLast = nh.subscribe(\"/laser_cloud_corner_last\", 100, laserCloudCornerLastHandler);\n\n\tros::Subscriber subLaserCloudSurfLast = nh.subscribe(\"/laser_cloud_surf_last\", 100, laserCloudSurfLastHandler);\n\n\tros::Subscriber subLaserOdometry = nh.subscribe(\"/laser_odom_to_init\", 100, laserOdometryHandler);\n\n\tros::Subscriber subLaserCloudFullRes = nh.subscribe(\"/velodyne_cloud_3\", 100, laserCloudFullResHandler);\n\n\tpubLaserCloudSurround = nh.advertise(\"/laser_cloud_surround\", 100);\n\n\tpubLaserCloudMap = nh.advertise(\"/laser_cloud_map\", 100);\n\n\tpubLaserCloudFullRes = nh.advertise(\"/velodyne_cloud_registered\", 100);\n\tpubLaserCloudFullResLocal = nh.advertise(\"/velodyne_cloud_registered_local\", 100);\n\n\tpubOdomAftMapped = nh.advertise(\"/aft_mapped_to_init\", 100);\n\n\tpubOdomAftMappedHighFrec = nh.advertise(\"/aft_mapped_to_init_high_frec\", 100);\n\n\tpubLaserAfterMappedPath = nh.advertise(\"/aft_mapped_path\", 100);\n\n\tfor (int i = 0; i < laserCloudNum; i++)\n\t{\n\t\tlaserCloudCornerArray[i].reset(new pcl::PointCloud());\n\t\tlaserCloudSurfArray[i].reset(new pcl::PointCloud());\n\t}\n\n\tstd::thread mapping_process{process};\n\n\tros::spin();\n\n\treturn 0;\n}" }, { "path": "SC-PGO/src/laserOdometry.cpp", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. \n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"aloam_velodyne/common.h\"\n#include \"aloam_velodyne/tic_toc.h\"\n#include \"lidarFactor.hpp\"\n\n#define DISTORTION 0\n\n\nint corner_correspondence = 0, plane_correspondence = 0;\n\nconstexpr double SCAN_PERIOD = 0.1;\nconstexpr double DISTANCE_SQ_THRESHOLD = 25;\nconstexpr double NEARBY_SCAN = 2.5;\n\nint skipFrameNum = 5;\nbool systemInited = false;\n\ndouble timeCornerPointsSharp = 0;\ndouble timeCornerPointsLessSharp = 0;\ndouble timeSurfPointsFlat = 0;\ndouble timeSurfPointsLessFlat = 0;\ndouble timeLaserCloudFullRes = 0;\n\npcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN());\npcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN());\n\npcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud());\npcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud());\npcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud());\npcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud());\n\npcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud());\npcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud());\npcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());\n\nint laserCloudCornerLastNum = 0;\nint laserCloudSurfLastNum = 0;\n\n// Transformation from current frame to world frame\nEigen::Quaterniond q_w_curr(1, 0, 0, 0);\nEigen::Vector3d t_w_curr(0, 0, 0);\n\n// q_curr_last(x, y, z, w), t_curr_last\ndouble para_q[4] = {0, 0, 0, 1};\ndouble para_t[3] = {0, 0, 0};\n\nEigen::Map q_last_curr(para_q);\nEigen::Map t_last_curr(para_t);\n\nstd::queue cornerSharpBuf;\nstd::queue cornerLessSharpBuf;\nstd::queue surfFlatBuf;\nstd::queue surfLessFlatBuf;\nstd::queue fullPointsBuf;\nstd::mutex mBuf;\n\n// undistort lidar point\nvoid TransformToStart(PointType const *const pi, PointType *const po)\n{\n //interpolation ratio\n double s;\n if (DISTORTION)\n s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;\n else\n s = 1.0;\n //s = 1;\n Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);\n Eigen::Vector3d t_point_last = s * t_last_curr;\n Eigen::Vector3d point(pi->x, pi->y, pi->z);\n Eigen::Vector3d un_point = q_point_last * point + t_point_last;\n\n po->x = un_point.x();\n po->y = un_point.y();\n po->z = un_point.z();\n po->intensity = pi->intensity;\n}\n\n// transform all lidar points to the start of the next frame\n\nvoid TransformToEnd(PointType const *const pi, PointType *const po)\n{\n // undistort point first\n pcl::PointXYZI un_point_tmp;\n TransformToStart(pi, &un_point_tmp);\n\n Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);\n Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);\n\n po->x = point_end.x();\n po->y = point_end.y();\n po->z = point_end.z();\n\n //Remove distortion time info\n po->intensity = int(pi->intensity);\n}\n\nvoid laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)\n{\n mBuf.lock();\n cornerSharpBuf.push(cornerPointsSharp2);\n mBuf.unlock();\n}\n\nvoid laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)\n{\n mBuf.lock();\n cornerLessSharpBuf.push(cornerPointsLessSharp2);\n mBuf.unlock();\n}\n\nvoid laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)\n{\n mBuf.lock();\n surfFlatBuf.push(surfPointsFlat2);\n mBuf.unlock();\n}\n\nvoid laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)\n{\n mBuf.lock();\n surfLessFlatBuf.push(surfPointsLessFlat2);\n mBuf.unlock();\n}\n\n//receive all point cloud\nvoid laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)\n{\n mBuf.lock();\n fullPointsBuf.push(laserCloudFullRes2);\n mBuf.unlock();\n}\n\nint main(int argc, char **argv)\n{\n ros::init(argc, argv, \"laserOdometry\");\n ros::NodeHandle nh;\n\n nh.param(\"mapping_skip_frame\", skipFrameNum, 2);\n\n //printf(\"Mapping %d Hz \\n\", 10 / skipFrameNum);\n\n ros::Subscriber subCornerPointsSharp = nh.subscribe(\"/laser_cloud_sharp\", 100, laserCloudSharpHandler);\n\n ros::Subscriber subCornerPointsLessSharp = nh.subscribe(\"/laser_cloud_less_sharp\", 100, laserCloudLessSharpHandler);\n\n ros::Subscriber subSurfPointsFlat = nh.subscribe(\"/laser_cloud_flat\", 100, laserCloudFlatHandler);\n\n ros::Subscriber subSurfPointsLessFlat = nh.subscribe(\"/laser_cloud_less_flat\", 100, laserCloudLessFlatHandler);\n\n ros::Subscriber subLaserCloudFullRes = nh.subscribe(\"/velodyne_cloud_2\", 100, laserCloudFullResHandler);\n\n ros::Publisher pubLaserCloudCornerLast = nh.advertise(\"/laser_cloud_corner_last\", 100);\n\n ros::Publisher pubLaserCloudSurfLast = nh.advertise(\"/laser_cloud_surf_last\", 100);\n\n ros::Publisher pubLaserCloudFullRes = nh.advertise(\"/velodyne_cloud_3\", 100);\n\n ros::Publisher pubLaserOdometry = nh.advertise(\"/laser_odom_to_init\", 100);\n\n ros::Publisher pubLaserPath = nh.advertise(\"/laser_odom_path\", 100);\n\n nav_msgs::Path laserPath;\n\n int frameCount = 0;\n ros::Rate rate(100);\n\n while (ros::ok())\n {\n ros::spinOnce();\n\n if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&\n !surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&\n !fullPointsBuf.empty())\n {\n timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();\n timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();\n timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();\n timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();\n timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();\n\n if (timeCornerPointsSharp != timeLaserCloudFullRes ||\n timeCornerPointsLessSharp != timeLaserCloudFullRes ||\n timeSurfPointsFlat != timeLaserCloudFullRes ||\n timeSurfPointsLessFlat != timeLaserCloudFullRes)\n {\n //printf(\"unsync messeage!\");\n ROS_BREAK();\n }\n\n mBuf.lock();\n cornerPointsSharp->clear();\n pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);\n cornerSharpBuf.pop();\n\n cornerPointsLessSharp->clear();\n pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);\n cornerLessSharpBuf.pop();\n\n surfPointsFlat->clear();\n pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);\n surfFlatBuf.pop();\n\n surfPointsLessFlat->clear();\n pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);\n surfLessFlatBuf.pop();\n\n laserCloudFullRes->clear();\n pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);\n fullPointsBuf.pop();\n mBuf.unlock();\n\n TicToc t_whole;\n // initializing\n if (!systemInited)\n {\n systemInited = true;\n std::cout << \"Initialization finished \\n\";\n }\n else\n {\n int cornerPointsSharpNum = cornerPointsSharp->points.size();\n int surfPointsFlatNum = surfPointsFlat->points.size();\n\n TicToc t_opt;\n for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)\n {\n corner_correspondence = 0;\n plane_correspondence = 0;\n\n //ceres::LossFunction *loss_function = NULL;\n ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);\n ceres::LocalParameterization *q_parameterization =\n new ceres::EigenQuaternionParameterization();\n ceres::Problem::Options problem_options;\n\n ceres::Problem problem(problem_options);\n problem.AddParameterBlock(para_q, 4, q_parameterization);\n problem.AddParameterBlock(para_t, 3);\n\n pcl::PointXYZI pointSel;\n std::vector pointSearchInd;\n std::vector pointSearchSqDis;\n\n TicToc t_data;\n // find correspondence for corner features\n for (int i = 0; i < cornerPointsSharpNum; ++i)\n {\n TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);\n kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);\n\n int closestPointInd = -1, minPointInd2 = -1;\n if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)\n {\n closestPointInd = pointSearchInd[0];\n int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);\n\n double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;\n // search in the direction of increasing scan line\n for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)\n {\n // if in the same scan line, continue\n if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)\n continue;\n\n // if not in nearby scans, end the loop\n if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))\n break;\n\n double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *\n (laserCloudCornerLast->points[j].x - pointSel.x) +\n (laserCloudCornerLast->points[j].y - pointSel.y) *\n (laserCloudCornerLast->points[j].y - pointSel.y) +\n (laserCloudCornerLast->points[j].z - pointSel.z) *\n (laserCloudCornerLast->points[j].z - pointSel.z);\n\n if (pointSqDis < minPointSqDis2)\n {\n // find nearer point\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n }\n\n // search in the direction of decreasing scan line\n for (int j = closestPointInd - 1; j >= 0; --j)\n {\n // if in the same scan line, continue\n if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)\n continue;\n\n // if not in nearby scans, end the loop\n if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))\n break;\n\n double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *\n (laserCloudCornerLast->points[j].x - pointSel.x) +\n (laserCloudCornerLast->points[j].y - pointSel.y) *\n (laserCloudCornerLast->points[j].y - pointSel.y) +\n (laserCloudCornerLast->points[j].z - pointSel.z) *\n (laserCloudCornerLast->points[j].z - pointSel.z);\n\n if (pointSqDis < minPointSqDis2)\n {\n // find nearer point\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n }\n }\n if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid\n {\n Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,\n cornerPointsSharp->points[i].y,\n cornerPointsSharp->points[i].z);\n Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,\n laserCloudCornerLast->points[closestPointInd].y,\n laserCloudCornerLast->points[closestPointInd].z);\n Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,\n laserCloudCornerLast->points[minPointInd2].y,\n laserCloudCornerLast->points[minPointInd2].z);\n\n double s;\n if (DISTORTION)\n s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;\n else\n s = 1.0;\n ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);\n problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);\n corner_correspondence++;\n }\n }\n\n // find correspondence for plane features\n for (int i = 0; i < surfPointsFlatNum; ++i)\n {\n TransformToStart(&(surfPointsFlat->points[i]), &pointSel);\n kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);\n\n int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;\n if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)\n {\n closestPointInd = pointSearchInd[0];\n\n // get closest point's scan ID\n int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);\n double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;\n\n // search in the direction of increasing scan line\n for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)\n {\n // if not in nearby scans, end the loop\n if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))\n break;\n\n double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *\n (laserCloudSurfLast->points[j].x - pointSel.x) +\n (laserCloudSurfLast->points[j].y - pointSel.y) *\n (laserCloudSurfLast->points[j].y - pointSel.y) +\n (laserCloudSurfLast->points[j].z - pointSel.z) *\n (laserCloudSurfLast->points[j].z - pointSel.z);\n\n // if in the same or lower scan line\n if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)\n {\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n // if in the higher scan line\n else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)\n {\n minPointSqDis3 = pointSqDis;\n minPointInd3 = j;\n }\n }\n\n // search in the direction of decreasing scan line\n for (int j = closestPointInd - 1; j >= 0; --j)\n {\n // if not in nearby scans, end the loop\n if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))\n break;\n\n double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *\n (laserCloudSurfLast->points[j].x - pointSel.x) +\n (laserCloudSurfLast->points[j].y - pointSel.y) *\n (laserCloudSurfLast->points[j].y - pointSel.y) +\n (laserCloudSurfLast->points[j].z - pointSel.z) *\n (laserCloudSurfLast->points[j].z - pointSel.z);\n\n // if in the same or higher scan line\n if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)\n {\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)\n {\n // find nearer point\n minPointSqDis3 = pointSqDis;\n minPointInd3 = j;\n }\n }\n\n if (minPointInd2 >= 0 && minPointInd3 >= 0)\n {\n\n Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,\n surfPointsFlat->points[i].y,\n surfPointsFlat->points[i].z);\n Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,\n laserCloudSurfLast->points[closestPointInd].y,\n laserCloudSurfLast->points[closestPointInd].z);\n Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,\n laserCloudSurfLast->points[minPointInd2].y,\n laserCloudSurfLast->points[minPointInd2].z);\n Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,\n laserCloudSurfLast->points[minPointInd3].y,\n laserCloudSurfLast->points[minPointInd3].z);\n\n double s;\n if (DISTORTION)\n s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;\n else\n s = 1.0;\n ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);\n problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);\n plane_correspondence++;\n }\n }\n }\n\n ////printf(\"coner_correspondance %d, plane_correspondence %d \\n\", corner_correspondence, plane_correspondence);\n //printf(\"data association time %f ms \\n\", t_data.toc());\n\n if ((corner_correspondence + plane_correspondence) < 10)\n {\n //printf(\"less correspondence! *************************************************\\n\");\n }\n\n TicToc t_solver;\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::DENSE_QR;\n options.max_num_iterations = 4;\n options.minimizer_progress_to_stdout = false;\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n //printf(\"solver time %f ms \\n\", t_solver.toc());\n }\n //printf(\"optimization twice time %f \\n\", t_opt.toc());\n\n t_w_curr = t_w_curr + q_w_curr * t_last_curr;\n q_w_curr = q_w_curr * q_last_curr;\n }\n\n TicToc t_pub;\n\n // publish odometry\n nav_msgs::Odometry laserOdometry;\n laserOdometry.header.frame_id = \"camera_init\";\n laserOdometry.child_frame_id = \"/laser_odom\";\n laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);\n laserOdometry.pose.pose.orientation.x = q_w_curr.x();\n laserOdometry.pose.pose.orientation.y = q_w_curr.y();\n laserOdometry.pose.pose.orientation.z = q_w_curr.z();\n laserOdometry.pose.pose.orientation.w = q_w_curr.w();\n laserOdometry.pose.pose.position.x = t_w_curr.x();\n laserOdometry.pose.pose.position.y = t_w_curr.y();\n laserOdometry.pose.pose.position.z = t_w_curr.z();\n pubLaserOdometry.publish(laserOdometry);\n\n geometry_msgs::PoseStamped laserPose;\n laserPose.header = laserOdometry.header;\n laserPose.pose = laserOdometry.pose.pose;\n laserPath.header.stamp = laserOdometry.header.stamp;\n laserPath.poses.push_back(laserPose);\n laserPath.header.frame_id = \"camera_init\";\n pubLaserPath.publish(laserPath);\n\n // transform corner features and plane features to the scan end point\n if (0)\n {\n int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();\n for (int i = 0; i < cornerPointsLessSharpNum; i++)\n {\n TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);\n }\n\n int surfPointsLessFlatNum = surfPointsLessFlat->points.size();\n for (int i = 0; i < surfPointsLessFlatNum; i++)\n {\n TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);\n }\n\n int laserCloudFullResNum = laserCloudFullRes->points.size();\n for (int i = 0; i < laserCloudFullResNum; i++)\n {\n TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);\n }\n }\n\n pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp;\n cornerPointsLessSharp = laserCloudCornerLast;\n laserCloudCornerLast = laserCloudTemp;\n\n laserCloudTemp = surfPointsLessFlat;\n surfPointsLessFlat = laserCloudSurfLast;\n laserCloudSurfLast = laserCloudTemp;\n\n laserCloudCornerLastNum = laserCloudCornerLast->points.size();\n laserCloudSurfLastNum = laserCloudSurfLast->points.size();\n\n // std::cout << \"the size of corner last is \" << laserCloudCornerLastNum << \", and the size of surf last is \" << laserCloudSurfLastNum << '\\n';\n\n kdtreeCornerLast->setInputCloud(laserCloudCornerLast);\n kdtreeSurfLast->setInputCloud(laserCloudSurfLast);\n\n if (frameCount % skipFrameNum == 0)\n {\n frameCount = 0;\n\n sensor_msgs::PointCloud2 laserCloudCornerLast2;\n pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);\n laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);\n laserCloudCornerLast2.header.frame_id = \"/camera\";\n pubLaserCloudCornerLast.publish(laserCloudCornerLast2);\n\n sensor_msgs::PointCloud2 laserCloudSurfLast2;\n pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);\n laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);\n laserCloudSurfLast2.header.frame_id = \"/camera\";\n pubLaserCloudSurfLast.publish(laserCloudSurfLast2);\n\n sensor_msgs::PointCloud2 laserCloudFullRes3;\n pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);\n laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);\n laserCloudFullRes3.header.frame_id = \"/camera\";\n pubLaserCloudFullRes.publish(laserCloudFullRes3);\n }\n //printf(\"publication time %f ms \\n\", t_pub.toc());\n //printf(\"whole laserOdometry time %f ms \\n\", t_whole.toc());\n if(t_whole.toc() > 100)\n ROS_WARN(\"odometry process over 100ms\");\n\n frameCount++;\n }\n rate.sleep();\n }\n return 0;\n}" }, { "path": "SC-PGO/src/laserPosegraphOptimization.cpp", "content": "#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"aloam_velodyne/common.h\"\n#include \"aloam_velodyne/tic_toc.h\"\n\n#include \"scancontext/Scancontext.h\"\n\nusing namespace gtsam;\n\nusing std::cout;\nusing std::endl;\n\ndouble keyframeMeterGap;\ndouble keyframeDegGap, keyframeRadGap;\ndouble translationAccumulated = 1000000.0; // large value means must add the first given frame.\ndouble rotaionAccumulated = 1000000.0; // large value means must add the first given frame.\n\nbool isNowKeyFrame = false; \n\nPose6D odom_pose_prev {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // init \nPose6D odom_pose_curr {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // init pose is zero \n\nstd::queue odometryBuf;\nstd::queue fullResBuf;\nstd::queue gpsBuf;\nstd::queue > scLoopICPBuf;\n\nstd::mutex mBuf;\nstd::mutex mKF;\n\ndouble timeLaserOdometry = 0.0;\ndouble timeLaser = 0.0;\n\npcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());\npcl::PointCloud::Ptr laserCloudMapAfterPGO(new pcl::PointCloud());\n\nstd::vector::Ptr> keyframeLaserClouds; \nstd::vector keyframePoses;\nstd::vector keyframePosesUpdated;\nstd::vector keyframeTimes;\nint recentIdxUpdated = 0;\n\ngtsam::NonlinearFactorGraph gtSAMgraph;\nbool gtSAMgraphMade = false;\ngtsam::Values initialEstimate;\ngtsam::ISAM2 *isam;\ngtsam::Values isamCurrentEstimate;\n\nnoiseModel::Diagonal::shared_ptr priorNoise;\nnoiseModel::Diagonal::shared_ptr odomNoise;\nnoiseModel::Base::shared_ptr robustLoopNoise;\nnoiseModel::Base::shared_ptr robustGPSNoise;\n\npcl::VoxelGrid downSizeFilterScancontext;\nSCManager scManager;\ndouble scDistThres, scMaximumRadius;\n\npcl::VoxelGrid downSizeFilterICP;\nstd::mutex mtxICP;\nstd::mutex mtxPosegraph;\nstd::mutex mtxRecentPose;\n\npcl::PointCloud::Ptr laserCloudMapPGO(new pcl::PointCloud());\npcl::VoxelGrid downSizeFilterMapPGO;\nbool laserCloudMapPGORedraw = true;\n\nbool useGPS = true;\n// bool useGPS = false;\nsensor_msgs::NavSatFix::ConstPtr currGPS;\nbool hasGPSforThisKF = false;\nbool gpsOffsetInitialized = false; \ndouble gpsAltitudeInitOffset = 0.0;\ndouble recentOptimizedX = 0.0;\ndouble recentOptimizedY = 0.0;\n\nros::Publisher pubMapAftPGO, pubOdomAftPGO, pubPathAftPGO;\nros::Publisher pubLoopScanLocal, pubLoopSubmapLocal;\nros::Publisher pubOdomRepubVerifier;\n\nstd::string save_directory;\nstd::string pgKITTIformat, pgScansDirectory;\nstd::string odomKITTIformat;\nstd::fstream pgTimeSaveStream;\n\nstd::string padZeros(int val, int num_digits = 6) {\n std::ostringstream out;\n out << std::internal << std::setfill('0') << std::setw(num_digits) << val;\n return out.str();\n}\n\ngtsam::Pose3 Pose6DtoGTSAMPose3(const Pose6D& p)\n{\n return gtsam::Pose3( gtsam::Rot3::RzRyRx(p.roll, p.pitch, p.yaw), gtsam::Point3(p.x, p.y, p.z) );\n} // Pose6DtoGTSAMPose3\n\nvoid saveOdometryVerticesKITTIformat(std::string _filename)\n{\n // ref from gtsam's original code \"dataset.cpp\"\n std::fstream stream(_filename.c_str(), std::fstream::out);\n for(const auto& _pose6d: keyframePoses) {\n gtsam::Pose3 pose = Pose6DtoGTSAMPose3(_pose6d);\n Point3 t = pose.translation();\n Rot3 R = pose.rotation();\n auto col1 = R.column(1); // Point3\n auto col2 = R.column(2); // Point3\n auto col3 = R.column(3); // Point3\n\n stream << col1.x() << \" \" << col2.x() << \" \" << col3.x() << \" \" << t.x() << \" \"\n << col1.y() << \" \" << col2.y() << \" \" << col3.y() << \" \" << t.y() << \" \"\n << col1.z() << \" \" << col2.z() << \" \" << col3.z() << \" \" << t.z() << std::endl;\n }\n}\n\nvoid saveOptimizedVerticesKITTIformat(gtsam::Values _estimates, std::string _filename)\n{\n using namespace gtsam;\n\n // ref from gtsam's original code \"dataset.cpp\"\n std::fstream stream(_filename.c_str(), std::fstream::out);\n\n for(const auto& key_value: _estimates) {\n auto p = dynamic_cast*>(&key_value.value);\n if (!p) continue;\n\n const Pose3& pose = p->value();\n\n Point3 t = pose.translation();\n Rot3 R = pose.rotation();\n auto col1 = R.column(1); // Point3\n auto col2 = R.column(2); // Point3\n auto col3 = R.column(3); // Point3\n\n stream << col1.x() << \" \" << col2.x() << \" \" << col3.x() << \" \" << t.x() << \" \"\n << col1.y() << \" \" << col2.y() << \" \" << col3.y() << \" \" << t.y() << \" \"\n << col1.z() << \" \" << col2.z() << \" \" << col3.z() << \" \" << t.z() << std::endl;\n }\n}\n\nvoid laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &_laserOdometry)\n{\n\tmBuf.lock();\n\todometryBuf.push(_laserOdometry);\n\tmBuf.unlock();\n} // laserOdometryHandler\n\nvoid laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &_laserCloudFullRes)\n{\n\tmBuf.lock();\n\tfullResBuf.push(_laserCloudFullRes);\n\tmBuf.unlock();\n} // laserCloudFullResHandler\n\nvoid gpsHandler(const sensor_msgs::NavSatFix::ConstPtr &_gps)\n{\n if(useGPS) {\n mBuf.lock();\n gpsBuf.push(_gps);\n mBuf.unlock();\n }\n} // gpsHandler\n\nvoid initNoises( void )\n{\n gtsam::Vector priorNoiseVector6(6);\n priorNoiseVector6 << 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12;\n priorNoise = noiseModel::Diagonal::Variances(priorNoiseVector6);\n\n gtsam::Vector odomNoiseVector6(6);\n // odomNoiseVector6 << 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4;\n odomNoiseVector6 << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4;\n odomNoise = noiseModel::Diagonal::Variances(odomNoiseVector6);\n\n double loopNoiseScore = 0.5; // constant is ok...\n gtsam::Vector robustNoiseVector6(6); // gtsam::Pose3 factor has 6 elements (6D)\n robustNoiseVector6 << loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore, loopNoiseScore;\n robustLoopNoise = gtsam::noiseModel::Robust::Create(\n gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure is okay but Cauchy is empirically good.\n gtsam::noiseModel::Diagonal::Variances(robustNoiseVector6) );\n\n double bigNoiseTolerentToXY = 1000000000.0; // 1e9\n double gpsAltitudeNoiseScore = 250.0; // if height is misaligned after loop clsosing, use this value bigger\n gtsam::Vector robustNoiseVector3(3); // gps factor has 3 elements (xyz)\n robustNoiseVector3 << bigNoiseTolerentToXY, bigNoiseTolerentToXY, gpsAltitudeNoiseScore; // means only caring altitude here. (because LOAM-like-methods tends to be asymptotically flyging)\n robustGPSNoise = gtsam::noiseModel::Robust::Create(\n gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure is okay but Cauchy is empirically good.\n gtsam::noiseModel::Diagonal::Variances(robustNoiseVector3) );\n\n} // initNoises\n\nPose6D getOdom(nav_msgs::Odometry::ConstPtr _odom)\n{\n auto tx = _odom->pose.pose.position.x;\n auto ty = _odom->pose.pose.position.y;\n auto tz = _odom->pose.pose.position.z;\n\n double roll, pitch, yaw;\n geometry_msgs::Quaternion quat = _odom->pose.pose.orientation;\n tf::Matrix3x3(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(roll, pitch, yaw);\n\n return Pose6D{tx, ty, tz, roll, pitch, yaw}; \n} // getOdom\n\nPose6D diffTransformation(const Pose6D& _p1, const Pose6D& _p2)\n{\n Eigen::Affine3f SE3_p1 = pcl::getTransformation(_p1.x, _p1.y, _p1.z, _p1.roll, _p1.pitch, _p1.yaw);\n Eigen::Affine3f SE3_p2 = pcl::getTransformation(_p2.x, _p2.y, _p2.z, _p2.roll, _p2.pitch, _p2.yaw);\n Eigen::Matrix4f SE3_delta0 = SE3_p1.matrix().inverse() * SE3_p2.matrix();\n Eigen::Affine3f SE3_delta; SE3_delta.matrix() = SE3_delta0;\n float dx, dy, dz, droll, dpitch, dyaw;\n pcl::getTranslationAndEulerAngles (SE3_delta, dx, dy, dz, droll, dpitch, dyaw);\n // std::cout << \"delta : \" << dx << \", \" << dy << \", \" << dz << \", \" << droll << \", \" << dpitch << \", \" << dyaw << std::endl;\n\n return Pose6D{double(abs(dx)), double(abs(dy)), double(abs(dz)), double(abs(droll)), double(abs(dpitch)), double(abs(dyaw))};\n} // SE3Diff\n\npcl::PointCloud::Ptr local2global(const pcl::PointCloud::Ptr &cloudIn, const Pose6D& tf)\n{\n pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud());\n\n int cloudSize = cloudIn->size();\n cloudOut->resize(cloudSize);\n\n Eigen::Affine3f transCur = pcl::getTransformation(tf.x, tf.y, tf.z, tf.roll, tf.pitch, tf.yaw);\n \n int numberOfCores = 16;\n #pragma omp parallel for num_threads(numberOfCores)\n for (int i = 0; i < cloudSize; ++i)\n {\n const auto &pointFrom = cloudIn->points[i];\n cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3);\n cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3);\n cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3);\n cloudOut->points[i].intensity = pointFrom.intensity;\n }\n\n return cloudOut;\n}\n\nvoid pubPath( void )\n{\n // pub odom and path \n nav_msgs::Odometry odomAftPGO;\n nav_msgs::Path pathAftPGO;\n pathAftPGO.header.frame_id = \"camera_init\";\n mKF.lock(); \n // 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)\n 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)\n {\n const Pose6D& pose_est = keyframePosesUpdated.at(node_idx); // upodated poses\n // const gtsam::Pose3& pose_est = isamCurrentEstimate.at(node_idx);\n\n nav_msgs::Odometry odomAftPGOthis;\n odomAftPGOthis.header.frame_id = \"camera_init\";\n odomAftPGOthis.child_frame_id = \"/aft_pgo\";\n odomAftPGOthis.header.stamp = ros::Time().fromSec(keyframeTimes.at(node_idx));\n odomAftPGOthis.pose.pose.position.x = pose_est.x;\n odomAftPGOthis.pose.pose.position.y = pose_est.y;\n odomAftPGOthis.pose.pose.position.z = pose_est.z;\n odomAftPGOthis.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(pose_est.roll, pose_est.pitch, pose_est.yaw);\n odomAftPGO = odomAftPGOthis;\n\n geometry_msgs::PoseStamped poseStampAftPGO;\n poseStampAftPGO.header = odomAftPGOthis.header;\n poseStampAftPGO.pose = odomAftPGOthis.pose.pose;\n\n pathAftPGO.header.stamp = odomAftPGOthis.header.stamp;\n pathAftPGO.header.frame_id = \"camera_init\";\n pathAftPGO.poses.push_back(poseStampAftPGO);\n }\n mKF.unlock(); \n pubOdomAftPGO.publish(odomAftPGO); // last pose \n pubPathAftPGO.publish(pathAftPGO); // poses \n\n static tf::TransformBroadcaster br;\n tf::Transform transform;\n tf::Quaternion q;\n transform.setOrigin(tf::Vector3(odomAftPGO.pose.pose.position.x, odomAftPGO.pose.pose.position.y, odomAftPGO.pose.pose.position.z));\n q.setW(odomAftPGO.pose.pose.orientation.w);\n q.setX(odomAftPGO.pose.pose.orientation.x);\n q.setY(odomAftPGO.pose.pose.orientation.y);\n q.setZ(odomAftPGO.pose.pose.orientation.z);\n transform.setRotation(q);\n br.sendTransform(tf::StampedTransform(transform, odomAftPGO.header.stamp, \"camera_init\", \"/aft_pgo\"));\n} // pubPath\n\nvoid updatePoses(void)\n{\n mKF.lock(); \n for (int node_idx=0; node_idx < int(isamCurrentEstimate.size()); node_idx++)\n {\n Pose6D& p =keyframePosesUpdated[node_idx];\n p.x = isamCurrentEstimate.at(node_idx).translation().x();\n p.y = isamCurrentEstimate.at(node_idx).translation().y();\n p.z = isamCurrentEstimate.at(node_idx).translation().z();\n p.roll = isamCurrentEstimate.at(node_idx).rotation().roll();\n p.pitch = isamCurrentEstimate.at(node_idx).rotation().pitch();\n p.yaw = isamCurrentEstimate.at(node_idx).rotation().yaw();\n }\n mKF.unlock();\n\n mtxRecentPose.lock();\n const gtsam::Pose3& lastOptimizedPose = isamCurrentEstimate.at(int(isamCurrentEstimate.size())-1);\n recentOptimizedX = lastOptimizedPose.translation().x();\n recentOptimizedY = lastOptimizedPose.translation().y();\n\n recentIdxUpdated = int(keyframePosesUpdated.size()) - 1;\n\n mtxRecentPose.unlock();\n} // updatePoses\n\nvoid runISAM2opt(void)\n{\n // called when a variable added \n isam->update(gtSAMgraph, initialEstimate);\n isam->update();\n \n gtSAMgraph.resize(0);\n initialEstimate.clear();\n\n isamCurrentEstimate = isam->calculateEstimate();\n updatePoses();\n}\n\npcl::PointCloud::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, gtsam::Pose3 transformIn)\n{\n pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud());\n\n PointType *pointFrom;\n\n int cloudSize = cloudIn->size();\n cloudOut->resize(cloudSize);\n\n Eigen::Affine3f transCur = pcl::getTransformation(\n transformIn.translation().x(), transformIn.translation().y(), transformIn.translation().z(), \n transformIn.rotation().roll(), transformIn.rotation().pitch(), transformIn.rotation().yaw() );\n \n int numberOfCores = 8; // TODO move to yaml \n #pragma omp parallel for num_threads(numberOfCores)\n for (int i = 0; i < cloudSize; ++i)\n {\n pointFrom = &cloudIn->points[i];\n cloudOut->points[i].x = transCur(0,0) * pointFrom->x + transCur(0,1) * pointFrom->y + transCur(0,2) * pointFrom->z + transCur(0,3);\n cloudOut->points[i].y = transCur(1,0) * pointFrom->x + transCur(1,1) * pointFrom->y + transCur(1,2) * pointFrom->z + transCur(1,3);\n cloudOut->points[i].z = transCur(2,0) * pointFrom->x + transCur(2,1) * pointFrom->y + transCur(2,2) * pointFrom->z + transCur(2,3);\n cloudOut->points[i].intensity = pointFrom->intensity;\n }\n return cloudOut;\n} // transformPointCloud\n\nvoid loopFindNearKeyframesCloud( pcl::PointCloud::Ptr& nearKeyframes, const int& key, const int& submap_size, const int& root_idx)\n{\n // extract and stacking near keyframes (in global coord)\n nearKeyframes->clear();\n for (int i = -submap_size; i <= submap_size; ++i) {\n int keyNear = key + i;\n if (keyNear < 0 || keyNear >= int(keyframeLaserClouds.size()) )\n continue;\n\n mKF.lock(); \n *nearKeyframes += * local2global(keyframeLaserClouds[keyNear], keyframePosesUpdated[root_idx]);\n mKF.unlock(); \n }\n\n if (nearKeyframes->empty())\n return;\n\n // downsample near keyframes\n pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud());\n downSizeFilterICP.setInputCloud(nearKeyframes);\n downSizeFilterICP.filter(*cloud_temp);\n *nearKeyframes = *cloud_temp;\n} // loopFindNearKeyframesCloud\n\n\nstd::optional doICPVirtualRelative( int _loop_kf_idx, int _curr_kf_idx )\n{\n // parse pointclouds\n int historyKeyframeSearchNum = 25; // enough. ex. [-25, 25] covers submap length of 50x1 = 50m if every kf gap is 1m\n pcl::PointCloud::Ptr cureKeyframeCloud(new pcl::PointCloud());\n pcl::PointCloud::Ptr targetKeyframeCloud(new pcl::PointCloud());\n loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0, _loop_kf_idx); // use same root of loop kf idx \n loopFindNearKeyframesCloud(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum, _loop_kf_idx); \n\n // loop verification \n sensor_msgs::PointCloud2 cureKeyframeCloudMsg;\n pcl::toROSMsg(*cureKeyframeCloud, cureKeyframeCloudMsg);\n cureKeyframeCloudMsg.header.frame_id = \"camera_init\";\n pubLoopScanLocal.publish(cureKeyframeCloudMsg);\n\n sensor_msgs::PointCloud2 targetKeyframeCloudMsg;\n pcl::toROSMsg(*targetKeyframeCloud, targetKeyframeCloudMsg);\n targetKeyframeCloudMsg.header.frame_id = \"camera_init\";\n pubLoopSubmapLocal.publish(targetKeyframeCloudMsg);\n\n // ICP Settings\n pcl::IterativeClosestPoint icp;\n icp.setMaxCorrespondenceDistance(150); // giseop , use a value can cover 2*historyKeyframeSearchNum range in meter \n icp.setMaximumIterations(100);\n icp.setTransformationEpsilon(1e-6);\n icp.setEuclideanFitnessEpsilon(1e-6);\n icp.setRANSACIterations(0);\n\n // Align pointclouds\n icp.setInputSource(cureKeyframeCloud);\n icp.setInputTarget(targetKeyframeCloud);\n pcl::PointCloud::Ptr unused_result(new pcl::PointCloud());\n icp.align(*unused_result);\n \n float loopFitnessScoreThreshold = 0.3; // user parameter but fixed low value is safe. \n if (icp.hasConverged() == false || icp.getFitnessScore() > loopFitnessScoreThreshold) {\n std::cout << \"[SC loop] ICP fitness test failed (\" << icp.getFitnessScore() << \" > \" << loopFitnessScoreThreshold << \"). Reject this SC loop.\" << std::endl;\n return std::nullopt;\n } else {\n std::cout << \"[SC loop] ICP fitness test passed (\" << icp.getFitnessScore() << \" < \" << loopFitnessScoreThreshold << \"). Add this SC loop.\" << std::endl;\n }\n\n // Get pose transformation\n float x, y, z, roll, pitch, yaw;\n Eigen::Affine3f correctionLidarFrame;\n correctionLidarFrame = icp.getFinalTransformation();\n pcl::getTranslationAndEulerAngles (correctionLidarFrame, x, y, z, roll, pitch, yaw);\n gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));\n gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));\n\n return poseFrom.between(poseTo);\n} // doICPVirtualRelative\n\nvoid process_pg()\n{\n while(1)\n {\n\t\twhile ( !odometryBuf.empty() && !fullResBuf.empty() )\n {\n //\n // pop and check keyframe is or not \n // \n\t\t\tmBuf.lock(); \n while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < fullResBuf.front()->header.stamp.toSec())\n odometryBuf.pop();\n if (odometryBuf.empty())\n {\n mBuf.unlock();\n break;\n }\n\n // Time equal check\n timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();\n timeLaser = fullResBuf.front()->header.stamp.toSec();\n // TODO\n\n laserCloudFullRes->clear();\n pcl::PointCloud::Ptr thisKeyFrame(new pcl::PointCloud());\n pcl::fromROSMsg(*fullResBuf.front(), *thisKeyFrame);\n fullResBuf.pop();\n\n Pose6D pose_curr = getOdom(odometryBuf.front());\n odometryBuf.pop();\n\n // find nearest gps \n double eps = 0.1; // find a gps topioc arrived within eps second \n while (!gpsBuf.empty()) {\n auto thisGPS = gpsBuf.front();\n auto thisGPSTime = thisGPS->header.stamp.toSec();\n if( abs(thisGPSTime - timeLaserOdometry) < eps ) {\n currGPS = thisGPS;\n hasGPSforThisKF = true; \n break;\n } else {\n hasGPSforThisKF = false;\n }\n gpsBuf.pop();\n }\n mBuf.unlock(); \n\n //\n // Early reject by counting local delta movement (for equi-spereated kf drop)\n // \n odom_pose_prev = odom_pose_curr;\n odom_pose_curr = pose_curr;\n Pose6D dtf = diffTransformation(odom_pose_prev, odom_pose_curr); // dtf means delta_transform\n\n double delta_translation = sqrt(dtf.x*dtf.x + dtf.y*dtf.y + dtf.z*dtf.z); // note: absolute value. \n translationAccumulated += delta_translation;\n rotaionAccumulated += (dtf.roll + dtf.pitch + dtf.yaw); // sum just naive approach. \n\n if( translationAccumulated > keyframeMeterGap || rotaionAccumulated > keyframeRadGap ) {\n isNowKeyFrame = true;\n translationAccumulated = 0.0; // reset \n rotaionAccumulated = 0.0; // reset \n } else {\n isNowKeyFrame = false;\n }\n\n if( ! isNowKeyFrame ) \n continue; \n\n if( !gpsOffsetInitialized ) {\n if(hasGPSforThisKF) { // if the very first frame \n gpsAltitudeInitOffset = currGPS->altitude;\n gpsOffsetInitialized = true;\n } \n }\n\n //\n // Save data and Add consecutive node \n //\n pcl::PointCloud::Ptr thisKeyFrameDS(new pcl::PointCloud());\n downSizeFilterScancontext.setInputCloud(thisKeyFrame);\n downSizeFilterScancontext.filter(*thisKeyFrameDS);\n\n mKF.lock(); \n keyframeLaserClouds.push_back(thisKeyFrameDS);\n keyframePoses.push_back(pose_curr);\n keyframePosesUpdated.push_back(pose_curr); // init\n keyframeTimes.push_back(timeLaserOdometry);\n\n scManager.makeAndSaveScancontextAndKeys(*thisKeyFrameDS);\n\n laserCloudMapPGORedraw = true;\n mKF.unlock(); \n\n const int prev_node_idx = keyframePoses.size() - 2; \n 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)\n if( ! gtSAMgraphMade /* prior node */) {\n const int init_node_idx = 0; \n gtsam::Pose3 poseOrigin = Pose6DtoGTSAMPose3(keyframePoses.at(init_node_idx));\n // auto poseOrigin = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(0.0, 0.0, 0.0));\n\n mtxPosegraph.lock();\n {\n // prior factor \n gtSAMgraph.add(gtsam::PriorFactor(init_node_idx, poseOrigin, priorNoise));\n initialEstimate.insert(init_node_idx, poseOrigin);\n // runISAM2opt(); \n } \n mtxPosegraph.unlock();\n\n gtSAMgraphMade = true; \n\n cout << \"posegraph prior node \" << init_node_idx << \" added\" << endl;\n } else /* consecutive node (and odom factor) after the prior added */ { // == keyframePoses.size() > 1 \n gtsam::Pose3 poseFrom = Pose6DtoGTSAMPose3(keyframePoses.at(prev_node_idx));\n gtsam::Pose3 poseTo = Pose6DtoGTSAMPose3(keyframePoses.at(curr_node_idx));\n\n mtxPosegraph.lock();\n {\n // odom factor\n gtSAMgraph.add(gtsam::BetweenFactor(prev_node_idx, curr_node_idx, poseFrom.between(poseTo), odomNoise));\n\n // gps factor \n if(hasGPSforThisKF) {\n double curr_altitude_offseted = currGPS->altitude - gpsAltitudeInitOffset;\n mtxRecentPose.lock();\n gtsam::Point3 gpsConstraint(recentOptimizedX, recentOptimizedY, curr_altitude_offseted); // in this example, only adjusting altitude (for x and y, very big noises are set) \n mtxRecentPose.unlock();\n gtSAMgraph.add(gtsam::GPSFactor(curr_node_idx, gpsConstraint, robustGPSNoise));\n cout << \"GPS factor added at node \" << curr_node_idx << endl;\n }\n initialEstimate.insert(curr_node_idx, poseTo); \n // runISAM2opt();\n }\n mtxPosegraph.unlock();\n\n if(curr_node_idx % 100 == 0)\n cout << \"posegraph odom node \" << curr_node_idx << \" added.\" << endl;\n }\n // if want to print the current graph, use gtSAMgraph.print(\"\\nFactor Graph:\\n\");\n\n // save utility \n std::string curr_node_idx_str = padZeros(curr_node_idx);\n pcl::io::savePCDFileBinary(pgScansDirectory + curr_node_idx_str + \".pcd\", *thisKeyFrame); // scan \n pgTimeSaveStream << timeLaser << std::endl; // path \n }\n\n // ps. \n // scan context detector is running in another thread (in constant Hz, e.g., 1 Hz)\n // pub path and point cloud in another thread\n\n // wait (must required for running the while loop)\n std::chrono::milliseconds dura(2);\n std::this_thread::sleep_for(dura);\n }\n} // process_pg\n\nvoid performSCLoopClosure(void)\n{\n if( int(keyframePoses.size()) < scManager.NUM_EXCLUDE_RECENT) // do not try too early \n return;\n\n auto detectResult = scManager.detectLoopClosureID(); // first: nn index, second: yaw diff \n int SCclosestHistoryFrameID = detectResult.first;\n if( SCclosestHistoryFrameID != -1 ) { \n const int prev_node_idx = SCclosestHistoryFrameID;\n const int curr_node_idx = keyframePoses.size() - 1; // because cpp starts 0 and ends n-1\n cout << \"Loop detected! - between \" << prev_node_idx << \" and \" << curr_node_idx << \"\" << endl;\n\n mBuf.lock();\n scLoopICPBuf.push(std::pair(prev_node_idx, curr_node_idx));\n // addding actual 6D constraints in the other thread, icp_calculation.\n mBuf.unlock();\n }\n} // performSCLoopClosure\n\nvoid process_lcd(void)\n{\n float loopClosureFrequency = 1.0; // can change \n ros::Rate rate(loopClosureFrequency);\n while (ros::ok())\n {\n rate.sleep();\n performSCLoopClosure();\n // performRSLoopClosure(); // TODO\n }\n} // process_lcd\n\nvoid process_icp(void)\n{\n while(1)\n {\n\t\twhile ( !scLoopICPBuf.empty() )\n {\n if( scLoopICPBuf.size() > 30 ) {\n ROS_WARN(\"Too many loop clousre candidates to be ICPed is waiting ... Do process_lcd less frequently (adjust loopClosureFrequency)\");\n }\n\n mBuf.lock(); \n std::pair loop_idx_pair = scLoopICPBuf.front();\n scLoopICPBuf.pop();\n mBuf.unlock(); \n\n const int prev_node_idx = loop_idx_pair.first;\n const int curr_node_idx = loop_idx_pair.second;\n auto relative_pose_optional = doICPVirtualRelative(prev_node_idx, curr_node_idx);\n if(relative_pose_optional) {\n gtsam::Pose3 relative_pose = relative_pose_optional.value();\n mtxPosegraph.lock();\n gtSAMgraph.add(gtsam::BetweenFactor(prev_node_idx, curr_node_idx, relative_pose, robustLoopNoise));\n // runISAM2opt();\n mtxPosegraph.unlock();\n } \n }\n\n // wait (must required for running the while loop)\n std::chrono::milliseconds dura(2);\n std::this_thread::sleep_for(dura);\n }\n} // process_icp\n\nvoid process_viz_path(void)\n{\n float hz = 10.0; \n ros::Rate rate(hz);\n while (ros::ok()) {\n rate.sleep();\n if(recentIdxUpdated > 1) {\n pubPath();\n }\n }\n}\n\nvoid process_isam(void)\n{\n float hz = 1; \n ros::Rate rate(hz);\n while (ros::ok()) {\n rate.sleep();\n if( gtSAMgraphMade ) {\n mtxPosegraph.lock();\n runISAM2opt();\n cout << \"running isam2 optimization ...\" << endl;\n mtxPosegraph.unlock();\n\n saveOptimizedVerticesKITTIformat(isamCurrentEstimate, pgKITTIformat); // pose\n saveOdometryVerticesKITTIformat(odomKITTIformat); // pose\n }\n }\n}\n\nvoid pubMap(void)\n{\n int SKIP_FRAMES = 2; // sparse map visulalization to save computations \n int counter = 0;\n\n laserCloudMapPGO->clear();\n\n mKF.lock(); \n // for (int node_idx=0; node_idx < int(keyframePosesUpdated.size()); node_idx++) {\n for (int node_idx=0; node_idx < recentIdxUpdated; node_idx++) {\n if(counter % SKIP_FRAMES == 0) {\n *laserCloudMapPGO += *local2global(keyframeLaserClouds[node_idx], keyframePosesUpdated[node_idx]);\n }\n counter++;\n }\n mKF.unlock(); \n\n downSizeFilterMapPGO.setInputCloud(laserCloudMapPGO);\n downSizeFilterMapPGO.filter(*laserCloudMapPGO);\n\n sensor_msgs::PointCloud2 laserCloudMapPGOMsg;\n pcl::toROSMsg(*laserCloudMapPGO, laserCloudMapPGOMsg);\n laserCloudMapPGOMsg.header.frame_id = \"camera_init\";\n pubMapAftPGO.publish(laserCloudMapPGOMsg);\n}\n\nvoid process_viz_map(void)\n{\n float vizmapFrequency = 0.1; // 0.1 means run onces every 10s\n ros::Rate rate(vizmapFrequency);\n while (ros::ok()) {\n rate.sleep();\n if(recentIdxUpdated > 1) {\n pubMap();\n }\n }\n} // pointcloud_viz\n\n\nint main(int argc, char **argv)\n{\n\tros::init(argc, argv, \"laserPGO\");\n\tros::NodeHandle nh;\n\n\tnh.param(\"save_directory\", save_directory, \"/\"); // pose assignment every k m move \n pgKITTIformat = save_directory + \"optimized_poses.txt\";\n odomKITTIformat = save_directory + \"odom_poses.txt\";\n pgTimeSaveStream = std::fstream(save_directory + \"times.txt\", std::fstream::out); \n pgTimeSaveStream.precision(std::numeric_limits::max_digits10);\n pgScansDirectory = save_directory + \"Scans/\";\n auto unused = system((std::string(\"exec rm -r \") + pgScansDirectory).c_str());\n unused = system((std::string(\"mkdir -p \") + pgScansDirectory).c_str());\n\n\tnh.param(\"keyframe_meter_gap\", keyframeMeterGap, 2.0); // pose assignment every k m move \n\tnh.param(\"keyframe_deg_gap\", keyframeDegGap, 10.0); // pose assignment every k deg rot \n keyframeRadGap = deg2rad(keyframeDegGap);\n\n\tnh.param(\"sc_dist_thres\", scDistThres, 0.2); \n\tnh.param(\"sc_max_radius\", scMaximumRadius, 80.0); // 80 is recommended for outdoor, and lower (ex, 20, 40) values are recommended for indoor \n\n ISAM2Params parameters;\n parameters.relinearizeThreshold = 0.01;\n parameters.relinearizeSkip = 1;\n isam = new ISAM2(parameters);\n initNoises();\n\n scManager.setSCdistThres(scDistThres);\n scManager.setMaximumRadius(scMaximumRadius);\n\n float filter_size = 0.4; \n downSizeFilterScancontext.setLeafSize(filter_size, filter_size, filter_size);\n downSizeFilterICP.setLeafSize(filter_size, filter_size, filter_size);\n\n double mapVizFilterSize;\n\tnh.param(\"mapviz_filter_size\", mapVizFilterSize, 0.4); // pose assignment every k frames \n downSizeFilterMapPGO.setLeafSize(mapVizFilterSize, mapVizFilterSize, mapVizFilterSize);\n\n\tros::Subscriber subLaserCloudFullRes = nh.subscribe(\"/velodyne_cloud_registered_local\", 100, laserCloudFullResHandler);\n\tros::Subscriber subLaserOdometry = nh.subscribe(\"/aft_mapped_to_init\", 100, laserOdometryHandler);\n\tros::Subscriber subGPS = nh.subscribe(\"/gps/fix\", 100, gpsHandler);\n\n\tpubOdomAftPGO = nh.advertise(\"/aft_pgo_odom\", 100);\n\tpubOdomRepubVerifier = nh.advertise(\"/repub_odom\", 100);\n\tpubPathAftPGO = nh.advertise(\"/aft_pgo_path\", 100);\n\tpubMapAftPGO = nh.advertise(\"/aft_pgo_map\", 100);\n\n\tpubLoopScanLocal = nh.advertise(\"/loop_scan_local\", 100);\n\tpubLoopSubmapLocal = nh.advertise(\"/loop_submap_local\", 100);\n\n\tstd::thread posegraph_slam {process_pg}; // pose graph construction\n\tstd::thread lc_detection {process_lcd}; // loop closure detection \n\tstd::thread icp_calculation {process_icp}; // loop constraint calculation via icp \n\tstd::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. \n\n\tstd::thread viz_map {process_viz_map}; // visualization - map (low frequency because it is heavy)\n\tstd::thread viz_path {process_viz_path}; // visualization - path (high frequency)\n\n \tros::spin();\n\n\treturn 0;\n}\n" }, { "path": "SC-PGO/src/lidarFactor.hpp", "content": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nstruct LidarEdgeFactor\n{\n\tLidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,\n\t\t\t\t\tEigen::Vector3d last_point_b_, double s_)\n\t\t: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}\n\n\ttemplate \n\tbool operator()(const T *q, const T *t, T *residual) const\n\t{\n\n\t\tEigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};\n\t\tEigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};\n\t\tEigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};\n\n\t\t//Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};\n\t\tEigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};\n\t\tEigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};\n\t\tq_last_curr = q_identity.slerp(T(s), q_last_curr);\n\t\tEigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};\n\n\t\tEigen::Matrix lp;\n\t\tlp = q_last_curr * cp + t_last_curr;\n\n\t\tEigen::Matrix nu = (lp - lpa).cross(lp - lpb);\n\t\tEigen::Matrix de = lpa - lpb;\n\n\t\tresidual[0] = nu.x() / de.norm();\n\t\tresidual[1] = nu.y() / de.norm();\n\t\tresidual[2] = nu.z() / de.norm();\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,\n\t\t\t\t\t\t\t\t\t const Eigen::Vector3d last_point_b_, const double s_)\n\t{\n\t\treturn (new ceres::AutoDiffCostFunction<\n\t\t\t\tLidarEdgeFactor, 3, 4, 3>(\n\t\t\tnew LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));\n\t}\n\n\tEigen::Vector3d curr_point, last_point_a, last_point_b;\n\tdouble s;\n};\n\nstruct LidarPlaneFactor\n{\n\tLidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,\n\t\t\t\t\t Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)\n\t\t: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),\n\t\t last_point_m(last_point_m_), s(s_)\n\t{\n\t\tljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);\n\t\tljm_norm.normalize();\n\t}\n\n\ttemplate \n\tbool operator()(const T *q, const T *t, T *residual) const\n\t{\n\n\t\tEigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};\n\t\tEigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};\n\t\t//Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};\n\t\t//Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};\n\t\tEigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};\n\n\t\t//Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};\n\t\tEigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};\n\t\tEigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};\n\t\tq_last_curr = q_identity.slerp(T(s), q_last_curr);\n\t\tEigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};\n\n\t\tEigen::Matrix lp;\n\t\tlp = q_last_curr * cp + t_last_curr;\n\n\t\tresidual[0] = (lp - lpj).dot(ljm);\n\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,\n\t\t\t\t\t\t\t\t\t const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,\n\t\t\t\t\t\t\t\t\t const double s_)\n\t{\n\t\treturn (new ceres::AutoDiffCostFunction<\n\t\t\t\tLidarPlaneFactor, 1, 4, 3>(\n\t\t\tnew LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));\n\t}\n\n\tEigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;\n\tEigen::Vector3d ljm_norm;\n\tdouble s;\n};\n\nstruct LidarPlaneNormFactor\n{\n\n\tLidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,\n\t\t\t\t\t\t double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t negative_OA_dot_norm(negative_OA_dot_norm_) {}\n\n\ttemplate \n\tbool operator()(const T *q, const T *t, T *residual) const\n\t{\n\t\tEigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]};\n\t\tEigen::Matrix t_w_curr{t[0], t[1], t[2]};\n\t\tEigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};\n\t\tEigen::Matrix point_w;\n\t\tpoint_w = q_w_curr * cp + t_w_curr;\n\n\t\tEigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));\n\t\tresidual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_,\n\t\t\t\t\t\t\t\t\t const double negative_OA_dot_norm_)\n\t{\n\t\treturn (new ceres::AutoDiffCostFunction<\n\t\t\t\tLidarPlaneNormFactor, 1, 4, 3>(\n\t\t\tnew LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));\n\t}\n\n\tEigen::Vector3d curr_point;\n\tEigen::Vector3d plane_unit_norm;\n\tdouble negative_OA_dot_norm;\n};\n\n\nstruct LidarDistanceFactor\n{\n\n\tLidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) \n\t\t\t\t\t\t: curr_point(curr_point_), closed_point(closed_point_){}\n\n\ttemplate \n\tbool operator()(const T *q, const T *t, T *residual) const\n\t{\n\t\tEigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]};\n\t\tEigen::Matrix t_w_curr{t[0], t[1], t[2]};\n\t\tEigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};\n\t\tEigen::Matrix point_w;\n\t\tpoint_w = q_w_curr * cp + t_w_curr;\n\n\n\t\tresidual[0] = point_w.x() - T(closed_point.x());\n\t\tresidual[1] = point_w.y() - T(closed_point.y());\n\t\tresidual[2] = point_w.z() - T(closed_point.z());\n\t\treturn true;\n\t}\n\n\tstatic ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_)\n\t{\n\t\treturn (new ceres::AutoDiffCostFunction<\n\t\t\t\tLidarDistanceFactor, 3, 4, 3>(\n\t\t\tnew LidarDistanceFactor(curr_point_, closed_point_)));\n\t}\n\n\tEigen::Vector3d curr_point;\n\tEigen::Vector3d closed_point;\n};" }, { "path": "SC-PGO/src/scanRegistration.cpp", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. \n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n\n#include \n#include \n#include \n#include \"aloam_velodyne/common.h\"\n#include \"aloam_velodyne/tic_toc.h\"\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nusing std::atan2;\nusing std::cos;\nusing std::sin;\n\nstd::string LIDAR_TYPE;\n\nconst double scanPeriod = 0.1;\n\nconst int systemDelay = 0; \nint systemInitCount = 0;\nbool systemInited = false;\nint N_SCANS = 0;\nfloat cloudCurvature[400000];\nint cloudSortInd[400000];\nint cloudNeighborPicked[400000];\nint cloudLabel[400000];\n\nbool comp (int i,int j) { return (cloudCurvature[i] pubEachScan;\n\nbool PUB_EACH_LINE = false;\n\ndouble MINIMUM_RANGE = 0.1; \n\ntemplate \nvoid removeClosedPointCloud(const pcl::PointCloud &cloud_in,\n pcl::PointCloud &cloud_out, float thres)\n{\n if (&cloud_in != &cloud_out)\n {\n cloud_out.header = cloud_in.header;\n cloud_out.points.resize(cloud_in.points.size());\n }\n\n size_t j = 0;\n\n for (size_t i = 0; i < cloud_in.points.size(); ++i)\n {\n 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)\n continue;\n cloud_out.points[j] = cloud_in.points[i];\n j++;\n }\n if (j != cloud_in.points.size())\n {\n cloud_out.points.resize(j);\n }\n\n cloud_out.height = 1;\n cloud_out.width = static_cast(j);\n cloud_out.is_dense = true;\n}\n\nvoid laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)\n{\n if (!systemInited)\n { \n systemInitCount++;\n if (systemInitCount >= systemDelay)\n {\n systemInited = true;\n }\n else\n return;\n }\n\n TicToc t_whole;\n TicToc t_prepare;\n std::vector scanStartInd(N_SCANS, 0);\n std::vector scanEndInd(N_SCANS, 0);\n\n pcl::PointCloud laserCloudIn;\n pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);\n std::vector indices;\n\n pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);\n removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);\n\n\n int cloudSize = laserCloudIn.points.size();\n float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);\n float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,\n laserCloudIn.points[cloudSize - 1].x) +\n 2 * M_PI;\n\n if (endOri - startOri > 3 * M_PI)\n {\n endOri -= 2 * M_PI;\n }\n else if (endOri - startOri < M_PI)\n {\n endOri += 2 * M_PI;\n }\n ////printf(\"end Ori %f\\n\", endOri);\n\n bool halfPassed = false;\n int count = cloudSize;\n PointType point;\n std::vector> laserCloudScans(N_SCANS);\n for (int i = 0; i < cloudSize; i++)\n {\n point.x = laserCloudIn.points[i].x;\n point.y = laserCloudIn.points[i].y;\n point.z = laserCloudIn.points[i].z;\n\n float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;\n int scanID = 0;\n\n if (LIDAR_TYPE == \"VLP16\" && N_SCANS == 16)\n {\n scanID = int((angle + 15) / 2 + 0.5);\n if (scanID > (N_SCANS - 1) || scanID < 0)\n {\n count--;\n continue;\n }\n }\n else if (LIDAR_TYPE == \"HDL32\" && N_SCANS == 32)\n {\n scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);\n if (scanID > (N_SCANS - 1) || scanID < 0)\n {\n count--;\n continue;\n }\n }\n // HDL64 (e.g., KITTI)\n else if (LIDAR_TYPE == \"HDL64\" && N_SCANS == 64)\n { \n if (angle >= -8.83)\n scanID = int((2 - angle) * 3.0 + 0.5);\n else\n scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);\n\n // use [0 50] > 50 remove outlies \n if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)\n {\n count--;\n continue;\n }\n }\n // Ouster OS1-64 (e.g., MulRan)\n else if (LIDAR_TYPE == \"OS1-64\" && N_SCANS == 64)\n { \n 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/\n if (scanID > (N_SCANS - 1) || scanID < 0)\n {\n count--;\n continue;\n }\n }\n else\n {\n //printf(\"wrong scan number\\n\");\n ROS_BREAK();\n }\n ////printf(\"angle %f scanID %d \\n\", angle, scanID);\n\n float ori = -atan2(point.y, point.x);\n if (!halfPassed)\n { \n if (ori < startOri - M_PI / 2)\n {\n ori += 2 * M_PI;\n }\n else if (ori > startOri + M_PI * 3 / 2)\n {\n ori -= 2 * M_PI;\n }\n\n if (ori - startOri > M_PI)\n {\n halfPassed = true;\n }\n }\n else\n {\n ori += 2 * M_PI;\n if (ori < endOri - M_PI * 3 / 2)\n {\n ori += 2 * M_PI;\n }\n else if (ori > endOri + M_PI / 2)\n {\n ori -= 2 * M_PI;\n }\n }\n\n float relTime = (ori - startOri) / (endOri - startOri);\n point.intensity = scanID + scanPeriod * relTime;\n laserCloudScans[scanID].push_back(point); \n }\n \n cloudSize = count;\n //printf(\"points size %d \\n\", cloudSize);\n\n pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());\n for (int i = 0; i < N_SCANS; i++)\n { \n scanStartInd[i] = laserCloud->size() + 5;\n *laserCloud += laserCloudScans[i];\n scanEndInd[i] = laserCloud->size() - 6;\n }\n\n //printf(\"prepare time %f \\n\", t_prepare.toc());\n\n for (int i = 5; i < cloudSize - 5; i++)\n { \n 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;\n 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;\n 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;\n\n cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;\n cloudSortInd[i] = i;\n cloudNeighborPicked[i] = 0;\n cloudLabel[i] = 0;\n }\n\n\n TicToc t_pts;\n\n pcl::PointCloud cornerPointsSharp;\n pcl::PointCloud cornerPointsLessSharp;\n pcl::PointCloud surfPointsFlat;\n pcl::PointCloud surfPointsLessFlat;\n\n float t_q_sort = 0;\n for (int i = 0; i < N_SCANS; i++)\n {\n if( scanEndInd[i] - scanStartInd[i] < 6)\n continue;\n pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);\n for (int j = 0; j < 6; j++)\n {\n int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; \n int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;\n\n TicToc t_tmp;\n std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);\n t_q_sort += t_tmp.toc();\n\n int largestPickedNum = 0;\n for (int k = ep; k >= sp; k--)\n {\n int ind = cloudSortInd[k]; \n\n if (cloudNeighborPicked[ind] == 0 &&\n cloudCurvature[ind] > 0.1)\n {\n\n largestPickedNum++;\n if (largestPickedNum <= 2)\n { \n cloudLabel[ind] = 2;\n cornerPointsSharp.push_back(laserCloud->points[ind]);\n cornerPointsLessSharp.push_back(laserCloud->points[ind]);\n }\n else if (largestPickedNum <= 20)\n { \n cloudLabel[ind] = 1; \n cornerPointsLessSharp.push_back(laserCloud->points[ind]);\n }\n else\n {\n break;\n }\n\n cloudNeighborPicked[ind] = 1; \n\n for (int l = 1; l <= 5; l++)\n {\n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n for (int l = -1; l >= -5; l--)\n {\n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n }\n }\n\n int smallestPickedNum = 0;\n for (int k = sp; k <= ep; k++)\n {\n int ind = cloudSortInd[k];\n\n if (cloudNeighborPicked[ind] == 0 &&\n cloudCurvature[ind] < 0.1)\n {\n\n cloudLabel[ind] = -1; \n surfPointsFlat.push_back(laserCloud->points[ind]);\n\n smallestPickedNum++;\n if (smallestPickedNum >= 4)\n { \n break;\n }\n\n cloudNeighborPicked[ind] = 1;\n for (int l = 1; l <= 5; l++)\n { \n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n for (int l = -1; l >= -5; l--)\n {\n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n }\n }\n\n for (int k = sp; k <= ep; k++)\n {\n if (cloudLabel[k] <= 0)\n {\n surfPointsLessFlatScan->push_back(laserCloud->points[k]);\n }\n }\n }\n\n pcl::PointCloud surfPointsLessFlatScanDS;\n pcl::VoxelGrid downSizeFilter;\n downSizeFilter.setInputCloud(surfPointsLessFlatScan);\n downSizeFilter.setLeafSize(0.2, 0.2, 0.2);\n downSizeFilter.filter(surfPointsLessFlatScanDS);\n\n surfPointsLessFlat += surfPointsLessFlatScanDS;\n }\n //printf(\"sort q time %f \\n\", t_q_sort);\n //printf(\"seperate points time %f \\n\", t_pts.toc());\n\n\n sensor_msgs::PointCloud2 laserCloudOutMsg;\n pcl::toROSMsg(*laserCloud, laserCloudOutMsg);\n laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;\n laserCloudOutMsg.header.frame_id = \"camera_init\";\n pubLaserCloud.publish(laserCloudOutMsg);\n\n sensor_msgs::PointCloud2 cornerPointsSharpMsg;\n pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);\n cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;\n cornerPointsSharpMsg.header.frame_id = \"camera_init\";\n pubCornerPointsSharp.publish(cornerPointsSharpMsg);\n\n sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;\n pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);\n cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;\n cornerPointsLessSharpMsg.header.frame_id = \"camera_init\";\n pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);\n\n sensor_msgs::PointCloud2 surfPointsFlat2;\n pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);\n surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;\n surfPointsFlat2.header.frame_id = \"camera_init\";\n pubSurfPointsFlat.publish(surfPointsFlat2);\n\n sensor_msgs::PointCloud2 surfPointsLessFlat2;\n pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);\n surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;\n surfPointsLessFlat2.header.frame_id = \"camera_init\";\n pubSurfPointsLessFlat.publish(surfPointsLessFlat2);\n // std::cout << \"published ... \" << std::endl;\n\n // pub each scam\n if(PUB_EACH_LINE)\n {\n for(int i = 0; i< N_SCANS; i++)\n {\n sensor_msgs::PointCloud2 scanMsg;\n pcl::toROSMsg(laserCloudScans[i], scanMsg);\n scanMsg.header.stamp = laserCloudMsg->header.stamp;\n scanMsg.header.frame_id = \"camera_init\";\n pubEachScan[i].publish(scanMsg);\n }\n }\n\n //printf(\"scan registration time %f ms *************\\n\", t_whole.toc());\n if(t_whole.toc() > 100)\n ROS_WARN(\"scan registration process over 100ms\");\n}\n\nint main(int argc, char **argv)\n{\n ros::init(argc, argv, \"scanRegistration\");\n ros::NodeHandle nh;\n\n nh.param(\"scan_line\", N_SCANS, 16);\n nh.param(\"lidar_type\", LIDAR_TYPE, \"KITTI\");\n nh.param(\"minimum_range\", MINIMUM_RANGE, 0.1);\n\n //printf(\"scan line number %d \\n\", N_SCANS);\n\n if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)\n {\n //printf(\"only support velodyne with 16, 32 or 64 scan line!\");\n return 0;\n }\n\n ros::Subscriber subLaserCloud = nh.subscribe(\"/velodyne_points\", 100, laserCloudHandler);\n\n pubLaserCloud = nh.advertise(\"/velodyne_cloud_2\", 100);\n\n pubCornerPointsSharp = nh.advertise(\"/laser_cloud_sharp\", 100);\n\n pubCornerPointsLessSharp = nh.advertise(\"/laser_cloud_less_sharp\", 100);\n\n pubSurfPointsFlat = nh.advertise(\"/laser_cloud_flat\", 100);\n\n pubSurfPointsLessFlat = nh.advertise(\"/laser_cloud_less_flat\", 100);\n\n pubRemovePoints = nh.advertise(\"/laser_remove_points\", 100);\n\n if(PUB_EACH_LINE)\n {\n for(int i = 0; i < N_SCANS; i++)\n {\n ros::Publisher tmp = nh.advertise(\"/laser_scanid_\" + std::to_string(i), 100);\n pubEachScan.push_back(tmp);\n }\n }\n ros::spin();\n\n return 0;\n}\n" }, { "path": "SC-PGO/utils/python/makeMergedMap.py", "content": "import os \nimport sys\nimport time \nimport copy \nfrom io import StringIO\n\nimport pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git\nfrom pypcd import pypcd\n\nimport numpy as np\nfrom numpy import linalg as LA\n\nimport open3d as o3d\n\nfrom pypcdMyUtils import * \n\njet_table = np.load('jet_table.npy')\nbone_table = np.load('bone_table.npy')\n\ncolor_table = jet_table\ncolor_table_len = color_table.shape[0]\n\n\n##########################\n# User only consider this block\n##########################\n\ndata_dir = \"/home/user/Documents/catkin2021/catkin_fastlio2/data/\" # should end with / \nscan_idx_range_to_stack = [0, 200] # if you want a whole map, use [0, len(scan_files)]\nnode_skip = 1\n\nnum_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.\n\nis_live_vis = False # recommend to use false \nis_o3d_vis = True\nintensity_color_max = 200\n\nis_near_removal = True\nthres_near_removal = 2 # meter (to remove platform-myself structure ghost points)\n\n##########################\n\n\n#\nscan_dir = data_dir + \"Scans\"\nscan_files = os.listdir(scan_dir) \nscan_files.sort()\n\nposes = []\nf = open(data_dir+\"optimized_poses.txt\", 'r')\nwhile True:\n line = f.readline()\n if not line: break\n pose_SE3 = np.asarray([float(i) for i in line.split()])\n pose_SE3 = np.vstack( (np.reshape(pose_SE3, (3, 4)), np.asarray([0,0,0,1])) )\n poses.append(pose_SE3)\nf.close()\n\n\n#\nassert (scan_idx_range_to_stack[1] > scan_idx_range_to_stack[0])\nprint(\"Merging scans from\", scan_idx_range_to_stack[0], \"to\", scan_idx_range_to_stack[1])\n\n\n#\nif(is_live_vis):\n vis = o3d.visualization.Visualizer() \n vis.create_window('Map', visible = True) \n\nnodes_count = 0\npcd_combined_for_vis = o3d.geometry.PointCloud()\npcd_combined_for_save = None\n\n# The scans from 000000.pcd should be prepared if it is not used (because below code indexing is designed in a naive way)\n\n# manually reserve memory for fast write \nnum_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))\n\nnp_xyz_all = np.empty([num_all_points_expected, 3])\nnp_intensity_all = np.empty([num_all_points_expected, 1])\ncurr_count = 0\n\nfor node_idx in range(len(scan_files)):\n if(node_idx < scan_idx_range_to_stack[0] or node_idx >= scan_idx_range_to_stack[1]):\n continue\n\n nodes_count = nodes_count + 1\n if( nodes_count % node_skip is not 0): \n if(node_idx is not scan_idx_range_to_stack[0]): # to ensure the vis init \n continue\n\n print(\"read keyframe scan idx\", node_idx)\n\n scan_pose = poses[node_idx]\n\n scan_path = os.path.join(scan_dir, scan_files[node_idx])\n scan_pcd = o3d.io.read_point_cloud(scan_path)\n scan_xyz_local = copy.deepcopy(np.asarray(scan_pcd.points))\n\n scan_pypcd_with_intensity = pypcd.PointCloud.from_path(scan_path)\n scan_intensity = scan_pypcd_with_intensity.pc_data['intensity']\n scan_intensity_colors_idx = np.round( (color_table_len-1) * np.minimum( 1, np.maximum(0, scan_intensity / intensity_color_max) ) )\n scan_intensity_colors = color_table[scan_intensity_colors_idx.astype(int)]\n\n scan_pcd_global = scan_pcd.transform(scan_pose) # global coord, note that this is not deepcopy\n scan_pcd_global.colors = o3d.utility.Vector3dVector(scan_intensity_colors)\n scan_xyz = np.asarray(scan_pcd_global.points)\n\n scan_intensity = np.expand_dims(scan_intensity, axis=1) \n scan_ranges = LA.norm(scan_xyz_local, axis=1)\n\n if(is_near_removal):\n eff_idxes = np.where (scan_ranges > thres_near_removal)\n scan_xyz = scan_xyz[eff_idxes[0], :]\n scan_intensity = scan_intensity[eff_idxes[0], :]\n\n scan_pcd_global = scan_pcd_global.select_by_index(eff_idxes[0])\n\n if(is_o3d_vis):\n pcd_combined_for_vis += scan_pcd_global # open3d pointcloud class append is fast \n\n if is_live_vis:\n if(node_idx is scan_idx_range_to_stack[0]): # to ensure the vis init \n vis.add_geometry(pcd_combined_for_vis) \n\n vis.update_geometry(pcd_combined_for_vis)\n vis.poll_events()\n vis.update_renderer()\n\n # save \n np_xyz_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_xyz\n np_intensity_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_intensity\n\n curr_count = curr_count + scan_xyz.shape[0]\n print(curr_count)\n \n#\nif(is_o3d_vis):\n print(\"draw the merged map.\")\n o3d.visualization.draw_geometries([pcd_combined_for_vis])\n\n\n# save ply having intensity\nnp_xyz_all = np_xyz_all[0:curr_count, :]\nnp_intensity_all = np_intensity_all[0:curr_count, :]\n\nnp_xyzi_all = np.hstack( (np_xyz_all, np_intensity_all) )\nxyzi = make_xyzi_point_cloud(np_xyzi_all)\n\nmap_name = data_dir + \"map_\" + str(scan_idx_range_to_stack[0]) + \"_to_\" + str(scan_idx_range_to_stack[1]) + \"_with_intensity.pcd\"\nxyzi.save_pcd(map_name, compression='binary_compressed')\nprint(\"intensity map is save (path:\", map_name, \")\")\n\n# save rgb colored points \n# map_name = data_dir + \"map_\" + str(scan_idx_range_to_stack[0]) + \"_to_\" + str(scan_idx_range_to_stack[1]) + \".pcd\"\n# o3d.io.write_point_cloud(map_name, pcd_combined_for_vis)\n# print(\"the map is save (path:\", map_name, \")\")\n\n\n" }, { "path": "SC-PGO/utils/python/pypcdMyUtils.py", "content": "import numpy as np\nimport pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git\nfrom pypcd import pypcd\n\ndef make_xyzi_point_cloud(xyzl, label_type='f'):\n \"\"\" Make XYZL point cloud from numpy array.\n TODO i labels?\n \"\"\"\n md = {'version': .7,\n 'fields': ['x', 'y', 'z', 'intensity'],\n 'count': [1, 1, 1, 1],\n 'width': len(xyzl),\n 'height': 1,\n 'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],\n 'points': len(xyzl),\n 'data': 'ASCII'}\n if label_type.lower() == 'f':\n md['size'] = [4, 4, 4, 4]\n md['type'] = ['F', 'F', 'F', 'F']\n elif label_type.lower() == 'u':\n md['size'] = [4, 4, 4, 1]\n md['type'] = ['F', 'F', 'F', 'U']\n else:\n raise ValueError('label type must be F or U')\n # TODO use .view()\n xyzl = xyzl.astype(np.float32)\n dt = np.dtype([('x', np.float32), ('y', np.float32), ('z', np.float32),\n ('intensity', np.float32)])\n pc_data = np.rec.fromarrays([xyzl[:, 0], xyzl[:, 1], xyzl[:, 2],\n xyzl[:, 3]], dtype=dt)\n pc = pypcd.PointCloud(md, pc_data)\n return pc\n" }, { "path": "SC-PGO/utils/sample_data/KAIST03/optimized_poses.txt", "content": "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\n0.999995 -0.00183662 0.00242372 -0.830723 0.00183814 0.999998 -0.000626489 -0.0702528 -0.00242256 0.000630942 0.999997 0.028506\n0.999955 -0.00619254 0.00712886 -2.0279 0.00621328 0.999977 -0.00289042 -0.0890035 -0.0071108 0.00293459 0.99997 0.0611351\n0.999896 -0.0101634 0.0102549 -3.23604 0.0101777 0.999947 -0.00135147 -0.094529 -0.0102406 0.0014557 0.999947 0.0943558\n0.999925 -0.0105464 0.00615898 -4.43987 0.0105666 0.999939 -0.00325993 -0.111933 -0.00612422 0.00332476 0.999976 0.112778\n0.999922 -0.0116081 0.00466198 -5.45798 0.0116165 0.999931 -0.00177551 -0.129743 -0.00464105 0.00182953 0.999988 0.149339\n0.999908 -0.0131164 0.00351053 -6.57744 0.0131274 0.999909 -0.00312253 -0.154525 -0.00346926 0.00316832 0.999989 0.16777\n0.999879 -0.0153974 0.00217493 -7.812 0.0153993 0.999881 -0.000849054 -0.157814 -0.0021616 0.000882443 0.999997 0.201606\n0.999857 -0.0163638 0.0041903 -9.13307 0.0163833 0.999855 -0.00466874 -0.194597 -0.00411329 0.00473672 0.99998 0.235488\n0.999829 -0.0177863 0.00496165 -10.5694 0.0178 0.999838 -0.00272271 -0.203606 -0.00491242 0.00281056 0.999984 0.267049\n0.99982 -0.018772 0.00264324 -11.5779 0.018783 0.999815 -0.00422143 -0.229772 -0.00256351 0.00427032 0.999988 0.292312\n0.999811 -0.0194355 0.000538649 -12.6198 0.0194381 0.999797 -0.00522362 -0.256755 -0.000437016 0.0052331 0.999986 0.309011\n0.999788 -0.0205736 0.00112994 -13.7042 0.0205779 0.999781 -0.00390498 -0.268662 -0.00104935 0.0039274 0.999992 0.335767\n0.999785 -0.0206704 0.00141779 -14.8214 0.0206772 0.999774 -0.0050044 -0.297563 -0.00131403 0.00503264 0.999986 0.362394\n0.999758 -0.0219373 -0.00176261 -15.982 0.0219281 0.999747 -0.005058 -0.321281 0.00187312 0.00501812 0.999986 0.37218\n0.999764 -0.0213675 -0.00404201 -17.1689 0.021348 0.999761 -0.00478866 -0.337659 0.00414336 0.00470124 0.99998 0.406462\n0.999803 -0.0196226 -0.00289677 -18.3763 0.0196064 0.999793 -0.00551055 -0.359554 0.0030043 0.00545267 0.999981 0.424904\n0.999839 -0.0178471 -0.00186246 -19.6187 0.0178382 0.99983 -0.00465797 -0.376691 0.00194528 0.004624 0.999987 0.439104\n0.99987 -0.01585 -0.00300788 -20.9112 0.0158355 0.999863 -0.00479842 -0.386074 0.00308352 0.00475017 0.999984 0.464757\n0.999895 -0.0142657 -0.00267759 -22.2296 0.0142546 0.99989 -0.00409914 -0.405231 0.00273578 0.00406054 0.999988 0.492367\n0.999922 -0.0122191 -0.00273686 -23.5692 0.0122053 0.999913 -0.0050065 -0.412837 0.00279779 0.00497271 0.999984 0.509906\n0.999928 -0.0102724 -0.00615312 -24.9569 0.0102525 0.999942 -0.0032604 -0.417687 0.00618626 0.00319709 0.999976 0.527312\n0.99994 -0.00833045 -0.00717445 -26.3628 0.0083144 0.999963 -0.00226466 -0.414567 0.00719305 0.00220488 0.999972 0.551214\n0.999959 -0.00647444 -0.00627222 -27.7871 0.00646454 0.999978 -0.00159864 -0.422627 0.00628243 0.00155803 0.999979 0.57781\n0.999979 -0.00423741 -0.00480929 -29.2351 0.00422703 0.999989 -0.00216671 -0.423229 0.00481841 0.00214633 0.999986 0.605058\n0.999975 -0.0019234 -0.00677742 -30.7076 0.00190565 0.999995 -0.00262539 -0.423579 0.00678243 0.00261241 0.999974 0.619955\n0.999941 0.000347003 -0.0108292 -32.2167 -0.000400607 0.999988 -0.00494819 -0.409744 0.0108274 0.00495223 0.999929 0.650678\n0.999965 0.00250141 -0.00799228 -33.7485 -0.00255279 0.999976 -0.00642575 -0.416179 0.00797602 0.00644593 0.999947 0.665858\n0.999956 0.00478632 -0.00803003 -35.281 -0.00482304 0.999978 -0.00456042 -0.401822 0.00800802 0.00459895 0.999957 0.679959\n0.999912 0.00560473 -0.0120096 -36.8471 -0.00563733 0.999981 -0.00268286 -0.377095 0.0119943 0.00275033 0.999924 0.692276\n0.999869 0.00454994 -0.0155517 -38.4331 -0.0045754 0.999988 -0.00160219 -0.362438 0.0155443 0.00167314 0.999878 0.715622\n0.999912 0.00388503 -0.0126591 -40.0408 -0.00390938 0.999991 -0.00189978 -0.359124 0.0126516 0.00194911 0.999918 0.733842\n0.99992 0.00325335 -0.0122313 -41.6595 -0.003262 0.999994 -0.000686914 -0.340403 0.012229 0.000726757 0.999925 0.74452\n0.999902 0.0026678 -0.0137442 -43.2728 -0.00266631 0.999996 0.000127089 -0.333153 0.0137445 -9.04307e-05 0.999906 0.77104\n0.999849 0.00162624 -0.0173283 -44.9106 -0.0016182 0.999999 0.000478112 -0.334345 0.017329 -0.000449999 0.99985 0.774508\n0.999732 0.00152652 -0.0230993 -46.5311 -0.00148958 0.999998 0.00161646 -0.332199 0.0231017 -0.00158162 0.999732 0.784287\n0.999811 0.000871191 -0.0194398 -48.1657 -0.00086688 1 0.00023019 -0.330082 0.01944 -0.000213295 0.999811 0.80549\n0.999921 0.000115175 -0.0126007 -49.7812 -0.000115823 1 -5.07525e-05 -0.317745 0.0126007 5.22079e-05 0.999921 0.807006\n0.999819 -0.000221692 -0.0190245 -51.4 0.000245069 0.999999 0.00122644 -0.310961 0.0190242 -0.00123088 0.999818 0.823888\n0.999735 -0.000259385 -0.0230115 -52.9964 0.000300172 0.999998 0.00176903 -0.307364 0.023011 -0.00177546 0.999734 0.832678\n0.99978 -0.000954219 -0.0209399 -54.5679 0.00100568 0.999996 0.00244721 -0.310612 0.0209375 -0.00246774 0.999778 0.842094\n0.999844 -0.00117056 -0.0176046 -56.1247 0.0012017 0.999998 0.0017586 -0.307098 0.0176025 -0.00177948 0.999843 0.861565\n0.999859 -0.000800726 -0.0167805 -57.6587 0.000818031 0.999999 0.00102441 -0.305357 0.0167797 -0.00103799 0.999859 0.865338\n0.999744 -0.000808112 -0.0226065 -59.2103 0.000849993 0.999998 0.00184307 -0.296377 0.022605 -0.00186181 0.999743 0.873276\n0.999708 -0.000589724 -0.0241482 -60.7201 0.000652981 0.999996 0.00261173 -0.299686 0.0241466 -0.00262673 0.999705 0.880218\n0.999797 -0.000181453 -0.020128 -62.2009 0.000213049 0.999999 0.0015676 -0.299851 0.0201277 -0.00157158 0.999796 0.902987\n0.999874 0.00134807 -0.0158233 -63.675 -0.00136805 0.999998 -0.00125157 -0.299722 0.0158216 0.00127306 0.999874 0.912534\n0.999817 0.0029464 -0.0189069 -65.153 -0.00297809 0.999994 -0.0016481 -0.296431 0.0189019 0.0017041 0.99982 0.914659\n0.999778 0.00497033 -0.0204849 -66.6121 -0.00501685 0.999985 -0.00222026 -0.281654 0.0204736 0.00232253 0.999788 0.924416\n0.999791 0.00664969 -0.0193123 -68.0477 -0.00669024 0.999976 -0.00203595 -0.273236 0.0192983 0.00216473 0.999811 0.938519\n0.999728 0.00843194 -0.0217442 -69.4976 -0.0085178 0.999956 -0.00385885 -0.266942 0.0217107 0.00404301 0.999756 0.944458\n0.999667 0.00981497 -0.0238651 -70.9531 -0.00994006 0.999937 -0.00512875 -0.249896 0.0238133 0.00536426 0.999702 0.944498\n0.999678 0.0106371 -0.0230457 -72.3951 -0.0107674 0.999927 -0.00554096 -0.230012 0.022985 0.00578732 0.999719 0.953214\n0.999686 0.0115917 -0.0222375 -73.8336 -0.0117632 0.999902 -0.00759634 -0.218305 0.0221472 0.00785554 0.999724 0.949802\n0.999542 0.0122646 -0.0276782 -75.2809 -0.0124518 0.999901 -0.00659876 -0.204613 0.0275945 0.00694037 0.999595 0.937946\n0.999289 0.0131764 -0.0353224 -76.7449 -0.0133179 0.999904 -0.00377472 -0.173019 0.0352693 0.00424245 0.999369 0.923618\n0.999396 0.0142741 -0.0316745 -78.1845 -0.0143916 0.99989 -0.00348446 -0.143875 0.0316213 0.0039382 0.999492 0.934749\n0.999744 0.0151394 -0.0168026 -79.6347 -0.0152334 0.999869 -0.00548034 -0.12423 0.0167175 0.0057349 0.999844 0.942736\n0.999742 0.0153425 -0.016749 -81.0827 -0.0154513 0.99986 -0.00638551 -0.104081 0.0166487 0.00664265 0.999839 0.945526\n0.999562 0.0163348 -0.0246726 -82.5563 -0.0165855 0.999813 -0.00999054 -0.0851321 0.0245048 0.0103954 0.999646 0.952955\n0.999807 0.0173985 -0.00908848 -83.9991 -0.017448 0.999833 -0.00539563 -0.0524397 0.00899309 0.00555317 0.999944 0.95772\n0.999785 0.0184078 0.00957443 -85.4304 -0.0184586 0.999816 0.00524604 -0.00380463 -0.0094761 -0.00542164 0.99994 0.993929\n0.999357 0.020509 -0.0294099 -86.9353 -0.0204722 0.999789 0.0015506 0.0249635 0.0294355 -0.000947518 0.999566 0.996213\n0.997568 0.0221877 -0.0660789 -88.4381 -0.0224619 0.999742 -0.00340991 0.0587651 0.0659862 0.00488588 0.997809 0.982381\n0.999178 0.0259119 -0.031181 -89.8861 -0.0261289 0.999637 -0.00657216 0.0967307 0.0309994 0.00738148 0.999492 0.966956\n0.99947 0.0291681 -0.0144853 -91.3476 -0.0292821 0.999541 -0.00772404 0.139726 0.0142533 0.00814411 0.999865 0.954579\n0.999129 0.0329718 -0.025583 -92.8651 -0.0331741 0.999421 -0.00752205 0.206921 0.0253201 0.00836419 0.999644 0.939938\n0.999034 0.0359342 -0.0252883 -94.3741 -0.0360788 0.999335 -0.00528484 0.275734 0.0250816 0.00619211 0.999666 0.948756\n0.998963 0.0389727 -0.0235172 -95.911 -0.0390272 0.999236 -0.00186254 0.339928 0.0234267 0.00277842 0.999722 0.934362\n0.998672 0.0431516 -0.0281437 -97.4494 -0.0432238 0.999063 -0.00196104 0.418702 0.0280327 0.00317491 0.999602 0.951712\n0.998551 0.0463483 -0.0273324 -99.0277 -0.0464583 0.998914 -0.00340187 0.486907 0.0271451 0.00466676 0.999621 0.937415\n0.99842 0.0497074 -0.0261861 -100.594 -0.0497074 0.998764 0.000652842 0.593596 0.0261862 0.000649831 0.999657 0.927809\n0.998255 0.0538298 -0.0242822 -102.189 -0.0538708 0.998547 -0.00104 0.683397 0.0241909 0.00234628 0.999705 0.940103\n0.998132 0.0574794 -0.0207196 -103.805 -0.057587 0.99833 -0.00463275 0.780015 0.0204187 0.00581727 0.999775 0.938506\n0.997817 0.0612176 -0.0247621 -105.431 -0.0613484 0.998106 -0.00455795 0.878749 0.0244362 0.00606712 0.999683 0.932503\n0.997267 0.0655989 -0.0340016 -107.075 -0.0658057 0.99782 -0.00499991 0.999029 0.0335995 0.00722375 0.999409 0.924833\n0.997146 0.070005 -0.028261 -108.723 -0.070252 0.997498 -0.00784277 1.11844 0.0276412 0.00980578 0.99957 0.917284\n0.997065 0.0745653 -0.0173652 -110.377 -0.0747404 0.997156 -0.00966361 1.23452 0.0165952 0.0109331 0.999803 0.915406\n0.996619 0.07891 -0.0228737 -112.06 -0.0791569 0.996811 -0.010097 1.38238 0.022004 0.0118735 0.999687 0.919478\n0.99592 0.0845322 -0.0315781 -113.756 -0.0847843 0.996377 -0.00673051 1.53402 0.0308947 0.00938037 0.999479 0.902298\n0.995569 0.0890701 -0.0301616 -115.435 -0.0892573 0.995996 -0.00491529 1.70281 0.029603 0.00758565 0.999533 0.913445\n0.995534 0.0928534 -0.017045 -117.155 -0.0930093 0.995628 -0.0085908 1.87636 0.0161728 0.0101378 0.999818 0.900303\n0.995219 0.0960647 -0.0176372 -118.861 -0.0961738 0.99535 -0.00544 2.04344 0.0170326 0.00711023 0.99983 0.91538\n0.994907 0.0987931 -0.0199952 -120.59 -0.098924 0.995079 -0.00566581 2.21767 0.0193371 0.00761496 0.999784 0.929805\n0.99469 0.101442 -0.017331 -122.305 -0.101583 0.994799 -0.00746538 2.38726 0.0164835 0.00918628 0.999822 0.934681\n0.994116 0.104519 -0.0284424 -124.035 -0.104659 0.994502 -0.00348936 2.58676 0.0279214 0.00644559 0.999589 0.941475\n0.993476 0.108013 -0.0365965 -125.761 -0.108193 0.994125 -0.00297087 2.7741 0.0360606 0.00691098 0.999326 0.920828\n0.993425 0.110759 -0.0289659 -127.473 -0.110853 0.993835 -0.00164337 2.9697 0.0286054 0.00484351 0.999579 0.902153\n0.993109 0.113923 -0.0274958 -129.201 -0.114063 0.993467 -0.00356539 3.17278 0.02691 0.00667708 0.999616 0.9133\n0.992617 0.116772 -0.0328094 -130.956 -0.116939 0.993134 -0.00320765 3.38911 0.0322096 0.00702067 0.999456 0.888669\n0.992239 0.11977 -0.033415 -132.714 -0.119822 0.992795 0.000460446 3.61678 0.0332294 0.00354697 0.999441 0.872709\n0.992066 0.122383 -0.028778 -134.474 -0.122393 0.992481 0.00140582 3.84292 0.0287337 0.00212758 0.999585 0.865437\n0.991704 0.124554 -0.0317668 -136.238 -0.12454 0.992212 0.00241063 4.07511 0.0318196 0.00156561 0.999492 0.848593\n0.99144 0.126005 -0.0342116 -138.007 -0.126006 0.992027 0.00211798 4.30708 0.0342057 0.00221102 0.999412 0.841349\n0.991388 0.126305 -0.0345918 -139.792 -0.126323 0.991988 0.0016587 4.54241 0.0345242 0.00272535 0.9994 0.815614\n0.991518 0.126177 -0.031157 -141.553 -0.126245 0.991999 -0.000207109 4.75593 0.0308815 0.00413875 0.999514 0.809015\n0.991632 0.125632 -0.0297032 -143.344 -0.125656 0.992073 0.00107425 4.99671 0.0296027 0.00266711 0.999558 0.786039\n0.991662 0.125438 -0.0295425 -145.107 -0.125485 0.992095 0.000250888 5.21772 0.0293404 0.00345835 0.999563 0.7879\n0.991706 0.124514 -0.0318558 -146.895 -0.124593 0.992208 -0.000501944 5.45087 0.0315451 0.00446679 0.999492 0.771089\n0.991735 0.124416 -0.0313499 -148.661 -0.124524 0.992215 -0.00152673 5.67618 0.0309159 0.00541792 0.999507 0.757635\n0.991793 0.124064 -0.0308984 -150.433 -0.124143 0.992264 -0.000645294 5.90309 0.0305793 0.00447581 0.999522 0.742313\n0.99185 0.123685 -0.0305918 -152.191 -0.123765 0.992311 -0.000742914 6.12408 0.0302647 0.00452306 0.999532 0.729167\n0.991974 0.123088 -0.0289285 -153.93 -0.123083 0.992394 0.00193891 6.36114 0.0289472 0.00163727 0.99958 0.728292\n0.991903 0.12278 -0.0324487 -155.665 -0.122722 0.992434 0.00376523 6.58626 0.0326655 0.000247433 0.999466 0.726314\n0.991986 0.12233 -0.0316018 -157.383 -0.122247 0.992489 0.00457166 6.79597 0.0319237 -0.000671817 0.99949 0.721669\n0.992124 0.121754 -0.0294291 -159.09 -0.121673 0.99256 0.00453419 7.01175 0.0297622 -0.000917742 0.999557 0.709524\n0.992158 0.121249 -0.0303541 -160.781 -0.121173 0.992622 0.00433076 7.22294 0.0306552 -0.000618715 0.99953 0.707036\n0.992067 0.121275 -0.0330855 -162.487 -0.121182 0.992619 0.00479841 7.42594 0.0334232 -0.000750964 0.999441 0.685673\n0.992059 0.120792 -0.0350592 -164.154 -0.120667 0.992677 0.00565842 7.6346 0.035486 -0.001383 0.999369 0.676517\n0.992157 0.120351 -0.0337657 -165.824 -0.120254 0.992731 0.00489276 7.83691 0.0341091 -0.000793932 0.999418 0.661173\n0.992223 0.120029 -0.0329739 -167.457 -0.119914 0.992769 0.00543476 8.05219 0.0333878 -0.00143845 0.999441 0.650302\n0.992255 0.119927 -0.0323805 -169.104 -0.119796 0.992781 0.00596926 8.25903 0.0328627 -0.00204398 0.999458 0.640254\n0.992373 0.119774 -0.0291536 -170.714 -0.119628 0.992796 0.00671345 8.46111 0.0297477 -0.00317466 0.999552 0.634852\n0.992368 0.119741 -0.0294539 -172.33 -0.11961 0.992802 0.00619613 8.65842 0.0299838 -0.00262587 0.999547 0.634673\n0.992366 0.119075 -0.0320977 -173.929 -0.119009 0.992885 0.00395425 8.85001 0.0323402 -0.000104144 0.999477 0.620622\n0.992343 0.118945 -0.0332858 -175.503 -0.118915 0.9929 0.00286992 9.03977 0.0333908 0.00111024 0.999442 0.612208\n0.9924 0.118754 -0.0322349 -177.081 -0.118782 0.99292 0.00107009 9.2316 0.0321337 0.00276695 0.99948 0.603588\n0.992543 0.118363 -0.0291414 -178.643 -0.118401 0.992966 0.000418498 9.42476 0.0289859 0.00303499 0.999575 0.598992\n0.992669 0.117833 -0.0268929 -180.2 -0.11792 0.993022 -0.00166422 9.60193 0.0265091 0.00482324 0.999637 0.595815\n0.99276 0.116881 -0.0276743 -181.778 -0.117097 0.9931 -0.00632259 9.7846 0.0267444 0.00951741 0.999597 0.604232\n0.99316 0.116324 -0.0100813 -183.326 -0.116385 0.993188 -0.00563842 9.96727 0.00935675 0.00677316 0.999933 0.596649\n0.993161 0.116454 -0.00833313 -184.874 -0.116413 0.993187 0.00519763 10.1811 0.00888164 -0.004192 0.999952 0.618373\n0.991811 0.117158 -0.0508449 -186.493 -0.117012 0.993113 0.00584664 10.3727 0.0511797 0.000150699 0.998689 0.594474\n0.991042 0.118093 -0.0623744 -188.102 -0.117815 0.993002 0.00812732 10.5714 0.0628977 -0.00070587 0.99802 0.55988\n0.992696 0.119787 -0.0143321 -189.649 -0.119823 0.992794 -0.00166512 10.7535 0.0140294 0.00337028 0.999896 0.559824\n0.99265 0.120289 -0.0133062 -191.24 -0.120264 0.992738 0.00262257 10.9637 0.013525 -0.00100303 0.999908 0.547221\n0.991853 0.121993 -0.0366889 -192.873 -0.121724 0.992519 0.00947316 11.1837 0.0375701 -0.00493005 0.999282 0.538003\n0.991834 0.122269 -0.0362758 -194.49 -0.122109 0.992495 0.0065844 11.3654 0.0368086 -0.00210102 0.99932 0.539339\n0.992266 0.121506 -0.0253978 -196.117 -0.121342 0.992579 0.0079145 11.575 0.026171 -0.00477146 0.999646 0.496028\n0.991982 0.122189 -0.0322593 -197.77 -0.121889 0.992481 0.0111199 11.7982 0.0333755 -0.0070987 0.999418 0.502764\n0.992253 0.122031 -0.0233065 -199.419 -0.121793 0.99249 0.011386 11.9904 0.0245209 -0.00845921 0.999664 0.48661\n0.992337 0.121922 -0.020071 -201.063 -0.12171 0.9925 0.0114402 12.1927 0.0213153 -0.00890972 0.999733 0.484511\n0.992095 0.12184 -0.030044 -202.736 -0.121514 0.992512 0.0124572 12.4115 0.0313368 -0.008708 0.999471 0.491007\n0.991976 0.122372 -0.0317575 -204.414 -0.122133 0.992469 0.00937437 12.6016 0.0326655 -0.00542051 0.999452 0.481848\n0.992033 0.122027 -0.031307 -206.058 -0.121795 0.992513 0.00920538 12.8067 0.0321959 -0.00531899 0.999467 0.46152\n0.991955 0.122597 -0.0315619 -207.704 -0.12239 0.992446 0.00842133 13.0082 0.0323559 -0.00449072 0.999466 0.451409\n0.992001 0.122463 -0.0305932 -209.321 -0.122259 0.992462 0.00848739 13.2081 0.031402 -0.00467922 0.999496 0.449392\n0.992023 0.12258 -0.0294203 -210.928 -0.12237 0.992445 0.00883814 13.4132 0.0302814 -0.00516748 0.999528 0.42843\n0.991958 0.122591 -0.0314636 -212.536 -0.122317 0.992435 0.0105046 13.6184 0.0325133 -0.00657161 0.99945 0.425296\n0.991918 0.123026 -0.0310217 -214.114 -0.122758 0.992382 0.0103979 13.8199 0.0320646 -0.00650568 0.999465 0.425745\n0.992052 0.123029 -0.0263956 -215.676 -0.122828 0.992386 0.00910528 14.0125 0.0273149 -0.0057908 0.99961 0.414303\n0.991936 0.123044 -0.0303789 -217.23 -0.122778 0.992379 0.0104665 14.2267 0.0314352 -0.00665219 0.999484 0.39692\n0.991821 0.12313 -0.0336283 -218.777 -0.122826 0.992368 0.0109569 14.4252 0.0347207 -0.00673685 0.999374 0.396948\n0.992192 0.123467 -0.0176259 -220.288 -0.123331 0.992328 0.00861423 14.6069 0.0185543 -0.00637315 0.999808 0.407073\n0.992285 0.123844 -0.00581851 -222.534 -0.123738 0.992186 0.016003 14.9078 0.00775492 -0.0151596 0.999855 0.43348\n0.990539 0.124342 -0.0580606 -224.085 -0.123779 0.992222 0.0132146 15.0876 0.0592521 -0.00590291 0.998226 0.395898\n0.990184 0.125663 -0.0611905 -225.585 -0.12494 0.992043 0.0155195 15.2965 0.0626538 -0.00772205 0.998005 0.373201\n0.991888 0.126282 -0.0145452 -227.064 -0.126138 0.991958 0.0104267 15.4757 0.0157449 -0.00850743 0.99984 0.374841\n0.991908 0.125641 -0.0182214 -228.591 -0.125472 0.992045 0.010152 15.6673 0.0193519 -0.00778361 0.999782 0.340973\n0.991344 0.125749 -0.0377345 -230.131 -0.12528 0.992015 0.0145575 15.8722 0.0392638 -0.0097041 0.999182 0.342853\n0.991539 0.125797 -0.0320304 -231.662 -0.125349 0.99199 0.0156319 16.0861 0.0337403 -0.0114846 0.999365 0.333547\n0.991818 0.125729 -0.0221061 -233.189 -0.125488 0.992023 0.0119426 16.2804 0.0234313 -0.00907083 0.999684 0.337007\n0.991881 0.125487 -0.0206147 -234.734 -0.12533 0.992076 0.00876579 16.4573 0.0215514 -0.00611098 0.999749 0.335418\n0.991767 0.125363 -0.026111 -236.294 -0.125221 0.992104 0.00702582 16.6577 0.0267856 -0.00369833 0.999634 0.331449\n0.991578 0.125252 -0.0329442 -237.842 -0.125099 0.992122 0.00668042 16.845 0.0335214 -0.00250288 0.999435 0.324724\n0.991681 0.125059 -0.0304805 -239.388 -0.124955 0.992148 0.00528435 17.0447 0.030902 -0.0014317 0.999521 0.31093\n0.991826 0.124419 -0.0283044 -240.939 -0.124281 0.992225 0.00658446 17.2449 0.0289036 -0.00301294 0.999578 0.302349\n0.991758 0.124015 -0.0321862 -242.519 -0.123821 0.992272 0.00798127 17.4517 0.0329272 -0.00393017 0.99945 0.293686\n0.991793 0.124023 -0.0310524 -244.091 -0.123775 0.992262 0.00979914 17.6499 0.0320274 -0.00587521 0.99947 0.278568\n0.991912 0.123849 -0.0277931 -245.656 -0.123555 0.992264 0.0120592 17.8562 0.0290716 -0.00852764 0.999541 0.281243\n0.992047 0.123649 -0.0235297 -247.222 -0.123433 0.992298 0.0104384 18.0496 0.0246391 -0.007451 0.999669 0.275111\n0.992075 0.122994 -0.0256957 -248.774 -0.122761 0.992381 0.0104432 18.2455 0.0267843 -0.00720599 0.999615 0.280011\n0.992109 0.122241 -0.0278865 -251.072 -0.121993 0.992476 0.0104217 18.5327 0.0289506 -0.00693752 0.999557 0.273992\n0.992203 0.121366 -0.0283516 -252.585 -0.121095 0.992579 0.0110801 18.7211 0.029486 -0.0075605 0.999537 0.268395\n0.992357 0.119947 -0.0289956 -254.091 -0.119629 0.99274 0.0124657 18.916 0.0302803 -0.00890177 0.999502 0.262983\n0.992399 0.119176 -0.0306724 -255.561 -0.118808 0.992824 0.0135882 19.0924 0.0320717 -0.0098408 0.999437 0.258587\n0.992647 0.117779 -0.0279123 -257.059 -0.117435 0.992986 0.0136796 19.2731 0.0293277 -0.0103011 0.999517 0.253928\n0.992828 0.116842 -0.025301 -258.527 -0.116515 0.99309 0.0140331 19.4559 0.0267658 -0.0109846 0.999581 0.250439\n0.992847 0.115988 -0.02832 -259.979 -0.115601 0.993183 0.0149455 19.6218 0.0298604 -0.0115648 0.999487 0.247921\n0.992909 0.115309 -0.0289059 -261.424 -0.114914 0.993263 0.0149655 19.7841 0.0304368 -0.0115377 0.99947 0.252547\n0.993261 0.11447 -0.018147 -262.853 -0.114231 0.993359 0.0137251 19.9398 0.0195976 -0.0115596 0.999741 0.259768\n0.993332 0.113535 0.020021 -264.225 -0.113902 0.993324 0.0182741 20.1201 -0.0178126 -0.0204327 0.999633 0.293088\n0.993476 0.113427 0.0117953 -265.596 -0.1137 0.993177 0.0259161 20.3114 -0.00877522 -0.0270882 0.999595 0.328834\n0.991125 0.114912 -0.0668265 -267.781 -0.113425 0.993215 0.0256511 20.5619 0.0693207 -0.0178437 0.997435 0.28832\n0.991395 0.116796 -0.0591128 -269.18 -0.115347 0.992948 0.0273594 20.7277 0.0618914 -0.0203055 0.997876 0.269978\n0.99329 0.115549 -0.00481963 -271.273 -0.115479 0.993225 0.0129679 20.9624 0.0062854 -0.0123243 0.999904 0.272501\n0.993407 0.114113 -0.0109572 -272.706 -0.113935 0.993364 0.015721 21.1423 0.0126785 -0.0143689 0.999816 0.249537\n0.99331 0.112854 -0.0244721 -274.858 -0.112451 0.993507 0.0172571 21.3874 0.0262607 -0.0143897 0.999552 0.276924\n0.993731 0.110457 -0.0172447 -276.302 -0.11014 0.993747 0.0183414 21.5569 0.0191628 -0.0163271 0.999683 0.271332\n0.994169 0.106453 -0.0172 -278.467 -0.106143 0.994187 0.0180659 21.7926 0.0190232 -0.0161349 0.999689 0.291327\n0.994587 0.103151 -0.0124754 -279.924 -0.10296 0.994571 0.0150691 21.9445 0.0139621 -0.013703 0.999809 0.312072\n0.994933 0.0996653 -0.0132044 -281.369 -0.0994764 0.994938 0.0142613 22.0889 0.0145589 -0.0128755 0.999811 0.320162\n0.995322 0.0947114 -0.0190983 -283.572 -0.0944843 0.995448 0.0124646 22.2951 0.0201919 -0.0106018 0.99974 0.335876\n0.995647 0.0911624 -0.0194132 -285.048 -0.0909222 0.995774 0.0129148 22.4348 0.0205085 -0.0110935 0.999728 0.330833\n0.99612 0.086032 -0.0185393 -287.252 -0.085799 0.996227 0.0130187 22.6231 0.0195894 -0.0113776 0.999743 0.347658\n0.996428 0.0827384 -0.0169215 -288.746 -0.08252 0.996502 0.013223 22.7555 0.0179564 -0.0117794 0.999769 0.351238\n0.996704 0.0793154 -0.0170345 -290.24 -0.079115 0.996792 0.0121347 22.8702 0.0179423 -0.010747 0.999781 0.365914\n0.996924 0.0764242 -0.0173818 -291.727 -0.0762468 0.997032 0.0106498 22.9878 0.0181441 -0.00929169 0.999792 0.373562\n0.997068 0.0744627 -0.0176507 -293.241 -0.0742688 0.997173 0.0113982 23.1001 0.0184495 -0.0100539 0.999779 0.38145\n0.997187 0.0722935 -0.0197999 -294.753 -0.0720514 0.99732 0.0126794 23.2318 0.0206635 -0.0112171 0.999724 0.379582\n0.997415 0.0696214 -0.0177846 -296.259 -0.0694268 0.997523 0.0113362 23.3273 0.0185297 -0.0100722 0.999778 0.40126\n0.997816 0.0655916 -0.0078275 -297.772 -0.065507 0.997796 0.0106131 23.4244 0.00850637 -0.0100772 0.999913 0.408713\n0.998022 0.0612822 -0.0140167 -299.294 -0.0611327 0.998071 0.0108567 23.5278 0.0146549 -0.00997834 0.999843 0.434359\n0.998405 0.0543221 -0.0153821 -301.596 -0.0541754 0.998483 0.00980358 23.6469 0.0158913 -0.00895462 0.999834 0.452444\n0.998699 0.0501117 -0.00941879 -303.107 -0.0500152 0.998696 0.010215 23.7309 0.0099184 -0.00973061 0.999903 0.470583\n0.998844 0.0457875 -0.0146501 -304.624 -0.0456591 0.998917 0.00898384 23.7922 0.0150456 -0.00830454 0.999852 0.489392\n0.998994 0.041596 -0.0167828 -306.146 -0.0414479 0.999099 0.00907872 23.8509 0.0171453 -0.00837397 0.999818 0.503157\n0.999261 0.0353986 -0.0149486 -308.38 -0.0352588 0.999333 0.00951748 23.9377 0.0152755 -0.00898339 0.999843 0.519086\n0.999339 0.032083 -0.0170778 -309.845 -0.0318856 0.999423 0.0117104 23.9807 0.0174436 -0.0111581 0.999786 0.54051\n0.999374 0.0325961 -0.0137871 -311.318 -0.0324848 0.999438 0.00822217 24.0343 0.0140474 -0.00776915 0.999871 0.571499\n0.999463 0.0325578 -0.00362284 -312.773 -0.0325323 0.999447 0.00688715 24.0747 0.00384507 -0.00676559 0.99997 0.580735\n0.999426 0.0337409 0.00317153 -314.226 -0.0337603 0.99941 0.00628574 24.1304 -0.00295758 -0.0063892 0.999975 0.631288\n0.999048 0.0338399 0.0275287 -315.656 -0.0340602 0.999391 0.00757278 24.1883 -0.0272557 -0.00850321 0.999592 0.688883\n0.999149 0.0341748 -0.0230865 -317.906 -0.0339573 0.999376 0.00974616 24.2729 0.0234051 -0.00895391 0.999686 0.727219\n0.996778 0.0356215 -0.0718694 -319.403 -0.0347965 0.999314 0.0126997 24.3334 0.0722725 -0.0101579 0.997333 0.712549\n0.998814 0.0362965 -0.0324477 -320.811 -0.0360788 0.999322 0.00727137 24.3844 0.0326896 -0.00609208 0.999447 0.720855\n0.999277 0.0359145 0.0124745 -322.244 -0.0359837 0.999338 0.00536951 24.4345 -0.0122734 -0.00581451 0.999908 0.717384\n0.999338 0.0360582 -0.00480962 -323.739 -0.0360456 0.999347 0.00266275 24.4884 0.00490249 -0.00248762 0.999985 0.745015\n0.999344 0.0359061 -0.00477896 -325.199 -0.0359091 0.999355 -0.000535161 24.523 0.00475666 0.000706418 0.999988 0.785787\n0.999398 0.0345842 0.00260977 -326.665 -0.0345831 0.999402 -0.000455498 24.5737 -0.00262396 0.00036497 0.999996 0.809089\n0.999416 0.0329969 -0.00885217 -328.166 -0.0329951 0.999455 0.00034663 24.6288 0.00885879 -5.435e-05 0.999961 0.821041\n0.999404 0.0307483 -0.0156852 -329.681 -0.0307251 0.999526 0.00171711 24.6696 0.0157306 -0.00123415 0.999876 0.840807\n0.999529 0.0278157 -0.0129283 -331.195 -0.0277803 0.99961 0.00290796 24.7093 0.0130042 -0.00254744 0.999912 0.854768\n0.999628 0.025064 -0.0107502 -332.714 -0.0250167 0.999677 0.00451136 24.7583 0.0108598 -0.00424075 0.999932 0.860931\n0.999685 0.0214082 -0.0130691 -334.259 -0.0212855 0.999729 0.00945807 24.8004 0.0132681 -0.00917691 0.99987 0.878492\n0.999754 0.0193549 -0.0107998 -335.774 -0.0192323 0.999751 0.0113454 24.8283 0.0110167 -0.0111349 0.999877 0.902481\n0.999865 0.016302 -0.002158 -337.322 -0.0162776 0.999808 0.0108846 24.8489 0.00233503 -0.010848 0.999938 0.94135\n0.99991 0.0134455 0.000242166 -338.884 -0.013447 0.99988 0.00762899 24.8639 -0.000139561 -0.00763156 0.999971 0.974294\n0.999938 0.0107175 -0.00294769 -340.458 -0.0107083 0.999938 0.00311295 24.8739 0.00298087 -0.00308119 0.999991 1.00528\n0.999943 0.00748803 -0.00760445 -342.058 -0.00747993 0.999971 0.0010934 24.8858 0.00761242 -0.00103645 0.99997 1.02713\n0.999949 0.00674783 -0.00744796 -343.647 -0.00675169 0.999977 -0.000492853 24.8894 0.00744446 0.000543115 0.999972 1.05027\n0.999966 0.00711658 -0.00416401 -345.258 -0.00711885 0.999975 -0.00052981 24.9105 0.00416013 0.000559434 0.999991 1.07351\n0.999969 0.00715773 -0.00313537 -346.859 -0.00715833 0.999974 -0.000178208 24.9228 0.00313401 0.000200647 0.999995 1.10211\n0.999972 0.0066059 -0.00352603 -348.463 -0.00661195 0.999977 -0.0017082 24.9359 0.00351467 0.00173147 0.999992 1.13696\n0.999973 0.00547456 -0.00499298 -350.107 -0.00547823 0.999985 -0.000722 24.9572 0.00498895 0.000749333 0.999987 1.15834\n0.999947 0.00428413 -0.00931055 -351.75 -0.00427025 0.99999 0.00151038 24.9625 0.00931692 -0.00147054 0.999956 1.18744\n0.999955 0.00337015 -0.00883567 -353.394 -0.00335562 0.999993 0.00165904 24.969 0.0088412 -0.00162931 0.99996 1.22001\n0.999998 0.00180156 -0.000276485 -355.033 -0.00180112 0.999997 0.00159525 24.9853 0.000279358 -0.00159474 0.999999 1.24683\n0.999995 0.0010883 -0.00300914 -356.693 -0.00108433 0.999999 0.0013196 24.9801 0.00301057 -0.00131633 0.999995 1.28233\n0.999985 0.00116212 -0.00534317 -358.358 -0.00115392 0.999998 0.00153745 24.9897 0.00534494 -0.00153127 0.999985 1.30919\n0.999981 0.00126454 -0.00599619 -360.03 -0.00124863 0.999996 0.00265531 24.9996 0.00599952 -0.00264777 0.999978 1.33971\n0.999996 0.00162416 -0.00221741 -362.552 -0.00161769 0.999994 0.00291317 24.9928 0.00222213 -0.00290957 0.999993 1.3892\n0.999999 0.00154992 0.00045083 -364.244 -0.00155064 0.999998 0.00159133 24.9923 -0.000448363 -0.00159203 0.999999 1.42672\n0.999993 0.00113021 -0.00369196 -365.954 -0.00112606 0.999999 0.00112517 24.9849 0.00369323 -0.00112101 0.999993 1.4574\n0.999964 0.000717984 -0.00847995 -367.672 -0.000684094 0.999992 0.00399871 24.9941 0.00848275 -0.00399277 0.999956 1.48392\n0.999992 0.000501015 -0.00404284 -369.388 -0.000478437 0.999984 0.00558358 25.0053 0.00404557 -0.0055816 0.999976 1.52179\n0.999966 -0.000744611 0.00823438 -371.985 0.000710324 0.999991 0.004166 25.0068 -0.00823741 -0.00416 0.999957 1.5866\n0.99912 -0.00209773 0.0418951 -373.689 0.00166531 0.999945 0.0103539 25.0105 -0.0419145 -0.010275 0.999068 1.66983\n0.999836 -0.00404687 0.0176491 -375.424 0.00385705 0.999934 0.0107762 25.0189 -0.0176916 -0.0107064 0.999786 1.71778\n0.998072 -0.00597873 -0.061786 -377.247 0.00657316 0.999934 0.00942201 25.0136 0.0617255 -0.00980997 0.998045 1.73625\n0.999092 -0.00613622 -0.0421535 -378.989 0.00644498 0.999953 0.00719271 25.0265 0.0421074 -0.00745786 0.999085 1.73088\n0.999703 -0.00839686 0.0228636 -380.685 0.00822845 0.999938 0.00745003 24.9911 -0.0229248 -0.00725968 0.999711 1.73901\n0.999937 -0.0108018 -0.00320669 -382.471 0.0108298 0.999902 0.0088452 24.9797 0.00311083 -0.00887937 0.999956 1.77609\n0.999815 -0.011473 -0.0154249 -384.276 0.0116358 0.999877 0.0105059 24.9661 0.0153025 -0.0106834 0.999826 1.80764\n0.999912 -0.0131695 0.00178745 -386.044 0.0131501 0.999859 0.0104405 24.9466 -0.0019247 -0.010416 0.999944 1.84899\n0.99988 -0.0150213 0.00386299 -387.842 0.0149843 0.999843 0.00945967 24.909 -0.00400448 -0.00940065 0.999948 1.88981\n0.999823 -0.0169568 -0.00816032 -389.645 0.0170288 0.999816 0.00884207 24.8932 0.00800889 -0.00897946 0.999928 1.92126\n0.999791 -0.0183374 -0.00903012 -391.43 0.0184277 0.99978 0.0100164 24.865 0.00884445 -0.0101808 0.999909 1.96259\n0.999802 -0.0198753 -7.35591e-05 -393.191 0.019875 0.999749 0.0103619 24.8312 -0.000132406 -0.0103614 0.999946 2.00315\n0.999772 -0.0213224 -0.000840548 -394.94 0.0213296 0.999725 0.00972783 24.7995 0.000632896 -0.00974354 0.999952 2.03618\n0.999711 -0.0230015 -0.00698048 -396.663 0.0230735 0.99968 0.0104069 24.7669 0.00673887 -0.010565 0.999921 2.07835\n0.999669 -0.0250164 -0.00595651 -398.364 0.0251007 0.999579 0.0145301 24.738 0.00559051 -0.0146748 0.999877 2.11192\n0.999559 -0.0293499 0.00446018 -400.891 0.0292838 0.99947 0.014216 24.6586 -0.00487506 -0.0140792 0.999889 2.19029\n0.999463 -0.0327213 0.00179952 -402.571 0.0326927 0.999366 0.0141175 24.6068 -0.00226032 -0.0140511 0.999899 2.22944\n0.999344 -0.0361325 -0.00237448 -404.264 0.036162 0.99925 0.0138746 24.5534 0.00187137 -0.0139514 0.999901 2.26809\n0.999213 -0.0396638 -0.000578243 -405.915 0.039668 0.999108 0.0144918 24.488 2.92785e-06 -0.0145033 0.999895 2.31017\n0.999061 -0.0433202 0.000627004 -407.574 0.0433048 0.998936 0.0158876 24.4093 -0.00131459 -0.0158456 0.999874 2.34711\n0.998894 -0.0470158 0.000881507 -409.233 0.0469968 0.998778 0.0153136 24.3348 -0.00160041 -0.0152552 0.999882 2.40771\n0.998675 -0.050748 0.00850776 -410.905 0.0506006 0.998579 0.0167305 24.2473 -0.00934471 -0.0162779 0.999824 2.42701\n0.998353 -0.0536283 0.0203961 -412.538 0.0530534 0.998206 0.0277547 24.1892 -0.0218479 -0.0266269 0.999407 2.51655\n0.998356 -0.0569406 0.00654711 -414.193 0.0567966 0.998177 0.0204065 24.093 -0.00769713 -0.0200011 0.99977 2.59033\n0.997889 -0.0598227 -0.025283 -415.891 0.0601861 0.998091 0.0138629 23.986 0.0244054 -0.0153554 0.999584 2.59048\n0.997898 -0.0602312 -0.0239182 -417.542 0.0605607 0.998076 0.0132953 23.8728 0.0230714 -0.0147158 0.999626 2.62206\n0.998006 -0.0623757 0.00965961 -419.199 0.0622406 0.997967 0.0137139 23.7644 -0.0104954 -0.0130853 0.999859 2.65202\n0.997846 -0.0652549 0.00676818 -420.879 0.0651565 0.997779 0.0138556 23.6738 -0.00765729 -0.0133847 0.999881 2.68078\n0.99771 -0.0671161 -0.00840026 -422.56 0.0672263 0.997645 0.0136061 23.566 0.0074673 -0.0141396 0.999872 2.71634\n0.997503 -0.0699163 -0.00993947 -424.249 0.0700603 0.997431 0.0149566 23.444 0.00886822 -0.0156156 0.999839 2.74576\n0.997278 -0.0736053 -0.00437253 -425.93 0.073674 0.997112 0.0184484 23.3292 0.00300201 -0.0187203 0.99982 2.77558\n0.997013 -0.0767602 -0.0085983 -427.622 0.0769142 0.996851 0.0192947 23.2091 0.00709016 -0.0198984 0.999777 2.80678\n0.996762 -0.0798951 -0.00906196 -429.326 0.0800606 0.996596 0.0196643 23.0795 0.00746003 -0.0203261 0.999766 2.83588\n0.996485 -0.0833804 0.00808622 -431.004 0.0831903 0.996301 0.0215301 22.9357 -0.0098515 -0.0207818 0.999735 2.8877\n0.996135 -0.0873145 0.00953256 -432.679 0.0871271 0.996024 0.0185651 22.7942 -0.0111157 -0.0176628 0.999782 2.94392\n0.995865 -0.0905794 -0.00688027 -434.363 0.0906639 0.995795 0.0131597 22.6397 0.00565934 -0.0137291 0.99989 2.99397\n0.995543 -0.0939748 -0.00798856 -435.99 0.0940711 0.995485 0.0126778 22.4876 0.00676109 -0.0133728 0.999888 3.0261\n0.995274 -0.0970315 -0.00391727 -437.614 0.0970723 0.995204 0.0120941 22.3249 0.00272497 -0.0124172 0.999919 3.05746\n0.994922 -0.100503 -0.00532267 -439.216 0.100567 0.994838 0.0135457 22.1713 0.0039338 -0.0140122 0.999894 3.09097\n0.994635 -0.103327 -0.0048854 -440.79 0.103381 0.994563 0.0124997 21.9971 0.00356728 -0.0129377 0.99991 3.14556\n0.994252 -0.107017 0.00324716 -442.345 0.106965 0.994173 0.0133735 21.8351 -0.00465943 -0.0129493 0.999905 3.1756\n0.993926 -0.110047 -0.00118604 -443.901 0.110053 0.993822 0.014374 21.6774 -0.000403111 -0.0144172 0.999896 3.20961\n0.993636 -0.112396 -0.00743879 -445.415 0.112483 0.993574 0.0125875 21.4946 0.0059762 -0.0133441 0.999893 3.27473\n0.992959 -0.115558 0.0260445 -446.881 0.114958 0.993093 0.0234552 21.3193 -0.0285751 -0.020296 0.999386 3.31987\n0.992149 -0.119147 0.0380009 -448.345 0.118099 0.992586 0.0287182 21.1813 -0.0411408 -0.0240049 0.998865 3.41143\n0.992395 -0.122471 -0.0123884 -449.886 0.122678 0.992289 0.01766 20.9944 0.01013 -0.0190455 0.999767 3.42424\n0.99066 -0.125178 -0.0540618 -451.392 0.126044 0.99194 0.0129222 20.806 0.0520085 -0.0196157 0.998454 3.4295\n0.99142 -0.127729 -0.0277887 -452.836 0.128156 0.991653 0.0141472 20.6101 0.0257498 -0.0175871 0.999514 3.44591\n0.991337 -0.130722 0.0127382 -454.279 0.130494 0.991297 0.0173394 20.4167 -0.014894 -0.0155269 0.999769 3.47451\n0.991017 -0.133652 0.00479564 -455.78 0.133564 0.990919 0.0154659 20.2232 -0.00681915 -0.0146864 0.999869 3.48631\n0.990621 -0.135755 -0.0155321 -457.287 0.136008 0.990569 0.0165781 20.0239 0.013135 -0.0185351 0.999742 3.52295\n0.990335 -0.138498 -0.00747949 -458.78 0.138605 0.99021 0.0165128 19.8068 0.00511929 -0.0173898 0.999836 3.56263\n0.989863 -0.142021 0.000730939 -460.272 0.141984 0.989701 0.0182296 19.6029 -0.0033124 -0.0179411 0.999834 3.5776\n0.989493 -0.144337 -0.00841168 -461.805 0.144462 0.989368 0.0168132 19.3835 0.00589546 -0.0178518 0.999823 3.61746\n0.989081 -0.147219 -0.00673779 -463.326 0.147311 0.988959 0.0160876 19.1528 0.00429499 -0.0169045 0.999848 3.65328\n0.988605 -0.150536 0.000126158 -464.829 0.150508 0.988441 0.0181892 18.9269 -0.00286283 -0.017963 0.999835 3.68432\n0.988216 -0.15299 -0.00474664 -466.356 0.153049 0.988074 0.0169156 18.6935 0.00210213 -0.0174427 0.999846 3.71605\n0.987711 -0.15612 -0.00732036 -467.881 0.156216 0.987606 0.015224 18.4415 0.00485286 -0.0161805 0.999857 3.75466\n0.987234 -0.159223 -0.00403668 -469.396 0.159267 0.987104 0.016123 18.2054 0.00141747 -0.0165601 0.999862 3.78426\n0.986656 -0.162648 -0.00742736 -470.938 0.162746 0.986549 0.0153124 17.9565 0.00483693 -0.0163169 0.999855 3.81963\n0.986179 -0.165423 -0.00933281 -472.446 0.165546 0.986097 0.0144005 17.6972 0.00682089 -0.0157464 0.999853 3.85484\n0.985667 -0.168563 -0.00688651 -473.94 0.168653 0.985549 0.0157881 17.4386 0.00412571 -0.0167232 0.999852 3.87724\n0.985156 -0.171383 -0.00974861 -475.415 0.171522 0.985052 0.0159004 17.1778 0.00687784 -0.0173365 0.999826 3.91292\n0.984731 -0.173993 -0.00552635 -476.86 0.174042 0.984686 0.0101759 16.9109 0.00367119 -0.0109823 0.999933 3.97832\n0.983948 -0.177555 0.0179082 -478.287 0.177247 0.98401 0.0175235 16.6566 -0.0207332 -0.014068 0.999686 4.00114\n0.983184 -0.180238 0.0293952 -479.676 0.179427 0.983366 0.028247 16.4248 -0.0339974 -0.0224977 0.999169 4.08167\n0.983107 -0.182546 -0.0133053 -481.143 0.1828 0.982918 0.0213654 16.1524 0.00917782 -0.0234367 0.999683 4.07436\n0.980755 -0.185566 -0.060703 -482.586 0.186655 0.982343 0.0127385 15.8727 0.0572674 -0.0238239 0.998075 4.10863\n0.981491 -0.18851 -0.0337597 -483.962 0.189122 0.981824 0.0159313 15.6011 0.0301428 -0.0220211 0.999303 4.09528\n0.9816 -0.190948 -0.00101975 -485.344 0.190923 0.981352 0.0222653 15.337 -0.00325079 -0.0220503 0.999752 4.11682\n0.980974 -0.194022 -0.00670505 -486.785 0.194113 0.980815 0.0179511 15.0482 0.00309351 -0.0189111 0.999816 4.12737\n0.980423 -0.196594 -0.0110386 -488.211 0.196767 0.98029 0.0177261 14.7605 0.00733622 -0.0195511 0.999782 4.15907\n0.980079 -0.198553 -0.00461182 -489.656 0.198606 0.979892 0.0191813 14.4682 0.000710584 -0.0197151 0.999805 4.19317\n0.979832 -0.199825 -0.000347604 -491.087 0.199804 0.979699 0.0163755 14.1725 -0.00293169 -0.0161147 0.999866 4.23624\n0.979732 -0.200307 0.00166166 -492.548 0.200264 0.979639 0.0142058 13.8633 -0.00447335 -0.0135851 0.999898 4.2703\n0.979613 -0.200893 -0.000859963 -494.024 0.200885 0.979508 0.0144447 13.5631 -0.0020595 -0.0143229 0.999895 4.29934\n0.979484 -0.201468 -0.00455299 -495.493 0.201515 0.979368 0.0151511 13.2646 0.00140659 -0.0157578 0.999875 4.33921\n0.979334 -0.202247 -0.000860852 -496.956 0.202238 0.979231 0.0143333 12.9624 -0.0020559 -0.0142112 0.999897 4.38383\n0.979079 -0.203479 0.000658527 -498.426 0.203447 0.978972 0.0149285 12.6557 -0.00368232 -0.0144822 0.999888 4.41698\n0.978789 -0.204831 -0.00412626 -499.925 0.20487 0.978684 0.0143525 12.342 0.00109847 -0.0148934 0.999888 4.45473\n0.978369 -0.206756 -0.00679873 -501.429 0.206835 0.978268 0.0145165 12.0319 0.00364961 -0.0156087 0.999872 4.48359\n0.977683 -0.209978 -0.00676722 -502.93 0.210063 0.977551 0.0163805 11.7179 0.00317576 -0.0174364 0.999843 4.51559\n0.976835 -0.213913 -0.00586324 -504.431 0.213981 0.976706 0.0160159 11.3867 0.00230066 -0.0168995 0.999855 4.54842\n0.975941 -0.217899 -0.00766515 -505.973 0.218002 0.975795 0.0173071 11.0452 0.00370842 -0.0185617 0.999821 4.57937\n0.975271 -0.220878 -0.00775076 -507.503 0.22098 0.975133 0.0168127 10.6925 0.00384446 -0.0181097 0.999829 4.61173\n0.97481 -0.222948 -0.00631018 -509.027 0.223027 0.974644 0.0180894 10.3445 0.00211718 -0.0190411 0.999816 4.64217\n0.974374 -0.224868 -0.00555402 -510.567 0.224932 0.974204 0.0182222 9.9859 0.00131316 -0.0190045 0.999819 4.67387\n0.973957 -0.226634 -0.00674811 -512.131 0.226723 0.973773 0.0190416 9.62828 0.00225567 -0.0200756 0.999796 4.70934\n0.973627 -0.228051 -0.0065124 -513.689 0.228136 0.973438 0.0192931 9.26871 0.00193961 -0.02027 0.999793 4.74904\n0.973179 -0.229986 -0.00538274 -515.255 0.230048 0.972972 0.020077 8.9029 0.000619828 -0.0207768 0.999784 4.78346\n0.972667 -0.231936 -0.0111831 -516.819 0.232118 0.972494 0.0193891 8.52566 0.00637849 -0.0214549 0.999749 4.80938\n0.972091 -0.234081 -0.0156312 -518.389 0.234358 0.971962 0.0191573 8.15214 0.0107085 -0.022286 0.999694 4.8412\n0.971597 -0.236317 -0.0124222 -519.917 0.236526 0.971431 0.019451 7.7758 0.00747072 -0.0218367 0.999734 4.8742\n0.970776 -0.239568 -0.0141759 -521.471 0.239854 0.970511 0.0240648 7.40223 0.00799269 -0.0267617 0.99961 4.88494\n0.969934 -0.242483 -0.020759 -522.983 0.24297 0.969697 0.0255551 7.03653 0.0139333 -0.0298305 0.999458 4.92512\n0.969111 -0.24617 -0.0149948 -524.476 0.246504 0.968753 0.0274528 6.6606 0.00776817 -0.030301 0.999511 4.95601\n0.968292 -0.249671 -0.00866353 -525.937 0.249814 0.967953 0.0256895 6.27954 0.00197196 -0.0270392 0.999632 5.01543\n0.967294 -0.253654 0.00108046 -527.408 0.253589 0.967128 0.0188698 5.88292 -0.00583134 -0.0179787 0.999821 5.06092\n0.966024 -0.258452 -0.000644474 -528.86 0.258419 0.965855 0.0185564 5.49122 -0.00417347 -0.0180925 0.999828 5.07034\n0.964948 -0.262125 -0.012901 -530.298 0.262294 0.964889 0.0138212 5.10058 0.00882513 -0.0167206 0.999821 5.1291\n0.963852 -0.266354 -0.00678136 -531.715 0.266413 0.963804 0.0103056 4.68526 0.00379097 -0.0117397 0.999924 5.15046\n0.962632 -0.2707 -0.00780061 -533.107 0.270789 0.962529 0.0145469 4.30818 0.00357045 -0.0161157 0.999864 5.16305\n0.961624 -0.27389 -0.0162081 -534.495 0.274149 0.961541 0.0167642 3.92396 0.0109932 -0.0205642 0.999728 5.20527\n0.960774 -0.277311 -0.00352643 -535.85 0.277322 0.960547 0.0209982 3.51244 -0.00243571 -0.0211525 0.999773 5.233\n0.959115 -0.281684 0.0274224 -537.173 0.280622 0.959104 0.0370274 3.12796 -0.036731 -0.0278182 0.998938 5.305\n0.958211 -0.285202 0.0221716 -538.498 0.284309 0.958041 0.036403 2.74847 -0.0316235 -0.0285781 0.999091 5.39307\n0.957412 -0.287511 -0.0264488 -539.87 0.287989 0.957493 0.0164291 2.32743 0.020601 -0.0233464 0.999515 5.39865\n0.955872 -0.289327 -0.0509813 -541.231 0.290091 0.956965 0.00811624 1.91447 0.0464391 -0.0225473 0.998667 5.40879\n0.956687 -0.290631 -0.0168588 -542.52 0.290895 0.956618 0.0161848 1.50671 0.0114236 -0.020388 0.999727 5.43185\n0.956323 -0.292282 0.00423253 -543.781 0.292101 0.956081 0.0242262 1.12493 -0.0111275 -0.0219318 0.999698 5.45068\n0.955986 -0.293348 -0.00613928 -545.077 0.293412 0.955769 0.0203817 0.715623 -0.000111194 -0.0212859 0.999773 5.47409\n0.955482 -0.29483 -0.0113602 -546.361 0.295015 0.955246 0.0216866 0.329321 0.00445791 -0.0240725 0.9997 5.51189\n0.955184 -0.295934 -0.00677795 -547.609 0.29601 0.954862 0.0248357 -0.0531755 -0.000877734 -0.025729 0.999669 5.55248\n0.954812 -0.297202 -0.00238965 -548.851 0.297172 0.954521 0.0240344 -0.434957 -0.00486211 -0.0236585 0.999708 5.59611\n0.954534 -0.298096 -0.00202905 -550.077 0.298064 0.954276 0.0226848 -0.825695 -0.00482597 -0.0222582 0.999741 5.63397\n0.954842 -0.297097 -0.0031115 -551.28 0.297096 0.954618 0.0209507 -1.19385 -0.0032541 -0.0209291 0.999776 5.67818\n0.956152 -0.292857 -0.00280867 -552.441 0.292859 0.955989 0.0178618 -1.55029 -0.0025459 -0.0179012 0.999837 5.71949\n0.959234 -0.282603 -0.0022356 -554.142 0.282604 0.959124 0.014677 -2.04533 -0.00200355 -0.0147105 0.99989 5.7735\n0.963125 -0.269029 -0.0036235 -555.234 0.269054 0.963054 0.0116775 -2.3355 0.000348045 -0.0122218 0.999925 5.81416\n0.968333 -0.249621 -0.00450682 -556.272 0.249655 0.968279 0.0104243 -2.58562 0.00176174 -0.0112193 0.999936 5.83516\n0.974282 -0.225206 -0.00754552 -557.293 0.225279 0.974232 0.0109827 -2.78229 0.00487774 -0.0124 0.999911 5.86326\n0.981069 -0.193442 -0.00911801 -558.279 0.193527 0.981048 0.00960963 -2.95233 0.0070863 -0.0111923 0.999912 5.88346\n0.990939 -0.133699 -0.0128398 -559.669 0.133865 0.990913 0.0131 -3.07363 0.0109717 -0.0147001 0.999832 5.90675\n0.998271 -0.058429 -0.0063626 -560.955 0.0585666 0.997991 0.0241691 -3.07188 0.00493764 -0.0245 0.999688 5.90786\n0.999669 0.0254658 -0.00351937 -562.197 -0.0253044 0.998876 0.0400893 -2.93936 0.00453631 -0.039987 0.99919 5.9534\n0.993004 0.117839 -0.00756438 -563.393 -0.117467 0.992336 0.0383389 -2.7061 0.0120242 -0.0371821 0.999236 5.97949\n0.974706 0.223242 0.0105725 -564.501 -0.223492 0.973584 0.0467428 -2.33504 0.000141785 -0.0479233 0.998851 5.9894\n0.939917 0.341378 0.00412856 -565.585 -0.34123 0.938985 0.0432448 -1.85167 0.0108862 -0.0420553 0.999056 6.01205\n0.884893 0.465787 0.0026189 -566.524 -0.465344 0.883778 0.0489058 -1.19777 0.0204651 -0.0444951 0.9988 6.00338\n0.804142 0.594436 -0.0011034 -567.399 -0.593739 0.803286 0.0469576 -0.411059 0.0287997 -0.0371055 0.998896 6.01442\n0.700909 0.713163 -0.0112221 -568.138 -0.71208 0.700572 0.0462676 0.521578 0.0408583 -0.0244383 0.998866 5.96777\n0.576968 0.816744 -0.00613842 -568.67 -0.815579 0.576518 0.0495798 1.57283 0.0440329 -0.0235996 0.998751 5.95505\n0.436223 0.899828 -0.0043381 -569.099 -0.89876 0.435931 0.0468523 2.73517 0.0440501 -0.0165392 0.998892 5.9003\n0.298779 0.954296 0.00702123 -569.36 -0.953483 0.2982 0.0441295 4.03339 0.0400189 -0.0198796 0.999001 5.86555\n0.185142 0.982536 0.0186022 -569.517 -0.982093 0.18432 0.0389841 5.40804 0.0348745 -0.0254867 0.999067 5.85971\n0.107087 0.993998 0.0223829 -569.628 -0.994167 0.10676 0.0153007 6.82479 0.0128193 -0.0238909 0.999632 5.84556\n0.0568807 0.997738 0.0358127 -569.645 -0.998381 0.0568321 0.0023744 8.26614 0.000333725 -0.0358897 0.999356 5.8592\n0.0340687 0.998891 0.0324893 -569.668 -0.999249 0.0334451 0.0195486 9.29129 0.0184403 -0.0331309 0.999281 5.86258\n0.020081 0.999208 0.0343641 -569.659 -0.998665 0.0184102 0.0482631 10.3712 0.0475922 -0.0352874 0.998243 5.81523\n0.00872091 0.999253 0.0376609 -569.669 -0.998117 0.00641208 0.0609971 11.4229 0.06071 -0.0381219 0.997427 5.82827\n-0.000528818 0.999531 0.0306121 -569.67 -0.998955 -0.00192712 0.0456666 12.4884 0.0457042 -0.030556 0.998488 5.81438\n-0.00583638 0.999317 0.0364798 -569.649 -0.999675 -0.0067365 0.0246006 13.5652 0.0248296 -0.0363244 0.999032 5.78498\n-0.008557 0.99941 0.0332672 -569.637 -0.999685 -0.00933443 0.0232846 14.6851 0.0235814 -0.0330575 0.999175 5.79833\n-0.0090841 0.999567 0.0279702 -569.623 -0.999494 -0.00992878 0.0302099 15.8635 0.0304745 -0.0276817 0.999152 5.76935\n-0.00847925 0.999417 0.0330872 -569.586 -0.999494 -0.00948485 0.0303548 17.0294 0.0306509 -0.032813 0.998991 5.76495\n-0.00851587 0.999557 0.0285141 -569.587 -0.999576 -0.00930334 0.0275992 18.2183 0.0278522 -0.028267 0.999212 5.7727\n-0.00764479 0.999602 0.0271591 -569.584 -0.999554 -0.00842293 0.028653 19.4326 0.0288703 -0.0269279 0.99922 5.74969\n-0.00629257 0.999575 0.0284597 -569.549 -0.99949 -0.00717788 0.0311131 20.6684 0.0313041 -0.0282494 0.999111 5.73573\n-0.00600962 0.99958 0.0283646 -569.547 -0.99953 -0.00685757 0.0298929 21.9189 0.0300749 -0.0281716 0.999151 5.72749\n-0.00431959 0.999557 0.029447 -569.532 -0.999585 -0.00515428 0.0283288 23.1786 0.028468 -0.0293125 0.999165 5.71121\n-0.00390141 0.999389 0.0347368 -569.516 -0.999586 -0.00488823 0.0283689 24.4848 0.0285213 -0.0346117 0.998994 5.68965\n-0.00438531 0.999279 0.0377196 -569.488 -0.999497 -0.00556496 0.0312261 25.7796 0.0314134 -0.0375637 0.9988 5.69587\n-0.00528407 0.999148 0.0409228 -569.466 -0.999471 -0.00659003 0.031844 27.1421 0.0320865 -0.0407329 0.998655 5.67661\n-0.00656149 0.998982 0.0446304 -569.435 -0.999517 -0.00790755 0.0300509 28.489 0.0303732 -0.0444116 0.998551 5.66492\n-0.00746806 0.999004 0.0440015 -569.429 -0.999452 -0.00887622 0.0318945 29.8687 0.0322533 -0.0437392 0.998522 5.65118\n-0.00830232 0.999059 0.0425751 -569.409 -0.999374 -0.00975361 0.0339943 31.2601 0.0343775 -0.0422663 0.998515 5.64205\n-0.0087094 0.998969 0.0445546 -569.372 -0.999535 -0.00999956 0.0288165 32.6622 0.0292323 -0.0442829 0.998591 5.61968\n-0.009803 0.998935 0.0450878 -569.364 -0.999608 -0.0109729 0.0257734 34.0849 0.0262407 -0.0448175 0.99865 5.62194\n-0.00819397 0.998833 0.0475926 -569.324 -0.99965 -0.0093799 0.0247487 35.5603 0.0251662 -0.0473732 0.99856 5.60805\n-0.00585685 0.998715 0.0503437 -569.312 -0.999701 -0.00704373 0.0234305 37.7691 0.023755 -0.0501914 0.998457 5.61709\n-0.00386109 0.99853 0.0540658 -569.288 -0.999715 -0.00512906 0.0233332 39.2626 0.0235762 -0.0539603 0.998265 5.60283\n-0.00205045 0.998405 0.0564151 -569.276 -0.999724 -0.0033673 0.0232571 40.7639 0.02341 -0.0563518 0.998136 5.60232\n-0.000979697 0.998281 0.0586058 -569.268 -0.999736 -0.00232398 0.0228739 42.2915 0.0229708 -0.0585679 0.998019 5.60001\n0.000470918 0.998012 0.0630236 -569.256 -0.999689 -0.00110168 0.0249155 43.8243 0.0249354 -0.0630157 0.997701 5.59313\n0.00190888 0.997558 0.069821 -569.227 -0.999617 -2.42101e-05 0.027675 45.3999 0.0276091 -0.0698471 0.997176 5.57612\n0.00175273 0.997124 0.0757622 -569.214 -0.999608 -0.000370284 0.0279989 47.7606 0.0279464 -0.0757816 0.996733 5.5758\n0.00138971 0.997023 0.0770944 -569.186 -0.999594 -0.000809963 0.0284935 50.1632 0.0284712 -0.0771027 0.996617 5.56548\n0.000401221 0.997419 0.0717953 -569.2 -0.999609 -0.00160771 0.0279214 51.7795 0.0279648 -0.0717784 0.997028 5.57443\n-0.000300058 0.997955 0.0639164 -569.187 -0.999599 -0.00210924 0.0282399 54.2297 0.028317 -0.0638823 0.997556 5.55475\n-0.00162515 0.998311 0.0580721 -569.202 -0.999368 -0.00368282 0.0353437 55.8707 0.0354979 -0.057978 0.997687 5.55402\n-0.00213686 0.998426 0.0560476 -569.2 -0.999352 -0.00414664 0.0357668 57.4812 0.0359429 -0.0559348 0.997787 5.53521\n-0.00281132 0.998491 0.0548372 -569.194 -0.999334 -0.00480068 0.0361797 59.1019 0.0363884 -0.0546989 0.99784 5.50467\n-0.00314832 0.998511 0.0544512 -569.17 -0.999338 -0.00511454 0.0360082 60.6897 0.0362331 -0.0543018 0.997867 5.49974\n-0.00304208 0.998716 0.0505697 -569.159 -0.999382 -0.00480702 0.0348162 62.286 0.0350146 -0.0504325 0.998113 5.47716\n-0.00265636 0.998594 0.0529479 -569.141 -0.999281 -0.00465268 0.037616 63.8639 0.0378095 -0.0528099 0.997889 5.4463\n-0.00334561 0.998366 0.0570514 -569.132 -0.999291 -0.00547744 0.0372515 65.4113 0.0375032 -0.0568863 0.997676 5.43985\n-0.00120276 0.998184 0.0602243 -569.107 -0.99936 -0.00335279 0.0356119 67.6602 0.0357492 -0.060143 0.997549 5.43156\n0.000466317 0.998112 0.0614261 -569.106 -0.999162 -0.0020494 0.0408859 69.1078 0.0409345 -0.0613937 0.997274 5.41105\n0.00193039 0.998525 0.0542651 -569.094 -0.999096 -0.000378313 0.0425026 71.1551 0.0424604 -0.0542981 0.997622 5.42983\n0.000128079 0.998847 0.0480168 -569.121 -0.999719 -0.00101076 0.0236925 72.4593 0.0237137 -0.0480064 0.998565 5.41558\n-0.000149868 0.998797 0.0490394 -569.091 -0.999672 -0.00140574 0.0255759 73.7114 0.025614 -0.0490195 0.998469 5.38827\n-0.000292454 0.999033 0.0439647 -569.09 -0.999246 -0.00199935 0.0387853 74.9357 0.0388356 -0.0439202 0.99828 5.42011\n-0.000623864 0.999226 0.0393222 -569.105 -0.999567 -0.00178005 0.0293747 76.129 0.029422 -0.0392869 0.998795 5.38673\n0.000708978 0.998948 0.0458583 -569.088 -0.999701 -0.000412138 0.0244334 77.2892 0.0244265 -0.0458619 0.998649 5.38867\n0.00157682 0.999154 0.0410848 -569.102 -0.999607 0.000425672 0.0280124 78.4414 0.0279713 -0.0411129 0.998763 5.39579\n0.00145432 0.999219 0.0394871 -569.119 -0.999727 0.000532428 0.0233471 79.6002 0.0233079 -0.0395103 0.998947 5.39842\n0.00187372 0.998945 0.0458771 -569.112 -0.999992 0.00203528 -0.00347515 80.7188 -0.00356485 -0.0458703 0.998941 5.41831\n0.00362176 0.998865 0.0474867 -569.081 -0.999993 0.00366815 -0.000889692 81.8697 -0.00106287 -0.0474832 0.998871 5.40973\n0.00489831 0.998893 0.046787 -569.087 -0.998691 0.00250443 0.0510876 83.0956 0.0509138 -0.046976 0.997598 5.33536\n0.00567105 0.99856 0.0533404 -569.073 -0.997517 0.00190426 0.0704053 84.2621 0.0702023 -0.0536072 0.996091 5.36023\n0.00227974 0.998477 0.0551159 -569.066 -0.999449 0.00045052 0.0331783 85.4058 0.0331029 -0.0551611 0.997929 5.3979\n0.00102431 0.998596 0.0529569 -569.065 -0.999935 0.000420893 0.0114043 86.603 0.011366 -0.0529652 0.998532 5.33017\n0.000594137 0.998464 0.0554088 -569.049 -0.999555 -0.00106019 0.0298225 87.8637 0.0298354 -0.0554019 0.998018 5.30705\n-0.000137221 0.998553 0.0537818 -569.046 -0.999373 -0.00204086 0.0353423 89.1018 0.0354009 -0.0537432 0.997927 5.33692\n-0.00173706 0.998509 0.0545628 -569.029 -0.999738 -0.00297908 0.0226901 90.9812 0.0228188 -0.0545091 0.998253 5.30066\n-0.00294448 0.998567 0.0534257 -569.032 -0.999591 -0.00445946 0.0282596 92.2584 0.0284574 -0.0533206 0.998172 5.30775\n-0.00399261 0.998744 0.0499492 -569.027 -0.999646 -0.00530028 0.0260751 94.2206 0.0263071 -0.0498274 0.998411 5.29864\n-0.00448519 0.998886 0.0469723 -569.022 -0.999672 -0.00566293 0.02497 95.5219 0.0252081 -0.0468449 0.998584 5.3025\n-0.00407959 0.998894 0.0468358 -569.008 -0.999618 -0.00535413 0.0271198 96.855 0.0273406 -0.0467073 0.998534 5.27776\n-0.00402063 0.99885 0.047781 -568.992 -0.999468 -0.00556077 0.0321443 98.8265 0.032373 -0.0476263 0.99834 5.27604\n-0.00310727 0.998685 0.0511642 -568.968 -0.99951 -0.00469508 0.0309429 100.131 0.0311424 -0.051043 0.998211 5.25621\n-0.00274282 0.99854 0.0539468 -568.962 -0.999481 -0.00446856 0.031895 101.418 0.0320895 -0.0538314 0.998034 5.2686\n-0.00160611 0.998612 0.0526473 -568.958 -0.99946 -0.00333099 0.0326916 102.699 0.0328216 -0.0525664 0.998078 5.25192\n-0.000744003 0.998701 0.0509406 -568.958 -0.99947 -0.00240058 0.0324663 103.969 0.0325464 -0.0508894 0.998174 5.25693\n0.000289676 0.998885 0.0472006 -568.96 -0.999466 -0.00125365 0.0326642 105.219 0.032687 -0.0471849 0.998351 5.24572\n0.0018436 0.998987 0.0449548 -568.968 -0.999439 0.00033718 0.0334944 106.467 0.0334453 -0.0449913 0.998427 5.23278\n0.0029523 0.999074 0.0429246 -568.979 -0.999676 0.00186359 0.0253812 108.293 0.0252777 -0.0429856 0.998756 5.23391\n0.00232153 0.998833 0.0482432 -568.972 -0.999977 0.00262523 -0.00623267 109.464 -0.00635204 -0.0482276 0.998816 5.26101\n0.00437401 0.999218 0.0392924 -568.986 -0.999807 0.00361804 0.01929 111.306 0.0191328 -0.0393692 0.999042 5.24082\n0.00651425 0.999276 0.0374718 -568.976 -0.997304 0.00375305 0.0732912 112.542 0.0730975 -0.0378482 0.996606 5.19738\n0.00408605 0.999155 0.040896 -568.977 -0.998853 0.00212734 0.0478243 113.654 0.0476969 -0.0410445 0.998018 5.28649\n0.0014018 0.99948 0.032227 -569.004 -0.999986 0.00123612 0.00516015 114.812 0.00511763 -0.0322338 0.999467 5.2565\n0.000690922 0.9995 0.0316142 -568.992 -0.99989 0.000221382 0.0148533 116.042 0.0148388 -0.031621 0.99939 5.20702\n-0.000985068 0.999348 0.0360859 -568.976 -0.999622 -0.0019756 0.027424 117.234 0.0274775 -0.0360452 0.998972 5.23176\n-0.00317156 0.999217 0.0394273 -568.963 -0.999815 -0.00391645 0.0188298 118.408 0.0189695 -0.0393603 0.999045 5.24174\n-0.00496715 0.999049 0.0433143 -568.932 -0.999844 -0.0056957 0.0167128 119.612 0.0169436 -0.0432245 0.998922 5.22761\n-0.00621275 0.998538 0.0536919 -568.922 -0.999701 -0.00747098 0.0232655 120.816 0.0236326 -0.0535313 0.998286 5.22129\n-0.00713424 0.998099 0.0612194 -568.901 -0.999633 -0.00871844 0.0256493 122.002 0.0261343 -0.0610139 0.997795 5.23299\n-0.00797008 0.997879 0.0646108 -568.88 -0.999675 -0.0095158 0.0236513 123.193 0.024216 -0.0644013 0.99763 5.23288\n-0.00738505 0.997634 0.0683455 -568.855 -0.999653 -0.0090934 0.0247186 124.384 0.0252816 -0.0681392 0.997355 5.23246\n-0.00711259 0.997662 0.067972 -568.828 -0.999614 -0.00891993 0.0263231 126.179 0.0268679 -0.0677585 0.99734 5.24245\n-0.00777992 0.997769 0.0663083 -568.822 -0.999818 -0.00891735 0.016875 127.358 0.0174286 -0.0661649 0.997656 5.24408\n-0.0071822 0.997606 0.0687844 -568.809 -0.999927 -0.00783251 0.00918931 128.536 0.00970607 -0.0687134 0.997589 5.26448\n-0.00641852 0.997944 0.0637671 -568.801 -0.9992 -0.00891688 0.0389726 129.776 0.0394611 -0.063466 0.997204 5.22063\n-0.00485727 0.99754 0.0699357 -568.77 -0.998198 -0.00901974 0.0593264 130.996 0.0598113 -0.0695216 0.995786 5.20499\n-0.00729837 0.997164 0.0749069 -568.759 -0.999357 -0.00990221 0.0344487 132.162 0.0350927 -0.0746073 0.996595 5.20393\n-0.0092719 0.996918 0.077899 -568.741 -0.999494 -0.0116093 0.0296067 133.368 0.0304198 -0.0775851 0.996522 5.16716\n-0.0104157 0.996408 0.0840375 -568.701 -0.999118 -0.0137898 0.0396703 134.597 0.0406867 -0.0835502 0.995673 5.14456\n-0.0131663 0.995769 0.0909402 -568.666 -0.999232 -0.0164586 0.0355489 135.807 0.0368953 -0.0904023 0.995222 5.14402\n-0.0165825 0.995384 0.0945296 -568.633 -0.999354 -0.0195133 0.0301642 137.003 0.0318696 -0.0939684 0.995065 5.13225\n-0.019485 0.994848 0.0994901 -568.598 -0.999256 -0.0226906 0.0311912 138.227 0.033288 -0.0988083 0.99455 5.10781\n-0.0234398 0.994344 0.103587 -568.547 -0.999169 -0.026757 0.0307501 139.423 0.0333479 -0.102781 0.994145 5.11016\n-0.0300545 0.994336 0.101948 -568.508 -0.999292 -0.0321991 0.0194558 140.587 0.0226282 -0.101291 0.994599 5.13682\n-0.0404843 0.993844 0.103124 -568.451 -0.998973 -0.0423631 0.0160944 141.765 0.020364 -0.102366 0.994538 5.11126\n-0.051764 0.992855 0.107514 -568.36 -0.998056 -0.0551739 0.0289847 142.943 0.0347096 -0.105804 0.993781 5.11171\n-0.0674448 0.992268 0.104191 -568.263 -0.996982 -0.071049 0.0312727 144.092 0.0384336 -0.101768 0.994065 5.11239\n-0.0909386 0.99056 0.102572 -568.144 -0.995339 -0.0937267 0.0226879 145.223 0.0320875 -0.100031 0.994467 5.09514\n-0.121109 0.987256 0.103235 -567.98 -0.992141 -0.123687 0.0189244 146.366 0.031452 -0.100132 0.994477 5.08176\n-0.156412 0.982514 0.101001 -567.774 -0.987173 -0.158825 0.0162618 147.479 0.0320189 -0.0971616 0.994753 5.07561\n-0.1924 0.976199 0.100093 -567.526 -0.980743 -0.194772 0.0144 148.599 0.0335526 -0.0953951 0.994874 5.05168\n-0.228566 0.96843 0.0995017 -567.236 -0.973013 -0.230575 0.00902845 149.696 0.031686 -0.0947528 0.994996 5.03765\n-0.266483 0.959382 0.0925954 -566.913 -0.963494 -0.267727 0.00106003 150.775 0.0258073 -0.0889326 0.995703 5.07527\n-0.306433 0.948872 0.0757709 -566.55 -0.951685 -0.307054 -0.00360847 151.852 0.0198417 -0.0732158 0.997119 5.05565\n-0.345965 0.934825 0.0800663 -566.118 -0.93791 -0.346867 -0.00279569 152.913 0.0251589 -0.0760622 0.996786 5.00976\n-0.38714 0.918158 0.0843079 -565.623 -0.921575 -0.388176 -0.00440442 153.92 0.0286824 -0.0794012 0.99643 5.0351\n-0.450565 0.889474 0.0763413 -564.823 -0.892192 -0.451643 -0.00349011 155.445 0.0313746 -0.0696836 0.997076 4.98297\n-0.492324 0.866078 0.0867516 -564.209 -0.869559 -0.493803 -0.00498515 156.403 0.0385207 -0.07789 0.996217 4.96946\n-0.53557 0.839869 0.0882329 -563.564 -0.843629 -0.536814 -0.0109799 157.33 0.038143 -0.0803163 0.996039 4.96491\n-0.578078 0.812053 0.0799703 -562.872 -0.815226 -0.578978 -0.0137949 158.245 0.0350989 -0.0731684 0.996702 4.97061\n-0.618499 0.782211 0.0748691 -562.118 -0.784751 -0.619762 -0.00779292 159.141 0.0403053 -0.0635735 0.997163 4.9147\n-0.657191 0.749205 0.0824186 -561.306 -0.752127 -0.65898 -0.00703705 159.977 0.04904 -0.0666139 0.996573 4.8751\n-0.695212 0.714432 0.0791715 -560.453 -0.717235 -0.696748 -0.0107495 160.792 0.0474828 -0.0642577 0.996803 4.87188\n-0.731291 0.678015 0.0742294 -559.554 -0.680523 -0.732618 -0.0125907 161.568 0.0458451 -0.0597223 0.997162 4.82663\n-0.762855 0.642018 0.0765818 -558.597 -0.644704 -0.76429 -0.0147187 162.299 0.049081 -0.0606009 0.996955 4.78486\n-0.790435 0.60782 0.0759418 -557.586 -0.610544 -0.791789 -0.0175043 163.01 0.0494904 -0.0602018 0.996959 4.76278\n-0.815473 0.574006 0.0743052 -556.574 -0.5764 -0.817046 -0.0141116 163.693 0.0526106 -0.0543371 0.997136 4.72573\n-0.838131 0.540306 0.0748695 -555.514 -0.542559 -0.839928 -0.012252 164.337 0.0562652 -0.0508898 0.997118 4.67154\n-0.870244 0.487183 0.0730014 -553.851 -0.489325 -0.871991 -0.0138772 165.215 0.0568958 -0.047798 0.997235 4.60923\n-0.889388 0.450877 0.075494 -552.698 -0.453096 -0.891344 -0.014453 165.756 0.0607746 -0.0470603 0.997042 4.54212\n-0.906147 0.415162 0.0808559 -551.51 -0.41769 -0.908439 -0.0165672 166.251 0.0665745 -0.048785 0.996588 4.49278\n-0.926543 0.366994 0.0826688 -549.681 -0.369696 -0.928946 -0.0196119 166.936 0.0695974 -0.0487335 0.996384 4.42479\n-0.937973 0.337426 0.0796905 -548.452 -0.339667 -0.940409 -0.0160553 167.364 0.0695242 -0.0421277 0.99669 4.35271\n-0.947389 0.309808 0.0804535 -547.201 -0.312411 -0.949698 -0.0217606 167.739 0.0696649 -0.0457504 0.996521 4.26988\n-0.954017 0.288666 0.0807685 -545.944 -0.291418 -0.956285 -0.0243933 168.086 0.0701962 -0.046809 0.996434 4.24696\n-0.959277 0.271646 0.0774291 -544.705 -0.273714 -0.961656 -0.0172692 168.434 0.0697691 -0.0377594 0.996848 4.1813\n-0.96341 0.256482 0.0778327 -543.443 -0.258536 -0.965845 -0.0173892 168.752 0.0707143 -0.0368754 0.996815 4.1083\n-0.96628 0.245671 0.0771283 -542.179 -0.24747 -0.968786 -0.0145545 169.058 0.0711452 -0.0331507 0.996915 4.06929\n-0.968488 0.239332 0.0689311 -540.925 -0.240342 -0.970666 -0.00663687 169.387 0.0653206 -0.0229948 0.997599 4.01557\n-0.970206 0.237685 0.0469737 -539.686 -0.238715 -0.97093 -0.0176026 169.669 0.0414243 -0.0282915 0.998741 3.98639\n-0.969819 0.236856 0.0578796 -538.403 -0.23786 -0.971237 -0.0110214 169.955 0.0536043 -0.024456 0.998263 3.95182\n-0.965682 0.238134 0.103685 -537.072 -0.238625 -0.97108 0.00782485 170.307 0.10255 -0.0171856 0.994579 3.8472\n-0.968194 0.243536 0.0573576 -535.197 -0.244322 -0.969669 -0.00699503 170.785 0.0539144 -0.0207863 0.998329 3.77655\n-0.967074 0.248389 0.0554124 -533.9 -0.248942 -0.968513 -0.00321301 171.102 0.0528695 -0.0169017 0.998458 3.72162\n-0.964971 0.252491 0.0712703 -532.565 -0.252987 -0.967467 0.00212266 171.468 0.0694877 -0.0159822 0.997455 3.64966\n-0.964377 0.256546 0.0645088 -531.245 -0.25742 -0.966284 -0.00548379 171.813 0.060927 -0.0218943 0.997902 3.5978\n-0.963702 0.260238 0.0596245 -529.914 -0.26099 -0.965328 -0.00506504 172.168 0.0562391 -0.0204426 0.998208 3.56382\n-0.962728 0.263208 0.0622668 -528.557 -0.263938 -0.964533 -0.00365578 172.541 0.0590962 -0.0199541 0.998053 3.50129\n-0.961874 0.265904 0.063978 -527.184 -0.266718 -0.963764 -0.00438979 172.914 0.0604925 -0.0212865 0.997942 3.44662\n-0.961193 0.268385 0.0638576 -525.82 -0.269114 -0.963104 -0.00294155 173.31 0.0607121 -0.0200124 0.997955 3.40017\n-0.960338 0.271343 0.0642114 -524.424 -0.272019 -0.96229 -0.00186947 173.701 0.0612827 -0.0192621 0.997935 3.34491\n-0.959988 0.273176 0.0616294 -523.027 -0.273799 -0.961785 -0.00173227 174.098 0.058801 -0.018537 0.998098 3.29541\n-0.959879 0.274084 0.0592427 -521.612 -0.274651 -0.961543 -0.00149549 174.501 0.0565545 -0.0177066 0.998242 3.24621\n-0.959771 0.274416 0.0594626 -520.169 -0.275044 -0.961428 -0.00249483 174.91 0.0564844 -0.0187493 0.998227 3.19313\n-0.959475 0.274953 0.061709 -518.708 -0.27556 -0.961283 -0.00138187 175.326 0.0589398 -0.0183304 0.998093 3.14204\n-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\n-0.958982 0.275898 0.0650659 -515.764 -0.276517 -0.961009 -0.000521979 176.169 0.0623849 -0.0184924 0.997881 3.02157\n-0.959109 0.275066 0.0667052 -514.258 -0.275868 -0.961191 -0.0029489 176.593 0.0633053 -0.0212302 0.997768 2.94601\n-0.959236 0.274265 0.0681488 -512.748 -0.275186 -0.961381 -0.00434176 177.015 0.0643262 -0.0229184 0.997666 2.89658\n-0.959531 0.273832 0.0657026 -511.222 -0.274693 -0.961523 -0.00426289 177.455 0.0620072 -0.0221384 0.99783 2.83205\n-0.959678 0.273434 0.0652094 -509.677 -0.274153 -0.961684 -0.00217994 177.892 0.0621147 -0.0199694 0.997869 2.7703\n-0.959942 0.272809 0.063925 -508.126 -0.273428 -0.961892 -0.000978137 178.336 0.0612221 -0.0184178 0.997954 2.71324\n-0.960189 0.271987 0.0637146 -506.555 -0.27266 -0.962108 -0.00195717 178.781 0.060768 -0.0192517 0.997966 2.63753\n-0.96029 0.271338 0.0649472 -504.961 -0.272155 -0.962245 -0.00391587 179.217 0.0614326 -0.0214361 0.997881 2.57616\n-0.960231 0.270987 0.067244 -503.37 -0.271835 -0.962337 -0.00362418 179.66 0.0637293 -0.0217593 0.99773 2.51847\n-0.959962 0.271869 0.067528 -501.782 -0.272755 -0.962075 -0.00409562 180.101 0.0638535 -0.0223503 0.997709 2.45615\n-0.959561 0.27369 0.0658532 -500.208 -0.274525 -0.961572 -0.0038135 180.561 0.0622789 -0.0217377 0.997822 2.4009\n-0.959064 0.275353 0.0661663 -498.614 -0.276257 -0.961072 -0.00475182 181.012 0.0622821 -0.0228362 0.997797 2.32499\n-0.958502 0.277144 0.0668204 -497.021 -0.278339 -0.96044 -0.00911277 181.458 0.0616514 -0.0273333 0.997723 2.26475\n-0.957484 0.280201 0.0686447 -495.412 -0.281347 -0.959577 -0.0074495 181.918 0.0637825 -0.0264458 0.997613 2.20167\n-0.956092 0.285343 0.0668456 -493.801 -0.286554 -0.958021 -0.00909417 182.401 0.0614445 -0.0278498 0.997722 2.12714\n-0.954419 0.292306 0.0603412 -492.226 -0.293499 -0.955887 -0.0117444 182.894 0.0542464 -0.0289192 0.998109 2.09645\n-0.952103 0.299671 0.0607991 -490.6 -0.300968 -0.953542 -0.0132323 183.398 0.0540092 -0.0308971 0.998062 2.02781\n-0.949659 0.306658 0.0641003 -488.981 -0.308318 -0.95112 -0.0176073 183.905 0.0555677 -0.0364842 0.997788 1.97883\n-0.947339 0.314219 0.0617627 -487.389 -0.315952 -0.948556 -0.0203893 184.429 0.0521786 -0.0388296 0.997883 1.93553\n-0.944938 0.322057 0.0580673 -485.795 -0.323687 -0.945931 -0.0210077 184.976 0.048162 -0.0386466 0.998092 1.89798\n-0.941877 0.330655 0.0594606 -484.188 -0.332182 -0.94305 -0.0176575 185.546 0.0502357 -0.0363829 0.998074 1.85657\n-0.938485 0.339678 0.0621637 -482.581 -0.341155 -0.939894 -0.0145895 186.133 0.0534715 -0.0348995 0.997959 1.81126\n-0.935289 0.348506 0.0614703 -480.988 -0.34978 -0.936767 -0.0109976 186.732 0.0537506 -0.031787 0.998048 1.76996\n-0.932108 0.357529 0.0578652 -479.4 -0.358643 -0.933423 -0.00980873 187.345 0.0505058 -0.0298957 0.998276 1.71934\n-0.92876 0.366094 0.0581354 -477.809 -0.367133 -0.930135 -0.00794518 187.974 0.0511651 -0.0287226 0.998277 1.6717\n-0.925172 0.374957 0.0588523 -476.229 -0.376037 -0.926569 -0.00808966 188.623 0.0514975 -0.029615 0.998234 1.63021\n-0.919346 0.389064 0.0585923 -473.854 -0.390145 -0.92072 -0.00783002 189.634 0.0509007 -0.030058 0.998251 1.5648\n-0.91564 0.398114 0.0557509 -472.283 -0.399011 -0.91693 -0.00551812 190.318 0.0489228 -0.0272978 0.998429 1.51917\n-0.912104 0.406194 0.0554409 -470.704 -0.407161 -0.91333 -0.00693627 191.027 0.0478183 -0.0289 0.998438 1.48737\n-0.908646 0.413689 0.0567762 -469.116 -0.414549 -0.910019 -0.00376374 191.755 0.0501104 -0.0269565 0.99838 1.42746\n-0.905813 0.420237 0.053892 -467.552 -0.420964 -0.907074 -0.00237457 192.484 0.0478862 -0.0248375 0.998544 1.39333\n-0.902932 0.426562 0.0525273 -465.99 -0.427328 -0.904089 -0.00376891 193.222 0.0458817 -0.0258494 0.998612 1.36009\n-0.899891 0.432566 0.0555269 -464.42 -0.433312 -0.901243 -0.00155616 193.981 0.0493701 -0.0254608 0.998456 1.3178\n-0.896761 0.439212 0.0539729 -462.85 -0.439963 -0.898013 -0.00227418 194.75 0.0474695 -0.0257854 0.99854 1.27025\n-0.894198 0.444762 0.0509559 -461.304 -0.44561 -0.895207 -0.006073 195.517 0.0429151 -0.0281369 0.998682 1.24583\n-0.890962 0.451275 0.0503642 -459.74 -0.452105 -0.891946 -0.00586389 196.31 0.0422759 -0.0279944 0.998714 1.21012\n-0.887074 0.458616 0.0526417 -458.168 -0.459464 -0.888184 -0.00461906 197.125 0.0446372 -0.0282844 0.998603 1.1717\n-0.882978 0.46639 0.0532034 -456.623 -0.467268 -0.884103 -0.00471606 197.938 0.0448378 -0.0290244 0.998573 1.14428\n-0.87912 0.47414 0.0483661 -455.076 -0.47483 -0.880072 -0.00321014 198.782 0.0410436 -0.0257877 0.998825 1.11889\n-0.874429 0.482563 0.0500688 -453.524 -0.483274 -0.875466 -0.00244345 199.645 0.0426544 -0.0263336 0.998743 1.08\n-0.869936 0.49033 0.0527939 -451.982 -0.491053 -0.871129 -0.000822691 200.521 0.0455869 -0.0266403 0.998605 1.05348\n-0.865136 0.498608 0.054123 -450.442 -0.499618 -0.866224 -0.00612213 201.4 0.0438301 -0.0323373 0.998516 1.00183\n-0.860741 0.50619 0.0538174 -448.901 -0.507332 -0.861701 -0.00924374 202.302 0.0416954 -0.0352597 0.998508 0.979193\n-0.856023 0.514271 0.052436 -447.374 -0.515619 -0.856676 -0.0156047 203.216 0.0368956 -0.040395 0.998502 0.951712\n-0.851103 0.522162 0.0545043 -445.851 -0.523622 -0.851798 -0.0161291 204.138 0.0380046 -0.0422671 0.998383 0.930199\n-0.843534 0.534261 0.054916 -443.59 -0.535732 -0.844243 -0.0156857 205.566 0.0379822 -0.0426516 0.998368 0.901186\n-0.838659 0.542211 0.0515589 -442.091 -0.543555 -0.839222 -0.0159363 206.541 0.0346286 -0.0413902 0.998543 0.876201\n-0.833493 0.550187 0.0508283 -440.597 -0.551482 -0.834048 -0.0152214 207.522 0.0340186 -0.0407178 0.998591 0.85951\n-0.828413 0.557959 0.049121 -439.105 -0.55909 -0.829017 -0.0121984 208.536 0.0339159 -0.0375683 0.998718 0.842993\n-0.822868 0.566033 0.0499514 -437.64 -0.567011 -0.823682 -0.00689361 209.55 0.037242 -0.0339955 0.998728 0.827322\n-0.81744 0.573856 0.0498179 -436.188 -0.574591 -0.818441 -0.000537892 210.581 0.0404643 -0.0290646 0.998758 0.802041\n-0.812069 0.581557 0.0483346 -434.752 -0.582128 -0.813093 0.00273082 211.614 0.0408886 -0.0259193 0.998827 0.763382\n-0.806933 0.588873 0.0456955 -433.343 -0.589274 -0.807914 0.0055629 212.655 0.0401939 -0.0224383 0.99894 0.74398\n-0.801806 0.595921 0.0445538 -431.94 -0.596307 -0.802738 0.00553511 213.701 0.0390635 -0.0221297 0.998992 0.708552\n-0.796895 0.602414 0.0453356 -430.561 -0.602786 -0.797876 0.00651312 214.749 0.0400958 -0.0221374 0.998951 0.68981\n-0.791775 0.609249 0.0436812 -429.203 -0.609682 -0.792635 0.00413495 215.791 0.0371424 -0.0233577 0.999037 0.663551\n-0.78672 0.615884 0.0419406 -427.869 -0.616342 -0.787475 0.00248602 216.834 0.0345582 -0.0238939 0.999117 0.660407\n-0.781685 0.622266 0.0418718 -426.534 -0.622737 -0.782428 0.00223549 217.898 0.0341527 -0.0243277 0.99912 0.642417\n-0.776762 0.628277 0.043686 -425.218 -0.628637 -0.77767 0.00665781 218.973 0.0381563 -0.0222911 0.999023 0.610567\n-0.771503 0.634327 0.0491048 -423.948 -0.634741 -0.772676 0.00864778 220.014 0.0434276 -0.024497 0.998756 0.615942\n-0.768128 0.638205 0.0517112 -422.735 -0.638326 -0.769594 0.0162958 221.029 0.0501967 -0.0204913 0.998529 0.588081\n-0.767088 0.639448 0.0517824 -421.558 -0.639727 -0.76849 0.0131755 222.005 0.0482193 -0.0230198 0.998571 0.562717\n-0.765258 0.641468 0.0538465 -420.433 -0.641987 -0.766659 0.00931176 222.938 0.0472551 -0.0274428 0.998506 0.531918\n-0.763371 0.643044 0.061314 -419.349 -0.643762 -0.765162 0.00983653 223.849 0.0532404 -0.0319627 0.99807 0.519765\n-0.762475 0.644043 0.0619687 -418.325 -0.645037 -0.764135 0.00501085 224.714 0.0505796 -0.0361514 0.998066 0.50788\n-0.761151 0.646487 0.0520004 -416.852 -0.647279 -0.762251 0.00208275 225.963 0.0409838 -0.0320735 0.998645 0.499674\n-0.75952 0.648642 0.0489232 -415.897 -0.649355 -0.760484 0.00171292 226.787 0.0383163 -0.0304675 0.998801 0.45543\n-0.758345 0.64974 0.0524524 -414.97 -0.650477 -0.759517 0.00385916 227.579 0.0423459 -0.0311925 0.998616 0.450119\n-0.758741 0.649445 0.0503326 -414.083 -0.649826 -0.760009 0.0106045 228.357 0.0451402 -0.0246614 0.998676 0.448529\n-0.758838 0.649462 0.0486306 -413.225 -0.649939 -0.75995 0.00740536 229.094 0.0417663 -0.0259874 0.998789 0.424421\n-0.758918 0.649452 0.0474921 -412.392 -0.649897 -0.759985 0.00748153 229.811 0.0409522 -0.0251871 0.998844 0.423903\n-0.758968 0.649826 0.0411517 -411.574 -0.650161 -0.759769 0.00646414 230.514 0.0354664 -0.0218492 0.999132 0.413703\n-0.755286 0.654037 0.0421673 -410.757 -0.654472 -0.756073 0.00440525 231.227 0.0347628 -0.0242701 0.999101 0.388923\n-0.748668 0.661382 0.0455105 -409.964 -0.661813 -0.749636 0.00698318 231.953 0.0387349 -0.0248914 0.998939 0.386432\n-0.739596 0.671484 0.0458998 -409.196 -0.671762 -0.74068 0.0113755 232.698 0.0416355 -0.0224205 0.998881 0.36757\n-0.728025 0.683797 0.049006 -408.418 -0.684243 -0.72919 0.00962915 233.43 0.0423191 -0.0265217 0.998752 0.345682\n-0.714154 0.698169 0.0504367 -407.673 -0.69869 -0.715363 0.00936231 234.175 0.042617 -0.0285535 0.998683 0.330487\n-0.699954 0.712517 0.0488327 -406.95 -0.712957 -0.701125 0.0107803 234.942 0.0419189 -0.0272699 0.998749 0.316048\n-0.687108 0.724851 0.0497253 -406.227 -0.725352 -0.688299 0.0104312 235.732 0.0417869 -0.028901 0.998708 0.289133\n-0.674536 0.736331 0.0530808 -405.502 -0.737035 -0.6758 0.00858987 236.529 0.042197 -0.0333282 0.998553 0.274603\n-0.663125 0.746736 0.051488 -404.799 -0.747371 -0.66434 0.00944267 237.35 0.0412567 -0.032219 0.998629 0.268196\n-0.653986 0.75499 0.0478818 -404.091 -0.755606 -0.654985 0.00735152 238.19 0.0369122 -0.0313719 0.998826 0.249362\n-0.645611 0.762152 0.0480588 -403.368 -0.762728 -0.646658 0.00887095 239.052 0.0378386 -0.0309286 0.998805 0.241283\n-0.638764 0.768842 0.0293819 -402.672 -0.76888 -0.639272 0.0124713 239.936 0.0283714 -0.0146249 0.99949 0.271555\n-0.631762 0.774954 0.0179685 -401.958 -0.775149 -0.631719 -0.00869483 240.837 0.00461292 -0.0194213 0.999801 0.255363\n-0.625875 0.779522 0.0250143 -401.202 -0.77975 -0.626091 0.00101586 241.787 0.0164531 -0.0188691 0.999687 0.252137\n-0.620082 0.782735 0.0531398 -400.409 -0.781033 -0.62229 0.0523728 242.812 0.0740624 -0.00902849 0.997213 0.167671\n-0.615389 0.78431 0.0784433 -399.607 -0.782382 -0.61989 0.0601288 243.769 0.0957859 -0.02437 0.995104 0.0923176\n-0.615522 0.785891 0.0592262 -398.85 -0.78576 -0.617755 0.0310015 244.734 0.0609511 -0.0274555 0.997763 0.121055\n-0.614967 0.787466 0.0413726 -398.059 -0.787948 -0.615704 0.00686968 245.765 0.0308829 -0.0283748 0.99912 0.0368558\n-0.613281 0.787743 0.0578578 -396.773 -0.788751 -0.61466 0.0080793 247.365 0.0419272 -0.0406805 0.998292 0.0195522\n-0.612104 0.788673 0.0576483 -395.937 -0.789687 -0.61346 0.00778087 248.459 0.0415015 -0.0407614 0.998307 -0.00399816\n-0.610935 0.789595 0.0574262 -395.081 -0.790619 -0.612264 0.00738485 249.579 0.0409911 -0.0408906 0.998322 -0.0289074\n-0.610342 0.79015 0.0560851 -394.198 -0.791114 -0.611622 0.00754242 250.715 0.0402626 -0.0397663 0.998398 -0.046508\n-0.609784 0.79131 0.04463 -393.325 -0.791982 -0.610531 0.0040416 251.873 0.0304461 -0.0328816 0.998995 -0.0446038\n-0.608706 0.792554 0.036541 -392.422 -0.79298 -0.609232 0.00431901 253.05 0.025685 -0.0263473 0.999323 -0.058629\n-0.607413 0.793561 0.0362112 -391.504 -0.793649 -0.608182 0.0153567 254.259 0.0342094 -0.0194112 0.999226 -0.0617951\n-0.606489 0.794331 0.0347837 -390.583 -0.794303 -0.607256 0.0179926 255.487 0.0354147 -0.0167165 0.999233 -0.0945668\n-0.605735 0.79494 0.0339946 -389.641 -0.794987 -0.60643 0.0154085 256.717 0.0328642 -0.0176919 0.999303 -0.115924\n-0.604541 0.795752 0.0361762 -388.683 -0.79582 -0.605321 0.0160414 257.959 0.0346632 -0.0190921 0.999217 -0.128601\n-0.603366 0.796713 0.0346071 -387.75 -0.796768 -0.604084 0.0155792 259.215 0.0333178 -0.0181739 0.99928 -0.146941\n-0.601479 0.797953 0.0386465 -386.787 -0.7981 -0.602331 0.0152893 260.479 0.0354781 -0.0216476 0.999136 -0.169391\n-0.59969 0.799021 0.0440001 -385.809 -0.799142 -0.600836 0.0191635 261.757 0.0417489 -0.0236701 0.998848 -0.202965\n-0.597755 0.800349 0.0461624 -384.846 -0.800617 -0.598935 0.0169857 263.056 0.0412428 -0.0268051 0.99879 -0.232323\n-0.595648 0.801953 0.0455553 -383.87 -0.802273 -0.596759 0.0153625 264.361 0.0395056 -0.0273972 0.998844 -0.250016\n-0.591433 0.805062 0.0456367 -382.876 -0.805325 -0.59259 0.0169838 265.702 0.0407168 -0.0267076 0.998814 -0.284183\n-0.585181 0.8096 0.0459524 -381.9 -0.809679 -0.586471 0.0217117 267.072 0.0445275 -0.0245014 0.998708 -0.305523\n-0.578539 0.814555 0.0423531 -380.932 -0.814556 -0.579674 0.0218207 268.467 0.0423251 -0.0218749 0.998864 -0.349063\n-0.567965 0.821724 0.0467347 -379.45 -0.821672 -0.569385 0.0255998 270.588 0.047646 -0.0238608 0.998579 -0.419259\n-0.560677 0.826533 0.0498497 -378.474 -0.826713 -0.562167 0.0226696 272.023 0.046761 -0.0285011 0.998499 -0.458059\n-0.553308 0.831411 0.0510542 -377.489 -0.831956 -0.554624 0.0155217 273.476 0.0412207 -0.0338866 0.998575 -0.495777\n-0.544571 0.836992 0.0537376 -376.515 -0.837815 -0.545835 0.0113529 274.963 0.0388342 -0.0388397 0.998491 -0.527125\n-0.53558 0.842338 0.0601675 -375.547 -0.843451 -0.537089 0.0112328 276.479 0.0417772 -0.0447323 0.998125 -0.554445\n-0.526705 0.848013 0.0587872 -374.592 -0.84914 -0.528079 0.00972043 278.015 0.0392873 -0.0447988 0.998223 -0.570697\n-0.516607 0.854268 0.0578254 -373.642 -0.855468 -0.517808 0.00701942 279.579 0.0359389 -0.0458415 0.998302 -0.592555\n-0.506291 0.860296 0.0596748 -372.707 -0.861293 -0.507896 0.0146768 281.179 0.042935 -0.0439668 0.99811 -0.62216\n-0.49617 0.866337 0.0572348 -371.776 -0.867038 -0.49786 0.0195008 282.807 0.0453892 -0.039949 0.99817 -0.64604\n-0.486433 0.872385 0.0482377 -370.9 -0.872691 -0.487796 0.0215635 284.444 0.0423418 -0.0316074 0.998603 -0.669482\n-0.47687 0.878213 0.0365684 -370.022 -0.878169 -0.477801 0.0229292 286.097 0.0376092 -0.021179 0.999068 -0.697328\n-0.468525 0.882899 0.031205 -369.152 -0.88276 -0.469263 0.022966 287.747 0.03492 -0.0167864 0.999249 -0.732924\n-0.460194 0.887212 0.0328235 -368.289 -0.887095 -0.460997 0.0233373 289.395 0.0358367 -0.0183779 0.999189 -0.760591\n-0.451816 0.891417 0.0351759 -367.44 -0.891326 -0.452721 0.0241105 291.043 0.0374174 -0.0204597 0.99909 -0.776225\n-0.441572 0.896537 0.0351388 -366.206 -0.896494 -0.442455 0.0230598 293.538 0.0362213 -0.0213192 0.999116 -0.813287\n-0.434754 0.899638 0.0404956 -365.392 -0.899689 -0.435865 0.0241122 295.209 0.0393429 -0.0259505 0.998889 -0.84067\n-0.428633 0.90221 0.0478592 -364.578 -0.902503 -0.43003 0.0237147 296.882 0.0419766 -0.0330282 0.998573 -0.870144\n-0.422948 0.904628 0.0525757 -363.772 -0.905256 -0.424399 0.0199142 298.567 0.040328 -0.0391718 0.998418 -0.882033\n-0.41445 0.908682 0.050286 -362.599 -0.909338 -0.415702 0.0172308 301.132 0.0365613 -0.0385857 0.998586 -0.897095\n-0.40849 0.911843 0.0409562 -361.845 -0.912224 -0.409379 0.0160063 302.849 0.0313619 -0.0308228 0.999033 -0.912032\n-0.402417 0.914746 0.036054 -361.108 -0.914827 -0.403289 0.0212021 304.573 0.0339347 -0.0244511 0.999125 -0.920053\n-0.396029 0.917642 0.0330652 -360.356 -0.917812 -0.396685 0.0161789 306.293 0.027963 -0.0239403 0.999322 -0.92887\n-0.388535 0.920781 0.0346887 -359.635 -0.921097 -0.389136 0.0123992 308.017 0.0249156 -0.0271341 0.999321 -0.931067\n-0.37579 0.925341 0.0502552 -358.533 -0.925914 -0.377157 0.0208707 310.634 0.0382666 -0.038689 0.998518 -0.955472\n-0.368345 0.92853 0.0464249 -357.863 -0.929018 -0.369516 0.0195581 312.367 0.035315 -0.0359254 0.99873 -0.937355\n-0.359153 0.932293 0.0428771 -357.21 -0.932776 -0.360088 0.0163012 314.094 0.030637 -0.0341401 0.998947 -0.958341\n-0.349548 0.935768 0.046418 -356.55 -0.93597 -0.350994 0.0276325 315.852 0.04215 -0.0337869 0.99854 -0.987545\n-0.339905 0.939508 0.0422987 -355.928 -0.939416 -0.341303 0.0317861 317.586 0.0443 -0.0289318 0.998599 -0.992567\n-0.329435 0.943425 0.0376987 -355.33 -0.943913 -0.330026 0.0105235 319.295 0.0223696 -0.0321175 0.999234 -0.988485\n-0.317248 0.947577 0.038107 -354.757 -0.948132 -0.317768 0.00829994 321.037 0.019974 -0.0334974 0.999239 -1.01215\n-0.303609 0.95181 0.0433475 -354.19 -0.95235 -0.304545 0.0167799 322.783 0.0291725 -0.0361875 0.998919 -1.00493\n-0.2894 0.956126 0.0454992 -353.657 -0.95691 -0.290169 0.011179 324.509 0.023891 -0.0403034 0.998902 -1.00803\n-0.273643 0.960612 0.0484202 -353.175 -0.961627 -0.274275 0.00681386 326.24 0.0198259 -0.0446976 0.998804 -0.992537\n-0.25495 0.965758 0.0480858 -352.721 -0.966767 -0.255565 0.00698918 327.984 0.0190389 -0.0447059 0.998819 -0.972247\n-0.23583 0.970653 0.0470904 -352.307 -0.971622 -0.236423 0.00736921 329.733 0.0182862 -0.0440162 0.998863 -0.954089\n-0.216619 0.975175 0.0459285 -351.922 -0.976028 -0.217346 0.0114215 331.485 0.0211203 -0.0423534 0.998879 -0.948657\n-0.197775 0.979354 0.0418513 -351.583 -0.980126 -0.198242 0.00727678 333.23 0.0154232 -0.0395804 0.999097 -0.906153\n-0.178882 0.98309 0.0391733 -351.265 -0.983819 -0.179138 0.00307375 334.988 0.0100392 -0.0379896 0.999228 -0.904351\n-0.151067 0.987971 0.0330459 -350.859 -0.988322 -0.151626 0.0151206 337.654 0.0199493 -0.0303758 0.999339 -0.873637\n-0.132466 0.990674 0.0319007 -350.628 -0.990966 -0.133048 0.0168749 339.435 0.0209618 -0.0293772 0.999349 -0.861268\n-0.1155 0.992918 0.0278248 -350.434 -0.993078 -0.11603 0.0182454 341.212 0.0213447 -0.0255249 0.999446 -0.836825\n-0.0930162 0.9954 0.0229528 -350.177 -0.995568 -0.093303 0.0117576 343.87 0.0138451 -0.0217575 0.999667 -0.835718\n-0.0805756 0.996601 0.0171604 -350.047 -0.996597 -0.0808512 0.0160171 345.652 0.0173501 -0.0158115 0.999724 -0.798283\n-0.0700092 0.997454 0.0135607 -349.927 -0.99736 -0.0702526 0.0183848 347.442 0.0192907 -0.0122378 0.999739 -0.808376\n-0.0615046 0.99797 0.0165156 -349.81 -0.997787 -0.0618952 0.0242793 349.208 0.0252523 -0.0149858 0.999569 -0.812667\n-0.0521975 0.998473 0.0180996 -349.669 -0.998534 -0.0524441 0.0134277 351.835 0.0143564 -0.0173722 0.999746 -0.783267\n-0.0463562 0.998792 0.0162801 -349.576 -0.998888 -0.0464887 0.00785702 353.609 0.00860438 -0.0158977 0.999837 -0.754757\n-0.0424931 0.999029 0.011644 -349.505 -0.99897 -0.0426707 0.0154589 355.403 0.0159408 -0.0109751 0.999813 -0.719976\n-0.0428406 0.999057 0.0070566 -349.452 -0.998986 -0.0429334 0.0135592 357.155 0.0138494 -0.00646856 0.999883 -0.679541\n-0.0442934 0.999001 0.00587795 -349.368 -0.998984 -0.0442424 -0.00853503 358.93 -0.00826645 -0.00625002 0.999946 -0.662682\n-0.0459543 0.998937 0.00349906 -349.268 -0.998902 -0.0459842 0.00901044 360.735 0.00916176 -0.00308115 0.999953 -0.646041\n-0.0488111 0.998784 0.00695201 -349.18 -0.998525 -0.0489616 0.0234492 362.508 0.0237611 -0.00579718 0.999701 -0.627383\n-0.0524131 0.998587 0.00876308 -349.074 -0.99855 -0.0525149 0.0118112 364.271 0.0122547 -0.00813131 0.999892 -0.580001\n-0.055241 0.998432 0.00905325 -348.974 -0.998439 -0.0553122 0.00781478 366.06 0.00830328 -0.00860742 0.999928 -0.587383\n-0.0581307 0.998263 0.00957986 -348.855 -0.998141 -0.0582942 0.0177761 367.86 0.0183036 -0.00852872 0.999796 -0.559615\n-0.0612418 0.998054 0.0117189 -348.738 -0.997994 -0.0614184 0.0153529 369.638 0.0160427 -0.0107552 0.999813 -0.539383\n-0.0638461 0.997829 0.0161592 -348.601 -0.997888 -0.0640274 0.0109673 371.416 0.0119781 -0.0154248 0.999809 -0.537217\n-0.0670001 0.997513 0.0218882 -348.471 -0.997687 -0.0672319 0.0100313 373.194 0.011478 -0.0211655 0.99971 -0.506347\n-0.0710637 0.997162 0.0248712 -348.34 -0.997435 -0.071254 0.00684666 374.972 0.0085994 -0.0243208 0.999667 -0.476802\n-0.074526 0.996757 0.0303462 -348.189 -0.997179 -0.0747626 0.00673768 376.75 0.00898459 -0.0297584 0.999517 -0.441768\n-0.0784334 0.996365 0.0332484 -348.038 -0.996844 -0.0787945 0.0096931 378.521 0.0122776 -0.0323832 0.9994 -0.403939\n-0.0844133 0.99598 0.0299827 -347.886 -0.996385 -0.0846599 0.00705041 380.27 0.0095604 -0.0292791 0.999526 -0.358141\n-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\n-0.0948267 0.995173 0.0252598 -347.567 -0.995469 -0.0949722 0.00462249 383.735 0.00699916 -0.024707 0.99967 -0.296894\n-0.100077 0.994707 0.0232932 -347.397 -0.994829 -0.100441 0.0150506 385.462 0.0173106 -0.0216666 0.999615 -0.257571\n-0.105773 0.994121 0.0231518 -347.211 -0.994259 -0.106109 0.0137976 387.179 0.0161731 -0.0215594 0.999637 -0.239488\n-0.111334 0.993533 0.0222912 -347.02 -0.993689 -0.111604 0.0112563 388.871 0.0136713 -0.0208974 0.999688 -0.225331\n-0.116555 0.993036 0.017166 -346.817 -0.993093 -0.116761 0.0115671 390.557 0.0134909 -0.0156992 0.999786 -0.195224\n-0.122169 0.992436 0.0120853 -346.616 -0.992501 -0.122209 0.00255832 392.217 0.00401589 -0.0116821 0.999924 -0.144419\n-0.129523 0.991557 0.0062472 -346.403 -0.991576 -0.129528 0.000355834 393.884 0.00116202 -0.00614849 0.99998 -0.116123\n-0.138456 0.990337 0.007875 -346.168 -0.990342 -0.138507 0.00626522 395.535 0.00729542 -0.00693149 0.999949 -0.0753762\n-0.150744 0.988543 0.00765698 -345.91 -0.988458 -0.15084 0.0140528 397.164 0.0150468 -0.00545023 0.999872 -0.0410686\n-0.164908 0.986283 0.00715092 -345.63 -0.986079 -0.165022 0.0204149 398.784 0.0213149 -0.00368479 0.999766 -0.0505591\n-0.180668 0.98346 0.0128496 -345.304 -0.983381 -0.180861 0.015845 400.366 0.017907 -0.00977339 0.999792 -0.0083421\n-0.199897 0.979764 0.0101481 -344.977 -0.979799 -0.199946 0.00403973 401.926 0.00598706 -0.00913557 0.99994 0.0188796\n-0.219519 0.97546 0.0170062 -344.607 -0.975493 -0.219727 0.0115141 403.471 0.0149683 -0.0140619 0.999789 0.0360259\n-0.240719 0.970435 0.0176185 -344.206 -0.970302 -0.241052 0.0201891 404.994 0.0238392 -0.0122353 0.999641 0.0636261\n-0.262779 0.964667 0.0191172 -343.781 -0.96466 -0.263073 0.0149626 406.456 0.0194631 -0.0145097 0.999705 0.0666647\n-0.28627 0.957753 0.0275279 -343.331 -0.957991 -0.286626 0.0098973 407.865 0.0173694 -0.0235382 0.999572 0.0950709\n-0.312831 0.949304 0.0309573 -342.855 -0.949529 -0.313364 0.0140563 409.241 0.0230446 -0.0249976 0.999422 0.112315\n-0.341198 0.939341 0.0349811 -342.346 -0.939649 -0.341842 0.0142875 410.583 0.0253788 -0.0279951 0.999286 0.120448\n-0.372355 0.927428 0.0350607 -341.787 -0.927719 -0.373009 0.0142166 411.891 0.0262629 -0.0272328 0.999284 0.124275\n-0.406676 0.913007 0.0321269 -341.193 -0.913277 -0.407187 0.0111328 413.161 0.023246 -0.0248134 0.999422 0.134025\n-0.44118 0.896981 0.0280309 -340.555 -0.897254 -0.441482 0.00534777 414.395 0.017172 -0.0227915 0.999593 0.142282\n-0.473346 0.880058 0.0379659 -339.858 -0.880453 -0.474016 0.0106003 415.606 0.0273253 -0.0284095 0.999223 0.109593\n-0.502983 0.863463 0.0379472 -339.156 -0.863945 -0.503546 0.00641097 416.773 0.0246438 -0.0295597 0.999259 0.172094\n-0.533144 0.84574 0.02192 -338.44 -0.845979 -0.533206 -0.00341887 417.897 0.00879637 -0.0203666 0.999754 0.159843\n-0.558772 0.828424 0.0385834 -337.648 -0.828922 -0.559342 0.00502204 418.961 0.0257417 -0.0291764 0.999243 0.149898\n-0.586677 0.808517 0.0459398 -336.863 -0.808763 -0.587866 0.0177792 420.006 0.0413812 -0.0267237 0.998786 0.160053\n-0.612689 0.788967 0.0462995 -336.064 -0.789366 -0.613779 0.0132887 420.975 0.038902 -0.0284054 0.998839 0.134641\n-0.63791 0.768607 0.0481037 -335.262 -0.769298 -0.638861 0.00603272 421.875 0.0353683 -0.0331577 0.998824 0.145366\n-0.662972 0.747375 0.0435629 -334.484 -0.747964 -0.663728 0.00400838 422.719 0.0319097 -0.029926 0.999043 0.155122\n-0.687857 0.724828 0.0384213 -333.684 -0.725315 -0.688415 0.00181009 423.507 0.0277618 -0.0266224 0.99926 0.14541\n-0.711432 0.701586 0.0405106 -332.879 -0.702178 -0.712001 -0.000541689 424.244 0.0284636 -0.028831 0.999179 0.137707\n-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\n-0.757304 0.652077 0.0358662 -331.3 -0.652381 -0.75788 0.00406079 425.587 0.0298303 -0.0203232 0.999348 0.159251\n-0.779048 0.62661 0.0210951 -330.496 -0.626642 -0.779285 0.00585394 426.221 0.0201073 -0.00865861 0.99976 0.158773\n-0.809938 0.586172 0.0200698 -329.239 -0.586498 -0.809699 -0.0201731 427.016 0.00442559 -0.0281099 0.999595 0.142595\n-0.830036 0.556357 0.0388153 -328.382 -0.556701 -0.830709 0.00228733 427.56 0.0335168 -0.01971 0.999244 0.126505\n-0.845829 0.529576 0.0642101 -327.487 -0.528697 -0.84823 0.0313817 428.098 0.0710839 -0.00740418 0.997443 0.0907812\n-0.875608 0.480826 0.0460174 -326.209 -0.481399 -0.8765 -0.00160118 428.736 0.0395643 -0.0235547 0.998939 0.126249\n-0.90444 0.425912 0.0242256 -324.862 -0.426076 -0.904686 -0.00178445 429.32 0.0211565 -0.0119359 0.999705 0.0789445\n-0.920412 0.388946 0.0395235 -323.913 -0.389343 -0.921089 -0.0025847 429.67 0.0353993 -0.0177672 0.999215 0.0698541\n-0.936147 0.34957 0.037812 -322.96 -0.34997 -0.936751 -0.00432135 429.977 0.0339098 -0.0172785 0.999276 0.071688\n-0.950207 0.309802 0.0336079 -321.993 -0.310048 -0.950718 -0.00222164 430.26 0.0312633 -0.0125311 0.999433 0.0567386\n-0.962047 0.270439 0.0364497 -320.994 -0.270726 -0.962652 -0.00307726 430.493 0.0342561 -0.0128283 0.999331 0.0420548\n-0.976895 0.210498 0.036967 -319.462 -0.210724 -0.977543 -0.00227099 430.773 0.0356588 -0.0100083 0.999314 0.0237979\n-0.984742 0.17036 0.0355121 -318.426 -0.170614 -0.985329 -0.00420825 430.911 0.0342742 -0.0102029 0.99936 0.0046861\n-0.990648 0.131476 0.0364833 -317.353 -0.131848 -0.991238 -0.00796762 430.999 0.0351161 -0.0127034 0.999302 -0.0146898\n-0.994488 0.0975815 0.0383485 -316.256 -0.0980373 -0.995131 -0.0101875 431.056 0.0371676 -0.0138909 0.999212 -0.0302254\n-0.996728 0.0710279 0.0385823 -315.147 -0.0715872 -0.997346 -0.0133118 431.103 0.0375344 -0.0160302 0.999167 -0.0454692\n-0.997945 0.0507262 0.0391507 -314.019 -0.0513283 -0.998576 -0.0145285 431.139 0.038358 -0.0165082 0.999128 -0.0608638\n-0.998555 0.0348222 0.0409338 -312.88 -0.0353317 -0.999306 -0.0117901 431.166 0.0404949 -0.0132193 0.999092 -0.0807247\n-0.998907 0.0212359 0.0416467 -311.716 -0.0217137 -0.999703 -0.0110535 431.161 0.0413996 -0.0119457 0.999071 -0.102687\n-0.999071 0.0111145 0.0416427 -310.541 -0.0115606 -0.999878 -0.0104868 431.157 0.041521 -0.0109585 0.999078 -0.11728\n-0.999146 0.00244649 0.0412478 -309.35 -0.00275946 -0.999968 -0.00753221 431.156 0.0412281 -0.0076396 0.999121 -0.14047\n-0.999105 -0.0049882 0.0420038 -308.132 0.00470842 -0.999966 -0.00675733 431.144 0.0420361 -0.00655351 0.999095 -0.171242\n-0.999046 -0.0108513 0.0423078 -306.899 0.0104308 -0.999894 -0.010147 431.115 0.0424135 -0.00969597 0.999053 -0.200525\n-0.999087 -0.0128055 0.0407598 -305.647 0.0122494 -0.999829 -0.0138637 431.077 0.0409303 -0.0133518 0.999073 -0.222958\n-0.999005 -0.0124202 0.0428299 -304.379 0.0117537 -0.999806 -0.0157775 431.044 0.0430175 -0.0152584 0.998958 -0.24983\n-0.999021 -0.0113674 0.0427614 -303.087 0.0105607 -0.999763 -0.0190429 431.025 0.0429677 -0.0185727 0.998904 -0.275778\n-0.99912 -0.010292 0.0406641 -301.787 0.00939436 -0.999709 -0.0222031 431.002 0.0408808 -0.0218016 0.998926 -0.297794\n-0.999202 -0.00902526 0.0388983 -300.474 0.00816095 -0.999718 -0.0223215 430.984 0.0390888 -0.0219863 0.998994 -0.324869\n-0.999286 -0.00775134 0.0369886 -299.145 0.00688998 -0.999703 -0.0233581 430.966 0.0371587 -0.0230866 0.999043 -0.331148\n-0.999372 -0.00654804 0.0348178 -297.805 0.00574233 -0.999715 -0.0231905 430.943 0.0349597 -0.0229761 0.999125 -0.353847\n-0.999367 -0.00517746 0.0351906 -296.444 0.00427529 -0.999661 -0.0256636 430.932 0.0353115 -0.0254969 0.999051 -0.379715\n-0.999365 -0.0042669 0.0353759 -295.069 0.00326804 -0.999596 -0.0282456 430.916 0.0354821 -0.028112 0.998975 -0.394735\n-0.999331 -0.00372996 0.0363711 -293.682 0.00302983 -0.999809 -0.0192856 430.92 0.0364361 -0.0191625 0.999152 -0.406463\n-0.999224 -0.0034038 0.0392502 -292.274 0.00210888 -0.999454 -0.0329857 430.891 0.039341 -0.0328773 0.998685 -0.44904\n-0.999075 -0.00350863 0.0428511 -290.853 0.00258418 -0.999763 -0.0216098 430.911 0.0429168 -0.0214791 0.998848 -0.416859\n-0.999226 -0.00225592 0.0392823 -289.427 0.00154617 -0.999835 -0.018089 430.911 0.0393167 -0.0180143 0.999064 -0.487909\n-0.999394 -0.00211479 0.0347494 -287.968 0.00131897 -0.999737 -0.0229084 430.909 0.0347887 -0.0228487 0.999133 -0.493599\n-0.99947 -0.00042876 0.0325601 -286.498 -0.000181173 -0.999825 -0.0187273 430.916 0.0325624 -0.0187232 0.999294 -0.507028\n-0.99935 0.00112731 0.0360399 -285.004 -0.0018199 -0.999814 -0.0191902 430.913 0.0360115 -0.0192433 0.999166 -0.541689\n-0.999277 0.00171911 0.0379709 -283.512 -0.002403 -0.999836 -0.0179725 430.924 0.0379338 -0.0180507 0.999117 -0.539905\n-0.999382 0.00266823 0.0350529 -282.004 -0.00316074 -0.999897 -0.0140027 430.928 0.0350119 -0.0141049 0.999287 -0.561295\n-0.999398 0.00307071 0.034571 -280.484 -0.00351617 -0.999911 -0.0128318 430.927 0.0345285 -0.0129457 0.99932 -0.584433\n-0.999386 0.00326409 0.0348931 -278.949 -0.00379458 -0.999878 -0.0151479 430.914 0.0348394 -0.015271 0.999276 -0.604216\n-0.999395 0.00278964 0.0346604 -277.398 -0.00337562 -0.999852 -0.0168594 430.909 0.0346082 -0.0169662 0.999257 -0.621408\n-0.999356 0.00131763 0.0358671 -275.841 -0.00194189 -0.999847 -0.0173757 430.909 0.0358387 -0.0174342 0.999206 -0.647272\n-0.999284 -0.00205375 0.0377716 -274.26 0.00135508 -0.999828 -0.0185135 430.894 0.0378031 -0.0184491 0.999115 -0.668364\n-0.999317 -0.00520668 0.0365776 -272.666 0.00447704 -0.99979 -0.0200014 430.879 0.0366741 -0.0198239 0.999131 -0.679403\n-0.999271 -0.00800821 0.037316 -271.046 0.00729443 -0.999789 -0.0192251 430.859 0.0374621 -0.0189388 0.999119 -0.698381\n-0.999242 -0.0103811 0.0375154 -269.413 0.00969885 -0.999785 -0.0183235 430.837 0.0376976 -0.0179458 0.999128 -0.726739\n-0.999229 -0.0123339 0.0372649 -267.75 0.0117167 -0.999791 -0.0167358 430.827 0.0374635 -0.0162863 0.999165 -0.755608\n-0.999199 -0.0134724 0.0376791 -266.076 0.012844 -0.999775 -0.0168684 430.797 0.0378979 -0.016371 0.999148 -0.784262\n-0.999134 -0.0141357 0.0391431 -264.4 0.0135145 -0.999779 -0.0160893 430.765 0.0393619 -0.0155464 0.999104 -0.807342\n-0.999173 -0.0144565 0.0379923 -262.698 0.0138817 -0.999786 -0.015351 430.744 0.0382061 -0.0148109 0.99916 -0.830716\n-0.999145 -0.0139514 0.0389144 -261.001 0.0133544 -0.99979 -0.0155589 430.714 0.0391232 -0.0150259 0.999121 -0.859128\n-0.999043 -0.0137537 0.0415144 -259.288 0.0131225 -0.999795 -0.0154395 430.692 0.0417182 -0.01488 0.999019 -0.889086\n-0.999063 -0.0131961 0.0412264 -257.577 0.0126119 -0.999817 -0.0143988 430.66 0.0414088 -0.0138653 0.999046 -0.914857\n-0.999084 -0.0126144 0.040895 -255.869 0.0120381 -0.999825 -0.0143078 430.634 0.0410683 -0.0138024 0.999061 -0.929993\n-0.99909 -0.0127524 0.0406961 -254.143 0.0122583 -0.999848 -0.0123693 430.629 0.0408476 -0.0118592 0.999095 -0.956383\n-0.999101 -0.013217 0.0402736 -252.455 0.012798 -0.999861 -0.0106458 430.614 0.0404087 -0.0101208 0.999132 -0.972722\n-0.999172 -0.0134919 0.0383866 -250.766 0.0131155 -0.999864 -0.0100383 430.602 0.0385168 -0.00952653 0.999213 -0.98212\n-0.9991 -0.0134791 0.0402202 -249.109 0.013007 -0.999844 -0.0119771 430.565 0.0403754 -0.0114431 0.999119 -1.00942\n-0.998983 -0.0135921 0.0429998 -247.458 0.0128518 -0.999765 -0.0174441 430.53 0.0432268 -0.0168737 0.998923 -1.02073\n-0.999102 -0.0104609 0.0410626 -245.819 0.00975033 -0.9998 -0.0174674 430.501 0.0412371 -0.0170514 0.999004 -1.04537\n-0.999245 -0.00670456 0.0382664 -244.177 0.00593422 -0.999778 -0.0202093 430.487 0.0383934 -0.0199669 0.999063 -1.06093\n-0.99934 -0.00462712 0.036023 -242.56 0.00388515 -0.999779 -0.0206401 430.485 0.0361106 -0.0204866 0.999138 -1.07239\n-0.999288 -0.00219843 0.0376562 -240.948 0.00136499 -0.999754 -0.0221444 430.473 0.0376956 -0.0220772 0.999045 -1.08809\n-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\n-0.999298 0.00148195 0.0374396 -237.72 -0.00231694 -0.999749 -0.0222689 430.477 0.0373972 -0.02234 0.999051 -1.11612\n-0.999262 0.00288861 0.0383016 -236.096 -0.00361088 -0.999817 -0.0188018 430.483 0.0382403 -0.0189262 0.999089 -1.12991\n-0.999271 0.00331631 0.0380302 -234.477 -0.00408659 -0.999788 -0.0201946 430.477 0.0379552 -0.0203352 0.999073 -1.16173\n-0.998792 0.00236644 0.0490878 -232.847 -0.00325966 -0.99983 -0.0181243 430.477 0.0490365 -0.0182625 0.99863 -1.20535\n-0.999022 0.00114326 0.0441947 -231.236 -0.00197844 -0.99982 -0.0188587 430.479 0.0441652 -0.0189277 0.998845 -1.21487\n-0.999315 0.000877912 0.0370059 -229.607 -0.00159821 -0.99981 -0.0194392 430.474 0.0369818 -0.019485 0.999126 -1.25282\n-0.999221 -0.000246634 0.0394525 -227.15 -0.000514568 -0.999814 -0.0192828 430.474 0.0394499 -0.0192881 0.999035 -1.28351\n-0.999263 -0.00110355 0.0383654 -225.512 0.000345826 -0.999805 -0.0197513 430.461 0.0383797 -0.0197235 0.999069 -1.30714\n-0.999315 -0.00212632 0.0369534 -223.875 0.0013777 -0.999794 -0.0202723 430.448 0.0369889 -0.0202075 0.999111 -1.33025\n-0.999246 -0.00293043 0.0387051 -222.25 0.0021009 -0.999768 -0.0214555 430.437 0.038759 -0.0213581 0.99902 -1.35334\n-0.999206 -0.00378307 0.0396564 -220.641 0.00286703 -0.999728 -0.0231311 430.432 0.0397331 -0.022999 0.998946 -1.37846\n-0.999178 -0.00473944 0.0402689 -219.043 0.00381072 -0.999726 -0.0231087 430.415 0.0403673 -0.0229363 0.998922 -1.3914\n-0.999173 -0.00561397 0.0402735 -217.456 0.00478661 -0.999776 -0.0206106 430.408 0.0403802 -0.0204008 0.998976 -1.40614\n-0.999175 -0.00585849 0.0401915 -215.889 0.00516726 -0.999837 -0.0172806 430.391 0.0402862 -0.0170586 0.999043 -1.42372\n-0.999136 -0.00662346 0.0410217 -214.331 0.0061069 -0.999901 -0.0127048 430.386 0.0411018 -0.0124433 0.999077 -1.45083\n-0.999076 -0.00755708 0.0423028 -212.813 0.00713604 -0.999924 -0.0100952 430.384 0.0423759 -0.00978399 0.999054 -1.47585\n-0.999038 -0.00916108 0.0428792 -211.336 0.00883216 -0.99993 -0.00785408 430.37 0.0429482 -0.00746782 0.999049 -1.49512\n-0.999023 -0.011927 0.0425528 -209.899 0.0116524 -0.99991 -0.00669576 430.351 0.0426288 -0.00619338 0.999072 -1.50974\n-0.999181 -0.0131841 0.0382657 -208.472 0.0129586 -0.999897 -0.00613312 430.321 0.0383426 -0.00563222 0.999249 -1.52714\n-0.999188 -0.0150053 0.0373817 -207.11 0.0146582 -0.999847 -0.00954457 430.295 0.0375192 -0.00898888 0.999255 -1.5436\n-0.999112 -0.0170781 0.0385162 -205.756 0.0166273 -0.99979 -0.011994 430.267 0.0387129 -0.0113429 0.999186 -1.55914\n-0.999031 -0.0182715 0.040036 -204.409 0.0176189 -0.999707 -0.0165933 430.226 0.0403275 -0.0158718 0.99906 -1.57553\n-0.99946 -0.0179477 0.0275164 -203.097 0.0174433 -0.999677 -0.0184634 430.192 0.0278389 -0.0179735 0.999451 -1.54973\n-0.998988 -0.0155635 0.0421994 -201.746 0.0148769 -0.999753 -0.0165359 430.178 0.0424463 -0.0158913 0.998972 -1.61794\n-0.997901 -0.0130741 0.0634222 -200.432 0.0122809 -0.999842 -0.0128814 430.16 0.0635805 -0.0120755 0.997904 -1.62057\n-0.999437 -0.00966285 0.0321296 -199.155 0.00933356 -0.999903 -0.010383 430.155 0.0322268 -0.0100773 0.99943 -1.62041\n-0.999749 -0.00512523 0.0218275 -197.845 0.00501375 -0.999974 -0.00515911 430.157 0.0218534 -0.00504838 0.999748 -1.65345\n-0.999356 -0.00145063 0.0358534 -196.53 0.00148332 -0.999999 0.000885194 430.158 0.035852 0.000937806 0.999357 -1.67273\n-0.999256 0.00228143 0.0384881 -195.216 -0.0021303 -0.99999 0.00396716 430.158 0.0384968 0.00388222 0.999251 -1.67359\n-0.999297 0.00560428 0.0370664 -193.92 -0.00541729 -0.999972 0.00514315 430.172 0.0370941 0.00493873 0.9993 -1.69223\n-0.999229 0.00874424 0.0382789 -192.633 -0.00848234 -0.99994 0.00699903 430.193 0.0383378 0.00666894 0.999243 -1.71109\n-0.999259 0.0119193 0.0365859 -191.369 -0.0117824 -0.999923 0.00395621 430.198 0.0366302 0.00352221 0.999323 -1.72407\n-0.999271 0.0142677 0.0354121 -190.114 -0.014333 -0.999896 -0.00159234 430.212 0.0353857 -0.00209875 0.999372 -1.74332\n-0.999192 0.0158718 0.0369142 -188.872 -0.0160219 -0.999865 -0.00377457 430.227 0.0368493 -0.00436296 0.999311 -1.75467\n-0.999131 0.0157669 0.0385795 -187.643 -0.0160593 -0.999845 -0.00728171 430.237 0.0384587 -0.00789494 0.999229 -1.76888\n-0.999076 0.0154509 0.0401059 -186.428 -0.015979 -0.999789 -0.0128812 430.247 0.0398984 -0.0135101 0.999112 -1.78375\n-0.999028 0.0152475 0.0413664 -185.216 -0.0157825 -0.999796 -0.0126366 430.257 0.0411653 -0.0132772 0.999064 -1.78733\n-0.999032 0.0153613 0.0412247 -184.039 -0.0159189 -0.999786 -0.0132328 430.264 0.0410126 -0.0138763 0.999062 -1.8081\n-0.999008 0.0151173 0.0418919 -182.902 -0.0156489 -0.999801 -0.0123893 430.289 0.0416963 -0.0130326 0.999045 -1.81612\n-0.999008 0.0149472 0.0419465 -181.802 -0.0154477 -0.999813 -0.0116329 430.31 0.0417648 -0.0122694 0.999052 -1.8306\n-0.999051 0.0168012 0.0401746 -180.744 -0.0172994 -0.999777 -0.0120836 430.326 0.0399626 -0.0127672 0.99912 -1.84868\n-0.9992 0.022371 0.0331611 -179.741 -0.0230409 -0.999535 -0.0199575 430.343 0.0326992 -0.0207056 0.999251 -1.84337\n-0.998451 0.0383847 0.0402685 -178.312 -0.038932 -0.999159 -0.0128948 430.435 0.0397397 -0.0144425 0.999106 -1.84461\n-0.996541 0.0722944 0.0409844 -176.989 -0.0730158 -0.997196 -0.0163852 430.531 0.0396849 -0.019321 0.999025 -1.87233\n-0.991829 0.122358 0.0361125 -175.706 -0.122737 -0.992403 -0.00846607 430.764 0.0348023 -0.0128292 0.999312 -1.86075\n-0.982351 0.183358 0.0369591 -174.459 -0.183625 -0.982988 -0.00394147 431.049 0.0356077 -0.0106585 0.999309 -1.89426\n-0.966948 0.252445 0.035809 -173.263 -0.252593 -0.967573 0.000411611 431.452 0.0347517 -0.00864709 0.999359 -1.88862\n-0.942972 0.329771 0.0453354 -172.084 -0.330197 -0.94391 -0.00204647 431.921 0.0421177 -0.0168994 0.99897 -1.88682\n-0.91064 0.41217 0.0291802 -171.006 -0.411688 -0.911078 0.0212208 432.565 0.035332 0.00731133 0.999349 -1.9155\n-0.867042 0.496757 0.0383531 -169.962 -0.496952 -0.867764 0.00494561 433.201 0.0357382 -0.0147716 0.999252 -1.96037\n-0.816581 0.576214 0.034237 -169.037 -0.576033 -0.817271 0.0159291 433.993 0.0371595 -0.00671419 0.999287 -1.92368\n-0.755858 0.654298 0.0239258 -168.197 -0.654304 -0.756182 0.00864673 434.852 0.0237498 -0.00911903 0.999676 -1.94945\n-0.685858 0.727115 0.0300283 -167.418 -0.727365 -0.686241 0.0035592 435.788 0.0231946 -0.0194004 0.999543 -1.97568\n-0.6102 0.791941 0.0220227 -166.773 -0.792026 -0.610451 0.00666025 436.813 0.0187183 -0.0133784 0.999735 -1.92695\n-0.526895 0.849707 0.0194722 -166.204 -0.849846 -0.527027 0.00196156 437.894 0.0119291 -0.0155148 0.999808 -1.95495\n-0.441304 0.897279 0.0119024 -165.75 -0.897269 -0.441408 0.00816746 439.044 0.0125823 -0.00707534 0.999896 -1.89828\n-0.353159 0.935437 0.0153525 -165.393 -0.935541 -0.353216 0.00111156 440.22 0.00646256 -0.0139704 0.999882 -1.92454\n-0.273326 0.961831 0.0131928 -165.153 -0.961875 -0.273422 0.00608558 441.436 0.0094605 -0.0110265 0.999894 -1.86521\n-0.204542 0.978785 0.0119488 -164.967 -0.978776 -0.204668 0.0104578 442.672 0.0126815 -0.00955618 0.999874 -1.88666\n-0.146565 0.989041 0.0178033 -164.823 -0.989101 -0.146783 0.0115992 443.936 0.0140853 -0.0159092 0.999774 -1.84579\n-0.09944 0.99497 0.0121394 -164.711 -0.994922 -0.099611 0.0144006 445.249 0.0155373 -0.0106458 0.999823 -1.85044\n-0.0657515 0.997771 0.0113746 -164.633 -0.997696 -0.0659287 0.015981 446.582 0.0166953 -0.0102976 0.999808 -1.83105\n-0.0452102 0.998926 0.0101574 -164.586 -0.998848 -0.0453656 0.0156281 447.956 0.0160721 -0.00943916 0.999826 -1.83692\n-0.0278983 0.999576 0.00839618 -164.566 -0.999456 -0.0280409 0.0173736 449.396 0.0176017 -0.00790692 0.999814 -1.82163\n-0.0151482 0.999861 0.00701936 -164.562 -0.999745 -0.0152634 0.0166598 450.868 0.0167646 -0.0067652 0.999837 -1.81987\n-0.00758712 0.999947 0.00701057 -164.547 -0.999845 -0.00769738 0.0158372 451.884 0.0158903 -0.00688932 0.99985 -1.8094\n-0.00215685 0.99999 0.00382845 -164.551 -0.999879 -0.00221543 0.0153656 452.901 0.0153739 -0.00379484 0.999875 -1.79401\n-0.00136052 0.999992 0.00364144 -164.548 -0.999903 -0.00141094 0.0138819 453.941 0.0138869 -0.0036222 0.999897 -1.79922\n-0.000102577 0.999983 0.00577111 -164.539 -0.999914 -0.000178095 0.0130866 454.991 0.0130874 -0.00576927 0.999898 -1.79253\n-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\n0.00058304 0.999972 0.00744463 -164.537 -0.999942 0.000502995 0.0107495 457.15 0.0107455 -0.00745047 0.999915 -1.77172\n-0.00202685 0.999953 0.00946149 -164.529 -0.999949 -0.00212031 0.00987802 458.247 0.00989762 -0.00944098 0.999906 -1.75566\n-0.00601665 0.999932 0.00995075 -164.512 -0.999934 -0.00611349 0.00973081 459.341 0.00979098 -0.00989154 0.999903 -1.73992\n-0.0111003 0.999894 0.00945953 -164.493 -0.999896 -0.0111862 0.00907626 460.477 0.00918111 -0.0093578 0.999914 -1.72133\n-0.0161413 0.99983 0.00891889 -164.471 -0.999817 -0.0162313 0.01011 461.629 0.0102531 -0.00875407 0.999909 -1.71669\n-0.0208243 0.999747 0.00848119 -164.444 -0.999717 -0.0209195 0.0113045 462.787 0.011479 -0.00824339 0.9999 -1.7\n-0.028147 0.999568 0.00841039 -164.39 -0.999547 -0.0282338 0.0103919 464.553 0.0106249 -0.00811408 0.999911 -1.69154\n-0.0307454 0.999431 0.0138551 -164.331 -0.999451 -0.0309111 0.011904 465.754 0.0123255 -0.0134815 0.999833 -1.66764\n-0.0337862 0.999333 0.0138769 -164.282 -0.999346 -0.0339596 0.0124567 466.987 0.0129196 -0.0134469 0.999826 -1.64905\n-0.0343704 0.999237 0.0185595 -164.226 -0.99933 -0.0345951 0.0119224 468.204 0.0125553 -0.0181373 0.999757 -1.64251\n-0.034352 0.999145 0.0229907 -164.18 -0.999314 -0.0346582 0.0130555 469.434 0.0138411 -0.0225264 0.99965 -1.63046\n-0.0337918 0.999102 0.0255583 -164.113 -0.999347 -0.0341044 0.0118985 470.699 0.0127595 -0.0251396 0.999603 -1.62707\n-0.0311145 0.999032 0.0310919 -164.068 -0.999438 -0.0314853 0.0115066 471.96 0.0124744 -0.0307164 0.99945 -1.61113\n-0.02943 0.999107 0.0303117 -164.031 -0.999466 -0.0298437 0.0132844 473.245 0.0141771 -0.0299046 0.999452 -1.60167\n-0.0263805 0.999105 0.0330791 -163.975 -0.999499 -0.0269409 0.0166094 475.193 0.0174857 -0.0326243 0.999315 -1.58132\n-0.024151 0.999145 0.0335568 -163.95 -0.999548 -0.0247347 0.0170905 476.49 0.0179059 -0.0331289 0.999291 -1.5786\n-0.0217338 0.999098 0.036474 -163.913 -0.999626 -0.0223221 0.0158018 477.823 0.0166017 -0.0361169 0.99921 -1.57082\n-0.0194755 0.999013 0.039918 -163.876 -0.999684 -0.0200932 0.015131 479.164 0.0159181 -0.0396107 0.999088 -1.55911\n-0.0178224 0.998978 0.0415424 -163.843 -0.999728 -0.0184291 0.0142687 480.523 0.0150197 -0.0412768 0.999035 -1.54484\n-0.015078 0.998944 0.043398 -163.792 -0.999787 -0.015675 0.013448 481.896 0.014114 -0.0431859 0.998967 -1.53141\n-0.0128589 0.999037 0.0419432 -163.789 -0.999806 -0.0134732 0.0143951 483.292 0.0149464 -0.0417499 0.999016 -1.51105\n-0.0104578 0.99918 0.0391216 -163.746 -0.999828 -0.0110491 0.0149285 485.387 0.0153485 -0.0389588 0.999123 -1.48696\n-0.0103144 0.999365 0.0340928 -163.737 -0.999867 -0.0107385 0.0122775 486.789 0.0126359 -0.0339617 0.999343 -1.47918\n-0.0099145 0.999387 0.0335746 -163.724 -0.99988 -0.0103085 0.0115814 488.198 0.0119204 -0.0334557 0.999369 -1.46397\n-0.0103382 0.999461 0.0311431 -163.705 -0.999864 -0.0107316 0.0124926 489.646 0.0128201 -0.0310097 0.999437 -1.45353\n-0.0102563 0.999418 0.0325429 -163.683 -0.999853 -0.0106974 0.0134115 491.079 0.0137518 -0.0324006 0.99938 -1.43862\n-0.0118119 0.999456 0.0307945 -163.663 -0.999865 -0.0121577 0.011068 492.517 0.0114364 -0.0306596 0.999464 -1.42073\n-0.0130865 0.999382 0.0326373 -163.619 -0.999821 -0.0135236 0.013206 494.699 0.0136392 -0.0324586 0.99938 -1.4082\n-0.0147667 0.999304 0.0342581 -163.59 -0.999822 -0.0151594 0.0112312 496.163 0.0117427 -0.0340861 0.99935 -1.38063\n-0.0162095 0.999289 0.0340401 -163.537 -0.999812 -0.0165602 0.0100475 498.37 0.010604 -0.0338708 0.99937 -1.35953\n-0.0175478 0.999223 0.0352774 -163.507 -0.99978 -0.0179419 0.0108861 499.855 0.0115106 -0.0350786 0.999318 -1.34464\n-0.0185229 0.999161 0.0365399 -163.459 -0.999768 -0.0189109 0.0103014 501.338 0.0109838 -0.0363406 0.999279 -1.32819\n-0.0208667 0.998986 0.0399037 -163.391 -0.999692 -0.0213854 0.0126152 503.562 0.0134557 -0.0396282 0.999124 -1.29839\n-0.0223043 0.998871 0.0419348 -163.351 -0.999641 -0.0229042 0.0138807 505.022 0.0148255 -0.0416101 0.999024 -1.27352\n-0.023753 0.998783 0.0432326 -163.299 -0.999611 -0.0243591 0.0135463 506.477 0.0145829 -0.0428941 0.998973 -1.25512\n-0.0251474 0.998628 0.0459262 -163.252 -0.999599 -0.0257183 0.0118831 507.904 0.0130479 -0.0456089 0.998874 -1.24414\n-0.0249116 0.998453 0.0496994 -163.2 -0.999621 -0.0254623 0.0104786 509.336 0.0117279 -0.0494195 0.998709 -1.21924\n-0.0243103 0.998407 0.0509129 -163.164 -0.999632 -0.0248912 0.0108054 510.758 0.0120555 -0.0506315 0.998645 -1.19673\n-0.0227028 0.99842 0.051405 -163.117 -0.999649 -0.0233726 0.0124665 512.193 0.0136483 -0.0511039 0.9986 -1.17901\n-0.02156 0.998568 0.048961 -163.05 -0.99965 -0.0222817 0.0142425 514.352 0.015313 -0.0486368 0.998699 -1.14808\n-0.0195718 0.998725 0.0465321 -163.023 -0.999711 -0.020199 0.0130457 515.787 0.013969 -0.0462633 0.998832 -1.14014\n-0.0176031 0.998748 0.046817 -162.988 -0.999783 -0.0181045 0.0103055 517.229 0.0111402 -0.0466255 0.99885 -1.12004\n-0.0138764 0.998965 0.0433173 -162.966 -0.999838 -0.0143584 0.0108364 518.667 0.0114472 -0.0431599 0.999003 -1.09223\n-0.00809382 0.999071 0.0423212 -162.962 -0.999886 -0.00862516 0.0123874 520.114 0.0127409 -0.0422161 0.999027 -1.09692\n-0.000846654 0.999141 0.0414253 -162.954 -0.999922 -0.00136129 0.0123967 521.568 0.0124424 -0.0414116 0.999065 -1.07576\n0.00577562 0.9992 0.0395677 -162.967 -0.999939 0.00539626 0.00968774 522.997 0.00946647 -0.0396212 0.99917 -1.05557\n0.0128067 0.999002 0.0427988 -162.984 -0.999892 0.0124872 0.00772307 524.441 0.00718092 -0.0428931 0.999054 -1.03449\n0.0195367 0.998977 0.040782 -163.019 -0.999683 0.0188713 0.0166387 525.899 0.0158521 -0.0410942 0.99903 -1.01716\n0.0270678 0.998877 0.0388816 -163.056 -0.999512 0.0264374 0.0166369 527.361 0.0155903 -0.039313 0.999105 -0.991882\n0.0330384 0.998749 0.0375407 -163.108 -0.999451 0.0329283 0.00354944 528.807 0.00230885 -0.0376373 0.999289 -0.954153\n0.0391078 0.998662 0.0338413 -163.162 -0.99923 0.038982 0.00437094 530.269 0.00304589 -0.0339862 0.999418 -0.940763\n0.0423357 0.998633 0.0306469 -163.217 -0.999069 0.0420602 0.00957859 531.751 0.00827649 -0.0310239 0.999484 -0.907375\n0.044625 0.998492 0.0319585 -163.282 -0.99899 0.0444334 0.00668118 533.205 0.00525108 -0.0322244 0.999467 -0.889632\n0.0457762 0.998499 0.0300825 -163.375 -0.998945 0.0456439 0.0050719 535.409 0.00369121 -0.030283 0.999535 -0.845481\n0.0435312 0.998606 0.0298522 -163.433 -0.999039 0.0433577 0.00643251 536.877 0.00512922 -0.0301035 0.999534 -0.822243\n0.0400851 0.998672 0.0323625 -163.499 -0.999184 0.0399056 0.00617607 539.116 0.00487642 -0.0325837 0.999457 -0.776642\n0.0375074 0.99877 0.0324318 -163.557 -0.999285 0.0373319 0.00600015 540.591 0.00478203 -0.0326337 0.999456 -0.743114\n0.0340766 0.998932 0.0312135 -163.627 -0.999402 0.0338737 0.00700765 542.817 0.00594285 -0.0314336 0.999488 -0.705475\n0.0302953 0.999096 0.0298233 -163.683 -0.999511 0.0300486 0.00868589 545.058 0.00778189 -0.0300719 0.999517 -0.66842\n0.0277758 0.999128 0.0311775 -163.713 -0.999564 0.0274495 0.0108448 546.554 0.00997949 -0.0314651 0.999455 -0.649963\n0.0255329 0.999134 0.0328665 -163.743 -0.999598 0.0251118 0.0131594 548.03 0.0123227 -0.0331893 0.999373 -0.617393\n0.0217672 0.999217 0.0330415 -163.785 -0.999719 0.0214456 0.0100565 550.227 0.00934 -0.0332511 0.999403 -0.579994\n0.0196377 0.99929 0.0321539 -163.813 -0.999771 0.0193551 0.00907602 551.698 0.00844724 -0.0323248 0.999442 -0.573331\n0.0182908 0.999256 0.0339455 -163.828 -0.999801 0.0180078 0.00862577 553.161 0.00800807 -0.0340965 0.999386 -0.546985\n0.0173645 0.999291 0.0334161 -163.851 -0.999826 0.0171254 0.00742736 554.629 0.00684983 -0.0335393 0.999414 -0.522264\n0.0162681 0.999292 0.0339109 -163.871 -0.999836 0.0159867 0.0085509 556.094 0.00800272 -0.0340444 0.999388 -0.496746\n0.0148403 0.999134 0.0388643 -163.884 -0.999864 0.0145499 0.00774345 558.302 0.00717127 -0.038974 0.999214 -0.460045\n0.0134577 0.999119 0.0397623 -163.895 -0.999897 0.0132464 0.00557324 559.776 0.00504162 -0.0398332 0.999194 -0.429789\n0.0132343 0.999082 0.040743 -163.905 -0.999903 0.0130451 0.00490564 561.253 0.00436964 -0.0408039 0.999158 -0.403\n0.0138689 0.999123 0.0395042 -163.925 -0.999892 0.0136663 0.0053931 562.727 0.00484849 -0.0395747 0.999205 -0.365448\n0.0146433 0.999403 0.0313078 -163.955 -0.999871 0.0144284 0.00708057 564.939 0.00662462 -0.0314075 0.999485 -0.319221\n0.0150563 0.99954 0.0263214 -163.985 -0.99985 0.0148249 0.00896284 566.411 0.00856851 -0.0264524 0.999613 -0.294087\n0.01596 0.999656 0.0208322 -164.015 -0.999823 0.015749 0.0102528 568.62 0.00992118 -0.0209921 0.99973 -0.248733\n0.016373 0.999735 0.0161834 -164.053 -0.999826 0.0162257 0.00919146 570.099 0.00892644 -0.016331 0.999827 -0.242307\n0.0184257 0.999664 0.0182204 -164.076 -0.999788 0.0182536 0.0095652 571.575 0.0092294 -0.0183928 0.999788 -0.2149\n0.0201005 0.999653 0.0170478 -164.106 -0.999747 0.0199236 0.0104826 573.04 0.0101393 -0.0172542 0.9998 -0.199233\n0.022586 0.999612 0.0163268 -164.133 -0.999687 0.0224062 0.0111085 574.516 0.0107384 -0.0165726 0.999805 -0.174215\n0.0249785 0.999557 0.0161676 -164.17 -0.999642 0.0248181 0.0100453 575.984 0.00963959 -0.0164127 0.999819 -0.149534\n0.0265234 0.999543 0.0144934 -164.213 -0.999599 0.0263758 0.0102878 577.434 0.00990079 -0.0147604 0.999842 -0.123136\n0.0288666 0.999514 0.0117643 -164.275 -0.999546 0.0287624 0.0089386 579.578 0.00859589 -0.012017 0.999891 -0.0806227\n0.0299004 0.999512 0.00905644 -164.312 -0.999511 0.0298146 0.00945911 580.992 0.00918448 -0.00933484 0.999914 -0.0500044\n0.031443 0.999495 0.0044861 -164.364 -0.999485 0.0314132 0.00655708 582.396 0.00641285 -0.00468996 0.999968 -0.0166383\n0.0333585 0.99944 0.0027981 -164.416 -0.999426 0.0333411 0.00606831 583.783 0.00597162 -0.00299892 0.999978 0.00941505\n0.0344382 0.999404 -0.0022357 -164.461 -0.999357 0.0344588 0.00994611 585.155 0.0100172 0.00189174 0.999948 0.0403365\n0.036171 0.999329 -0.00578785 -164.518 -0.99929 0.0362292 0.0102989 586.515 0.0105017 0.00541122 0.99993 0.0633299\n0.0377715 0.999249 -0.00861135 -164.572 -0.999249 0.0378434 0.00835003 587.848 0.00866964 0.00828949 0.999928 0.0904724\n0.0391012 0.99914 -0.0137814 -164.634 -0.999199 0.0392131 0.00794773 589.172 0.00848131 0.0134596 0.999873 0.112113\n0.0399143 0.999029 -0.0186539 -164.698 -0.999134 0.0401235 0.0109804 590.484 0.0117182 0.0181994 0.999766 0.136638\n0.0383116 0.99902 -0.0221545 -164.749 -0.999162 0.0386175 0.0135444 591.768 0.0143867 0.0216171 0.999663 0.156641\n0.0346043 0.999127 -0.023409 -164.798 -0.999315 0.0348986 0.0122819 593.057 0.0130882 0.022968 0.999651 0.177445\n0.0310421 0.999209 -0.0248473 -164.842 -0.999447 0.0313265 0.0111404 594.322 0.01191 0.0244877 0.999629 0.20332\n0.0267842 0.999211 -0.0293359 -164.86 -0.999554 0.0271581 0.0124215 595.57 0.0132084 0.0289901 0.999492 0.231375\n0.0158884 0.999324 -0.0331384 -164.886 -0.99978 0.0163314 0.013142 596.803 0.0136743 0.0329223 0.999364 0.241953\n0.000862956 0.999444 -0.0333221 -164.87 -0.999893 0.00134925 0.0145739 597.996 0.0146108 0.033306 0.999338 0.261228\n-0.017546 0.999186 -0.0363214 -164.837 -0.999718 -0.0169516 0.0166081 599.162 0.0159788 0.0366025 0.999202 0.275507\n-0.0429506 0.998298 -0.0394543 -164.769 -0.998947 -0.0422742 0.0178208 600.275 0.0161226 0.0401782 0.999062 0.286057\n-0.0785813 0.996263 -0.0358498 -164.633 -0.996836 -0.0780934 0.0148141 601.36 0.0119591 0.0369005 0.999247 0.298588\n-0.124663 0.991767 -0.0292764 -164.452 -0.992129 -0.12425 0.0155239 602.43 0.0117585 0.0309813 0.999451 0.312911\n-0.178627 0.983661 -0.0224266 -164.209 -0.983825 -0.178252 0.0177537 603.469 0.0134661 0.0252351 0.999591 0.318788\n-0.239809 0.97079 -0.00767748 -163.884 -0.970697 -0.239645 0.0178374 604.479 0.0154765 0.0117301 0.999811 0.337795\n-0.302887 0.952984 0.00902981 -163.473 -0.952901 -0.302987 0.0133448 605.448 0.0154533 -0.00456254 0.99987 0.34853\n-0.36631 0.930322 0.0178249 -163.015 -0.930318 -0.366545 0.012383 606.373 0.0180538 -0.0120468 0.999764 0.377151\n-0.427698 0.903537 0.0263827 -162.532 -0.903542 -0.428182 0.0164942 607.285 0.0261997 -0.0167834 0.999516 0.356572\n-0.485884 0.873311 0.0352861 -161.952 -0.873365 -0.486689 0.0191737 608.139 0.033918 -0.0215015 0.999193 0.382652\n-0.545098 0.838129 0.0202062 -161.372 -0.837929 -0.545435 0.0193669 608.971 0.0272531 -0.00637451 0.999608 0.397828\n-0.600712 0.79941 0.00946209 -160.724 -0.799368 -0.600781 0.00855321 609.738 0.0125222 -0.00242567 0.999919 0.393905\n-0.68004 0.733169 0.00304159 -159.645 -0.733175 -0.680032 -0.00330937 610.788 -0.000357948 -0.00448053 0.99999 0.42769\n-0.726976 0.686661 -0.0013455 -158.881 -0.686634 -0.726926 0.0106377 611.463 0.0063264 0.0086572 0.999943 0.410286\n-0.765018 0.643931 0.0100467 -158.045 -0.64386 -0.765083 0.00958838 612.071 0.0138608 0.000866586 0.999904 0.424264\n-0.795778 0.60555 0.00685574 -157.209 -0.605507 -0.795805 0.00747575 612.666 0.00998277 0.00179784 0.999949 0.475232\n-0.823448 0.567391 -0.000881315 -156.346 -0.567343 -0.823357 0.0142972 613.231 0.00738644 0.012273 0.999897 0.472256\n-0.848762 0.528774 -0.00084567 -155.44 -0.528741 -0.84869 0.0125287 613.751 0.00590714 0.011081 0.999921 0.476203\n-0.871712 0.490016 -0.00161822 -154.507 -0.490012 -0.87168 0.00791066 614.211 0.00246578 0.00768877 0.999967 0.520861\n-0.891293 0.453371 -0.0071212 -153.543 -0.453415 -0.891273 0.00682354 614.663 -0.00325334 0.00931064 0.999951 0.531458\n-0.912766 0.408448 -0.00533533 -152.037 -0.408471 -0.91276 0.00445788 615.298 -0.00304907 0.00624833 0.999976 0.575984\n-0.922714 0.385398 -0.00813311 -151.02 -0.385435 -0.922728 0.0035784 615.699 -0.00612554 0.00643663 0.999961 0.618439\n-0.929191 0.369514 -0.0079049 -149.947 -0.369539 -0.929213 0.00184189 616.098 -0.00666474 0.00463264 0.999967 0.640298\n-0.932804 0.360371 -0.00297913 -148.904 -0.36038 -0.9328 0.0031994 616.496 -0.00162596 0.00405803 0.99999 0.678208\n-0.935704 0.352786 0.00018288 -147.84 -0.352766 -0.935659 0.00994019 616.899 0.00367787 0.00923657 0.999951 0.698996\n-0.938726 0.344643 0.00391029 -146.758 -0.34462 -0.938726 0.00549908 617.289 0.00556591 0.00381456 0.999977 0.700431\n-0.941022 0.338347 0.000112117 -145.678 -0.338346 -0.94102 0.00169797 617.663 0.000680008 0.0015599 0.999999 0.752103\n-0.943092 0.332496 -0.00485006 -144.58 -0.332514 -0.943092 0.00342408 618.05 -0.00343556 0.00484193 0.999982 0.768907\n-0.944638 0.328115 -0.000537893 -143.446 -0.328115 -0.944637 0.00106992 618.425 -0.000157058 0.00118718 0.999999 0.794858\n-0.945957 0.324283 0.00254956 -142.329 -0.324278 -0.945959 0.00213234 618.798 0.00310326 0.00119033 0.999994 0.843278\n-0.946847 0.321678 0.00208417 -141.198 -0.321664 -0.946839 0.00536053 619.198 0.00369774 0.00440519 0.999983 0.857673\n-0.947639 0.319314 0.00444098 -140.045 -0.319301 -0.947647 0.00340868 619.567 0.00529692 0.00181219 0.999984 0.87222\n-0.947965 0.318338 0.00476023 -138.885 -0.318332 -0.947977 0.00208906 619.944 0.00517762 0.000465021 0.999986 0.904651\n-0.948273 0.317426 0.00432167 -137.715 -0.317426 -0.948283 0.000682279 620.327 0.00431474 -0.000724823 0.99999 0.916837\n-0.948506 0.3167 0.00615986 -136.524 -0.316703 -0.948525 0.00056133 620.715 0.00602055 -0.00141842 0.999981 0.947529\n-0.948707 0.316103 0.00581582 -135.329 -0.316096 -0.948725 0.00214956 621.117 0.0061971 0.000200949 0.999981 0.971337\n-0.948672 0.316118 0.00955481 -134.108 -0.316128 -0.948716 0.00043871 621.517 0.00920349 -0.00260435 0.999954 0.973757\n-0.948593 0.316195 0.0138852 -132.889 -0.316151 -0.948694 0.00530336 621.92 0.0148497 0.000640905 0.99989 1.00669\n-0.948479 0.316647 0.0110908 -131.66 -0.316612 -0.948543 0.00481583 622.345 0.012045 0.00105623 0.999927 1.01284\n-0.948368 0.31696 0.0116054 -130.407 -0.316961 -0.948437 0.00179366 622.738 0.0115755 -0.00197743 0.999931 1.0194\n-0.948409 0.316979 0.00669861 -129.135 -0.316982 -0.948431 0.000650205 623.178 0.00655927 -0.00150668 0.999977 1.04266\n-0.948133 0.317863 -0.00268063 -127.847 -0.317842 -0.948123 -0.00622509 623.594 -0.0045203 -0.0050502 0.999977 1.0741\n-0.948101 0.317965 0.00158778 -126.528 -0.317968 -0.948069 -0.00792237 624.025 -0.00101372 -0.00801607 0.999967 1.09554\n-0.948002 0.318175 0.00754223 -125.197 -0.318225 -0.94799 -0.00689366 624.455 0.00495657 -0.00893534 0.999948 1.14727\n-0.947981 0.318277 0.00561837 -123.873 -0.318306 -0.947973 -0.00533229 624.911 0.00362892 -0.00684327 0.99997 1.15852\n-0.947905 0.318394 0.0100379 -122.547 -0.318476 -0.947898 -0.00792241 625.345 0.00699249 -0.0107065 0.999918 1.1853\n-0.947852 0.318474 0.0123027 -121.193 -0.318564 -0.947882 -0.00608639 625.798 0.00972314 -0.00968818 0.999906 1.20663\n-0.947767 0.31872 0.0124734 -119.807 -0.318847 -0.947754 -0.00994624 626.253 0.00865161 -0.0134038 0.999873 1.22057\n-0.947645 0.319073 0.0126948 -118.406 -0.319232 -0.947581 -0.0134384 626.716 0.00774149 -0.0167874 0.999829 1.24773\n-0.947248 0.320286 0.0117407 -117.016 -0.320431 -0.947171 -0.0138479 627.18 0.00668518 -0.0168795 0.999835 1.26562\n-0.947025 0.320887 0.0132222 -115.589 -0.321077 -0.946912 -0.016361 627.656 0.00727026 -0.0197396 0.999779 1.28881\n-0.946903 0.321145 0.015492 -114.166 -0.321365 -0.946842 -0.0146586 628.142 0.0099609 -0.0188588 0.999773 1.32806\n-0.946483 0.322607 0.00976029 -112.729 -0.322696 -0.946453 -0.00967534 628.644 0.00611633 -0.0123072 0.999906 1.33865\n-0.946292 0.323122 0.0111513 -111.273 -0.323234 -0.946262 -0.0103555 629.135 0.00720594 -0.0134038 0.999884 1.35824\n-0.94602 0.32367 0.0168594 -109.779 -0.323801 -0.946108 -0.00564084 629.645 0.014125 -0.0107954 0.999842 1.36918\n-0.945647 0.324774 0.0165689 -108.294 -0.32488 -0.945746 -0.00412698 630.157 0.0143296 -0.00928557 0.999854 1.38644\n-0.945352 0.32577 0.0135298 -106.787 -0.32586 -0.945405 -0.00498732 630.67 0.0111664 -0.00912361 0.999896 1.409\n-0.945069 0.326694 0.0107625 -104.483 -0.326692 -0.945129 0.00200997 631.469 0.0108286 -0.00161647 0.99994 1.44716\n-0.944792 0.327478 0.0112574 -102.931 -0.327407 -0.944852 0.00769367 632.015 0.0131561 0.00358316 0.999907 1.47052\n-0.94431 0.329016 0.00519233 -101.374 -0.329033 -0.944314 -0.00295002 632.542 0.00393259 -0.00449418 0.999982 1.49272\n-0.944249 0.329226 0.00195056 -99.8023 -0.329229 -0.944249 -0.00118609 633.09 0.00145133 -0.00176214 0.999997 1.54355\n-0.943795 0.329971 0.0192457 -98.1943 -0.329994 -0.943981 0.00208151 633.653 0.0188544 -0.00438644 0.999813 1.49037\n-0.943931 0.329678 0.0175155 -96.5807 -0.329794 -0.944044 -0.00410408 634.206 0.0151824 -0.00965047 0.999838 1.53324\n-0.944028 0.329763 0.0082083 -94.9812 -0.329835 -0.943986 -0.00995903 634.752 0.00446441 -0.012109 0.999917 1.55728\n-0.944526 0.328209 0.0122077 -93.3256 -0.328311 -0.94454 -0.00748484 635.327 0.00907412 -0.0110776 0.999897 1.58362\n-0.945617 0.325161 0.008905 -90.8135 -0.325234 -0.945595 -0.00854723 636.17 0.00564129 -0.0109786 0.999924 1.63026\n-0.946727 0.321948 0.00763758 -89.1431 -0.322004 -0.946706 -0.00780739 636.736 0.00471697 -0.00985079 0.99994 1.66049\n-0.947718 0.318724 0.0156554 -86.5584 -0.31888 -0.947755 -0.00871687 637.584 0.0120592 -0.0132533 0.999839 1.67613\n-0.948527 0.316134 0.0188857 -84.8215 -0.316339 -0.948603 -0.00903916 638.155 0.0150575 -0.0145482 0.999781 1.71423\n-0.949395 0.313828 0.0126913 -83.0869 -0.313998 -0.949308 -0.0148108 638.721 0.00739987 -0.0180463 0.99981 1.72027\n-0.950518 0.310288 0.0153956 -81.3169 -0.310524 -0.950423 -0.016479 639.303 0.00951905 -0.0204443 0.999746 1.755\n-0.951124 0.308048 0.0216604 -79.5301 -0.308419 -0.951107 -0.0165244 639.864 0.015511 -0.0223972 0.999629 1.77309\n-0.951943 0.305359 0.0236965 -77.748 -0.305779 -0.951955 -0.0167479 640.427 0.0174439 -0.023189 0.999579 1.79201\n-0.952971 0.302514 0.0181938 -75.9957 -0.302814 -0.952901 -0.0168437 640.979 0.0122415 -0.0215608 0.999693 1.82922\n-0.953937 0.299534 0.0168388 -74.2281 -0.299829 -0.9538 -0.0191757 641.525 0.010317 -0.0233412 0.999674 1.84142\n-0.954716 0.29683 0.0202465 -72.4885 -0.297176 -0.954672 -0.0169436 642.066 0.0142994 -0.0221931 0.999651 1.88164\n-0.955668 0.29405 0.0152856 -70.7514 -0.294287 -0.955571 -0.0167078 642.601 0.00969357 -0.0204654 0.999744 1.89772\n-0.956822 0.289951 0.0205005 -68.1412 -0.29028 -0.956818 -0.0153829 643.379 0.015155 -0.0206696 0.999671 1.92847\n-0.957651 0.286767 0.0258807 -66.3815 -0.287251 -0.957697 -0.0174057 643.897 0.0197945 -0.0241029 0.999513 1.93241\n-0.959312 0.281296 0.0243488 -63.7688 -0.281842 -0.959187 -0.0229371 644.642 0.0169029 -0.0288664 0.99944 1.98307\n-0.960364 0.277486 0.0264962 -61.1416 -0.278128 -0.960225 -0.0247578 645.379 0.0185723 -0.0311458 0.999342 2.01815\n-0.961008 0.275176 0.0272389 -59.4091 -0.275724 -0.96105 -0.018922 645.895 0.0209711 -0.0256946 0.99945 2.01665\n-0.962046 0.271965 0.0224126 -57.7185 -0.272311 -0.962105 -0.014135 646.367 0.0177191 -0.0197017 0.999649 2.04107\n-0.96348 0.266864 0.0221194 -55.167 -0.267142 -0.963598 -0.0106991 647.08 0.018459 -0.0162174 0.999698 2.04046\n-0.965107 0.261058 0.0204286 -53.4817 -0.261292 -0.965211 -0.00971933 647.528 0.0171806 -0.014718 0.999744 2.07446\n-0.968295 0.249184 0.0176482 -51.7996 -0.249373 -0.968361 -0.00947062 647.955 0.0147299 -0.0135713 0.999799 2.08309\n-0.974713 0.223062 0.0133496 -49.3029 -0.223119 -0.974787 -0.00293659 648.524 0.012358 -0.00584088 0.999907 2.13868\n-0.979261 0.202556 0.00434703 -47.6187 -0.20256 -0.97927 -0.00043545 648.862 0.00416871 -0.00130695 0.99999 2.16112\n-0.983831 0.178983 0.00648723 -45.9149 -0.178957 -0.983847 0.00438084 649.169 0.00716653 0.00314907 0.999969 2.20192\n-0.987495 0.157143 0.0126262 -44.2017 -0.157021 -0.987543 0.0101688 649.44 0.0140668 0.00805903 0.999869 2.2152\n-0.990894 0.133826 0.0148441 -42.4682 -0.133628 -0.990938 0.0136134 649.668 0.0165314 0.0115058 0.999797 2.22961\n-0.99367 0.111469 0.0139784 -40.7342 -0.111272 -0.993689 0.0141653 649.851 0.0154692 0.0125202 0.999802 2.23702\n-0.996019 0.0883932 0.0115407 -38.9664 -0.0882519 -0.996023 0.0122236 649.984 0.0125753 0.0111565 0.999859 2.24811\n-0.99777 0.0652058 0.0142446 -37.1944 -0.0650225 -0.9978 0.0129687 650.086 0.0150589 0.0120136 0.999814 2.2756\n-0.998836 0.0450347 0.0172955 -35.4071 -0.0448296 -0.998922 0.0120654 650.162 0.0178202 0.011276 0.999778 2.27817\n-0.999335 0.0305509 0.0198917 -33.5985 -0.0303552 -0.999488 0.0100663 650.209 0.0201891 0.00945577 0.999751 2.29797\n-0.99961 0.0180829 0.0212724 -31.7775 -0.0178459 -0.999777 0.0112777 650.235 0.0214716 0.0108937 0.99971 2.30389\n-0.999679 0.00696704 0.0243464 -29.9757 -0.0067464 -0.999936 0.00913271 650.224 0.0244084 0.00896553 0.999662 2.30224\n-0.99971 -0.00171802 0.0240261 -28.1869 0.00186595 -0.999979 0.00613615 650.211 0.0240151 0.00617921 0.999692 2.31002\n-0.999664 -0.00757524 0.0247775 -26.408 0.0076633 -0.999965 0.00346106 650.196 0.0247505 0.00364978 0.999687 2.3123\n-0.999614 -0.012508 0.0248112 -24.6368 0.0126042 -0.999914 0.00372646 650.176 0.0247624 0.00403775 0.999685 2.32375\n-0.999552 -0.0156315 0.0255238 -22.8835 0.0158287 -0.999846 0.00754403 650.149 0.0254019 0.00794466 0.999646 2.32759\n-0.999478 -0.0182472 0.0266426 -21.1482 0.0185359 -0.999772 0.0106307 650.12 0.0264425 0.011119 0.999588 2.33484\n-0.999462 -0.0209019 0.0252741 -19.4097 0.021257 -0.999678 0.0138618 650.089 0.0249763 0.0143916 0.999584 2.33216\n-0.999461 -0.0221681 0.0242066 -17.6948 0.0225114 -0.999649 0.0140003 650.044 0.0238877 0.0145377 0.999609 2.32264\n-0.999351 -0.0222249 0.0283451 -15.9904 0.0226486 -0.999635 0.0147169 650.005 0.0280077 0.0153493 0.99949 2.33231\n-0.99922 -0.0220132 0.0327969 -14.2947 0.0225717 -0.999605 0.0167576 649.974 0.032415 0.0174848 0.999322 2.31515\n-0.999683 -0.0214226 0.0132228 -12.6361 0.0215917 -0.999685 0.0127797 649.931 0.0129449 0.0130612 0.999831 2.3305\n-0.999747 -0.0203547 0.00960113 -10.9675 0.0204354 -0.999756 0.00837972 649.89 0.00942822 0.0085738 0.999919 2.34046\n-0.999478 -0.0190105 0.0261219 -9.28623 0.019132 -0.999807 0.00440937 649.854 0.0260331 0.00490683 0.999649 2.3783\n-0.999563 -0.0173613 0.023929 -7.63161 0.0174859 -0.999835 0.00500784 649.809 0.0238381 0.00542407 0.999701 2.39466\n-0.999695 -0.0160312 0.0187898 -5.99667 0.0161735 -0.999841 0.00744346 649.785 0.0186675 0.00774508 0.999796 2.39396\n-0.999538 -0.0150175 0.0264208 -4.35101 0.0152048 -0.999861 0.00690451 649.75 0.0263134 0.00730304 0.999627 2.40062\n-0.999689 -0.0147105 0.0201299 -2.71438 0.0147618 -0.999888 0.00239865 649.711 0.0200924 0.00269505 0.999794 2.40604\n-0.99983 -0.0147021 0.0110951 -1.0961 0.0146909 -0.999891 -0.00108617 649.67 0.0111099 -0.000922988 0.999938 2.42793\n-0.999694 -0.0142317 0.0202537 0.552063 0.0141642 -0.999894 -0.00347426 649.623 0.020301 -0.00318631 0.999789 2.43583\n-0.999716 -0.0147459 0.0187299 2.16816 0.0146604 -0.999881 -0.00469749 649.588 0.0187969 -0.00442157 0.999814 2.4627\n-0.999763 -0.0165288 0.0141894 3.77605 0.0164856 -0.999859 -0.00316113 649.561 0.0142397 -0.00292646 0.999894 2.48464\n-0.999684 -0.0218549 0.0124115 5.37634 0.0217725 -0.99974 -0.00674092 649.511 0.0125556 -0.00646856 0.9999 2.498\n-0.999501 -0.0287565 0.0130606 6.96733 0.0286486 -0.999554 -0.0083748 649.449 0.0132956 -0.00799645 0.99988 2.52819\n-0.999015 -0.0378915 0.0231014 8.58036 0.0376993 -0.999251 -0.00870127 649.378 0.0234138 -0.00782179 0.999695 2.52931\n-0.998424 -0.0514836 0.0223241 10.1781 0.0511996 -0.998602 -0.0131136 649.282 0.0229681 -0.01195 0.999665 2.54384\n-0.997773 -0.0653922 0.0131631 11.7425 0.0651585 -0.997722 -0.01746 649.169 0.0142749 -0.0165634 0.999761 2.55775\n-0.996475 -0.0822226 0.0166368 13.3243 0.081919 -0.996473 -0.018176 649.027 0.0180726 -0.016749 0.999696 2.57927\n-0.9943 -0.103909 0.0238789 14.8963 0.103528 -0.994487 -0.0166677 648.854 0.0254792 -0.0141005 0.999576 2.5963\n-0.991425 -0.128917 0.0213749 16.4551 0.128631 -0.99159 -0.0142662 648.64 0.0230343 -0.0113944 0.99967 2.60752\n-0.987832 -0.155098 0.0115349 18.0155 0.154905 -0.987799 -0.0160511 648.381 0.0138837 -0.014069 0.999805 2.58507\n-0.98285 -0.184304 0.00615443 19.5466 0.184123 -0.982642 -0.0226432 648.072 0.0102208 -0.0211217 0.999725 2.62262\n-0.976668 -0.214182 0.0156465 21.0936 0.21368 -0.97648 -0.0287536 647.711 0.021437 -0.0247394 0.999464 2.63178\n-0.969894 -0.242547 0.0218186 22.6167 0.241802 -0.969797 -0.032037 647.308 0.0289301 -0.0257968 0.999249 2.63988\n-0.962414 -0.27127 0.0130864 24.1341 0.270792 -0.962168 -0.0300747 646.867 0.0207496 -0.0254007 0.999462 2.64709\n-0.954198 -0.298976 0.0108945 25.6278 0.298514 -0.953879 -0.031683 646.377 0.0198645 -0.0269798 0.999439 2.6434\n-0.945876 -0.324477 0.00570428 27.099 0.324054 -0.945299 -0.037405 645.828 0.0175293 -0.033532 0.999284 2.65483\n-0.937447 -0.348048 0.00740284 28.5555 0.347498 -0.93682 -0.0401726 645.249 0.0209171 -0.0350872 0.999165 2.67185\n-0.929665 -0.3681 0.0149815 30.0015 0.367149 -0.929085 -0.0447634 644.652 0.0303965 -0.0361146 0.998885 2.67171\n-0.921955 -0.386935 0.016722 31.4266 0.385898 -0.921436 -0.0451418 644.035 0.0328752 -0.0351657 0.998841 2.6599\n-0.915251 -0.402637 0.0140817 32.8338 0.401682 -0.914662 -0.0452294 643.392 0.031091 -0.0357399 0.998877 2.64343\n-0.908515 -0.417765 0.00851409 34.2146 0.417146 -0.907976 -0.0396001 642.732 0.0242741 -0.0324256 0.999179 2.65485\n-0.900079 -0.435645 0.00846391 35.59 0.43514 -0.899704 -0.0344473 642.053 0.0226218 -0.0273223 0.999371 2.65861\n-0.888927 -0.457748 0.0166241 36.948 0.457115 -0.888846 -0.0316043 641.366 0.0292431 -0.0204948 0.999362 2.65845\n-0.87578 -0.482413 0.0169528 38.2939 0.481855 -0.875778 -0.0287866 640.612 0.0287339 -0.0170419 0.999442 2.6438\n-0.86231 -0.506303 0.00887711 39.6083 0.505834 -0.86206 -0.0313905 639.807 0.0235457 -0.022578 0.999468 2.63711\n-0.847335 -0.531017 0.00660742 40.9099 0.530654 -0.847109 -0.028509 638.967 0.020736 -0.0206504 0.999572 2.65254\n-0.832291 -0.554305 0.00605771 42.1954 0.55397 -0.832087 -0.0273598 638.08 0.0202062 -0.0194155 0.999607 2.6422\n-0.815877 -0.578184 0.00692141 43.455 0.577771 -0.815651 -0.0299065 637.162 0.0229369 -0.0204011 0.999529 2.66194\n-0.79877 -0.601587 0.00769391 44.7035 0.601186 -0.798602 -0.0284488 636.194 0.0232588 -0.0180986 0.999566 2.6519\n-0.781456 -0.623948 0.00393051 45.9175 0.623524 -0.781129 -0.0324896 635.186 0.023342 -0.0229384 0.999464 2.64302\n-0.76376 -0.645417 0.0103547 47.1108 0.644622 -0.763459 -0.0399172 634.139 0.0336687 -0.0238123 0.999149 2.61899\n-0.746897 -0.664586 0.0216734 48.2899 0.663215 -0.74691 -0.0476532 633.048 0.0478577 -0.0212179 0.998629 2.64545\n-0.730555 -0.682853 0.00148286 49.4422 0.682586 -0.730327 -0.0264405 631.986 0.019138 -0.0183041 0.999649 2.62425\n-0.715838 -0.698192 -0.0101685 50.5328 0.698211 -0.715521 -0.0230644 630.875 0.00882759 -0.0236102 0.999682 2.60689\n-0.701719 -0.712329 0.013307 51.6784 0.711379 -0.701564 -0.0418077 629.683 0.0391166 -0.0198709 0.999037 2.59123\n-0.686275 -0.727191 0.0148347 52.7606 0.726288 -0.686235 -0.0398389 628.509 0.0391505 -0.0165662 0.999096 2.60906\n-0.670736 -0.741577 0.0133035 53.8238 0.741041 -0.67079 -0.0299905 627.32 0.0311642 -0.0102573 0.999462 2.5821\n-0.654505 -0.756027 0.00686215 54.8358 0.755649 -0.654422 -0.026945 626.131 0.0248619 -0.0124503 0.999613 2.58137\n-0.637111 -0.77075 0.00592225 55.8453 0.770444 -0.637045 -0.0242974 624.909 0.0224999 -0.0109174 0.999687 2.57981\n-0.620656 -0.784083 0.000395088 56.7974 0.783719 -0.620384 -0.0301377 623.649 0.0238756 -0.0183955 0.999546 2.567\n-0.602546 -0.798074 -0.00401013 57.7281 0.797412 -0.601824 -0.0440631 622.36 0.0327523 -0.0297478 0.999021 2.57971\n-0.584173 -0.811629 0.000887071 58.6488 0.810678 -0.583541 -0.047756 621.05 0.0392778 -0.0271786 0.998859 2.55153\n-0.566553 -0.824021 -0.00256761 59.5101 0.823206 -0.565848 -0.0463495 619.707 0.0367401 -0.0283731 0.998922 2.52446\n-0.547018 -0.837112 -0.00398594 60.3626 0.836586 -0.546492 -0.0383443 618.386 0.0299202 -0.0243096 0.999257 2.5203\n-0.525954 -0.850513 -0.000729579 61.2202 0.849845 -0.525506 -0.0400903 617.062 0.0337139 -0.0217057 0.999196 2.50766\n-0.508036 -0.861324 -0.00453778 62.0092 0.860817 -0.50754 -0.0373769 615.659 0.0298905 -0.022895 0.999291 2.49465\n-0.489134 -0.872195 -0.00488642 62.7756 0.871776 -0.488707 -0.0342215 614.276 0.0274598 -0.0209988 0.999402 2.4836\n-0.472627 -0.881263 -0.000393624 63.5156 0.880539 -0.47222 -0.0407345 612.836 0.0357119 -0.0195988 0.99917 2.46908\n-0.457146 -0.889371 -0.00610114 64.2478 0.888766 -0.456556 -0.0406436 611.374 0.0333617 -0.0240025 0.999155 2.45304\n-0.444589 -0.895543 -0.0185164 64.9515 0.895311 -0.443647 -0.0399477 609.93 0.0275602 -0.0343383 0.99903 2.42815\n-0.433112 -0.901167 -0.0176587 65.6416 0.900684 -0.431966 -0.0466271 608.435 0.0343909 -0.0360997 0.998756 2.44074\n-0.4228 -0.906013 -0.0195111 66.3085 0.90555 -0.421558 -0.0476141 606.937 0.034914 -0.0377995 0.998675 2.40099\n-0.411997 -0.910846 -0.0248676 66.9784 0.910754 -0.410809 -0.0419928 605.432 0.0280331 -0.0399491 0.998808 2.40479\n-0.401309 -0.915853 -0.0128549 67.6454 0.915076 -0.400279 -0.0491104 603.889 0.0398323 -0.0314716 0.998711 2.39798\n-0.390788 -0.920447 -0.00783527 68.3064 0.919302 -0.389842 -0.0539209 602.317 0.0465768 -0.0282746 0.998514 2.34588\n-0.380124 -0.924896 -0.00856536 68.9218 0.924321 -0.379518 -0.039968 600.768 0.0337155 -0.0231099 0.999164 2.32289\n-0.368965 -0.929388 -0.010154 69.5271 0.928966 -0.368404 -0.0360491 599.22 0.0297628 -0.0227336 0.999298 2.32109\n-0.355324 -0.934687 -0.0102076 70.1345 0.934365 -0.354848 -0.0323347 597.628 0.0266007 -0.0210269 0.999425 2.31777\n-0.340895 -0.940051 -0.00975275 70.6862 0.939731 -0.340452 -0.0315858 596.05 0.0263719 -0.0199324 0.999453 2.32366\n-0.326774 -0.945054 -0.0095822 71.2335 0.944649 -0.326286 -0.0342956 594.439 0.0292846 -0.0202587 0.999366 2.3274\n-0.30997 -0.950734 -0.00479283 71.758 0.950223 -0.309628 -0.0347257 592.865 0.031531 -0.0153182 0.999385 2.32469\n-0.283362 -0.959013 -0.000144445 72.4698 0.958359 -0.283163 -0.0369758 590.487 0.0354194 -0.010616 0.999316 2.3129\n-0.261978 -0.96506 0.00512397 72.9031 0.96426 -0.261971 -0.0396612 588.896 0.0396178 -0.00544951 0.9992 2.28927\n-0.240044 -0.970738 0.00681015 73.2713 0.969923 -0.240122 -0.039889 587.287 0.040357 -0.0029698 0.999181 2.25444\n-0.217759 -0.975979 0.00672277 73.6091 0.975332 -0.217859 -0.0355531 585.695 0.0361637 -0.00118505 0.999345 2.24089\n-0.194766 -0.980826 0.00675413 73.9157 0.98035 -0.194882 -0.0305819 584.098 0.0313118 0.000665082 0.999509 2.22551\n-0.172375 -0.984996 0.00838896 74.1785 0.984479 -0.172558 -0.032022 582.485 0.0329891 0.00273894 0.999452 2.223\n-0.148923 -0.988792 0.0106277 74.415 0.988248 -0.149199 -0.0332562 580.862 0.0344691 0.00555014 0.99939 2.20045\n-0.125361 -0.992069 0.00911219 74.608 0.991636 -0.12558 -0.0298078 579.253 0.0307157 0.00529925 0.999514 2.1845\n-0.102364 -0.99473 0.00578799 74.736 0.994219 -0.102497 -0.031994 577.619 0.0324186 0.00247951 0.999471 2.16195\n-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\n-0.0549943 -0.998484 -0.00221572 74.9039 0.997945 -0.0548913 -0.0330593 574.353 0.0328876 -0.00402924 0.999451 2.14236\n-0.0320104 -0.999463 -0.0069537 74.9492 0.999048 -0.0317893 -0.0298678 572.715 0.0296307 -0.00790316 0.99953 2.13266\n-0.00887584 -0.999926 -0.00829114 74.941 0.99946 -0.00860886 -0.0316989 571.085 0.0316252 -0.00856802 0.999463 2.12761\n0.0118416 -0.999901 -0.00754492 74.9257 0.999348 0.0120918 -0.0340259 569.452 0.0341137 -0.00713708 0.999392 2.11769\n0.0295122 -0.999512 -0.010245 74.8502 0.999015 0.0298342 -0.0328374 567.815 0.0331271 -0.00926584 0.999408 2.09619\n0.0417284 -0.999066 -0.0112129 74.7597 0.998473 0.0421051 -0.0357668 566.201 0.0362055 -0.00970328 0.999297 2.09392\n0.0526874 -0.998543 -0.0116768 74.6731 0.997855 0.0530988 -0.0382859 564.59 0.0388501 -0.00963455 0.999199 2.06829\n0.0623122 -0.997946 -0.0148469 74.5479 0.99714 0.0628855 -0.0419135 562.99 0.042761 -0.0121927 0.999011 2.05016\n0.0721066 -0.997271 -0.0158586 74.4355 0.996384 0.0727406 -0.0438988 561.392 0.0449325 -0.0126359 0.99891 2.01872\n0.0814426 -0.996501 -0.018781 74.2823 0.99581 0.0821436 -0.0401885 559.811 0.0415906 -0.0154292 0.999016 1.99005\n0.0897623 -0.995781 -0.019033 74.1125 0.995189 0.0904296 -0.037708 558.253 0.0392701 -0.0155566 0.999108 1.98003\n0.0984643 -0.994987 -0.0174869 73.9559 0.994136 0.0991391 -0.0431845 556.688 0.0447016 -0.0131322 0.998914 1.96266\n0.10708 -0.994106 -0.016933 73.776 0.992984 0.107788 -0.0486226 555.14 0.0501612 -0.0116077 0.998674 1.92768\n0.116028 -0.993145 -0.0141951 73.5993 0.992025 0.116582 -0.0478992 553.602 0.0492257 -0.00852427 0.998751 1.89109\n0.124582 -0.992119 -0.0133601 73.3931 0.991192 0.125053 -0.0435996 552.098 0.0449268 -0.00781062 0.99896 1.8487\n0.133507 -0.990936 -0.0149157 73.1794 0.989921 0.134057 -0.0456569 550.582 0.0472426 -0.00866984 0.998846 1.82475\n0.144476 -0.989419 -0.0133299 72.9584 0.988344 0.144946 -0.0465588 549.109 0.0479983 -0.00644786 0.998827 1.81218\n0.159969 -0.987087 -0.00837978 72.7152 0.986429 0.160168 -0.0361046 547.678 0.0369805 -0.00249046 0.999313 1.78157\n0.17842 -0.98384 -0.0150028 72.4356 0.983291 0.178839 -0.0339886 546.294 0.0361225 -0.00868782 0.99931 1.74862\n0.198813 -0.979821 -0.020578 72.1388 0.979095 0.199499 -0.0396589 544.956 0.0429639 -0.0122631 0.999001 1.74925\n0.222065 -0.974794 -0.0215407 71.8215 0.974085 0.222768 -0.0391446 543.66 0.0429565 -0.0122898 0.999001 1.74722\n0.249627 -0.968187 -0.0173425 71.4885 0.967183 0.250164 -0.0444387 542.383 0.0473635 -0.0056803 0.998862 1.73042\n0.279265 -0.960046 -0.0179578 71.0821 0.958561 0.279833 -0.0534345 541.132 0.0563247 -0.00229123 0.99841 1.66318\n0.315146 -0.948682 -0.0261779 70.6365 0.946891 0.316168 -0.0586023 539.911 0.0638716 -0.00631931 0.997938 1.6274\n0.351172 -0.935745 -0.0325537 70.1343 0.932244 0.352672 -0.0808946 538.683 0.0871775 -0.00194009 0.996191 1.55788\n0.385891 -0.921366 -0.0466226 69.5765 0.914629 0.388697 -0.111214 537.471 0.120591 0.000274231 0.992702 1.42612\n0.423305 -0.904236 -0.0563009 68.9803 0.89578 0.427029 -0.123391 536.273 0.135616 0.00179862 0.99076 1.30684\n0.45963 -0.886564 -0.052392 68.3276 0.876611 0.462351 -0.133359 535.092 0.142455 0.0153685 0.989682 1.19934\n0.492871 -0.868847 -0.04672 67.6444 0.857234 0.494082 -0.145029 533.904 0.149091 0.0314305 0.988324 1.00812\n0.524058 -0.850634 -0.042252 66.8902 0.839765 0.524358 -0.140867 532.738 0.141982 0.0383408 0.989126 0.838732\n0.555565 -0.830903 -0.0307867 66.0954 0.822678 0.554683 -0.124609 531.59 0.120615 0.0439007 0.991728 0.702211\n0.580625 -0.813559 -0.0315688 65.2504 0.806407 0.579998 -0.115374 530.421 0.112173 0.0415317 0.99282 0.542313\n0.59523 -0.802418 -0.0427512 64.3469 0.795345 0.595899 -0.111044 529.279 0.114579 0.0320946 0.992896 0.41565\n0.606591 -0.794097 -0.0381599 63.4302 0.787974 0.606901 -0.103772 528.13 0.105564 0.032878 0.993869 0.323818\n0.61815 -0.785295 -0.0346729 62.5219 0.779753 0.61817 -0.0992559 526.985 0.0993788 0.0343188 0.994458 0.210301\n0.627202 -0.778019 -0.0361064 61.5599 0.772547 0.627341 -0.0980523 525.842 0.0989376 0.0336047 0.994526 0.104201\n0.635827 -0.771103 -0.0335348 60.5851 0.765803 0.635684 -0.0972156 524.693 0.0962808 0.0361313 0.994698 0.00783555\n0.644753 -0.763939 -0.0262776 59.6061 0.759187 0.64399 -0.0943997 523.569 0.0890381 0.0409149 0.995188 -0.0872329\n0.653138 -0.757042 -0.0172861 58.6296 0.755073 0.652825 -0.060695 522.457 0.0572335 0.02659 0.998007 -0.154712\n0.660754 -0.750479 -0.0135911 57.6309 0.749495 0.660655 -0.0423486 521.356 0.0407608 0.0177956 0.99901 -0.178374\n0.667037 -0.744403 -0.0304349 56.6028 0.742859 0.667655 -0.0489532 520.248 0.056761 0.0100447 0.998337 -0.218824\n0.673218 -0.738553 -0.0362967 55.5681 0.736939 0.674165 -0.0492108 519.156 0.0608148 0.00638116 0.998129 -0.259945\n0.679743 -0.73275 -0.0320476 54.56 0.731502 0.680473 -0.0431529 518.094 0.0534278 0.00589004 0.998554 -0.304649\n0.686714 -0.726083 -0.035033 53.5442 0.725056 0.687607 -0.0386063 517.042 0.0521203 0.00111057 0.99864 -0.331525\n0.693784 -0.719407 -0.0334356 52.5214 0.718643 0.694588 -0.0331635 516.002 0.0470821 -0.00101994 0.998891 -0.359759\n0.701157 -0.711948 -0.0388559 51.481 0.711399 0.702194 -0.0289033 514.973 0.047862 -0.00737626 0.998827 -0.395733\n0.708739 -0.704093 -0.0440672 50.4272 0.703574 0.710031 -0.0289771 513.97 0.0516917 -0.0104674 0.998608 -0.419748\n0.717466 -0.695 -0.0470877 49.3801 0.694674 0.718867 -0.0256609 512.986 0.0516841 -0.0142998 0.998561 -0.463816\n0.725255 -0.686522 -0.0518875 48.3266 0.686248 0.726912 -0.0257524 512.012 0.0553972 -0.0169306 0.998321 -0.496731\n0.733555 -0.677602 -0.05246 47.274 0.677079 0.735303 -0.0299022 511.04 0.0588358 -0.0135846 0.998175 -0.535746\n0.742623 -0.667541 -0.0538485 46.2429 0.666888 0.744473 -0.0319486 510.093 0.0614158 -0.0121851 0.998038 -0.575964\n0.752727 -0.65626 -0.0522022 45.2023 0.655516 0.754473 -0.0326931 509.215 0.0608403 -0.0096104 0.998101 -0.613893\n0.762832 -0.643469 -0.0635221 44.1601 0.643132 0.76523 -0.0283472 508.373 0.0668496 -0.019229 0.997578 -0.663466\n0.773343 -0.631449 -0.0566875 43.142 0.630658 0.775352 -0.0331688 507.549 0.0648971 -0.0100995 0.997841 -0.688825\n0.783526 -0.618659 -0.0578577 42.1339 0.618448 0.78547 -0.0236408 506.78 0.0600711 -0.0172588 0.998045 -0.751954\n0.792807 -0.604815 -0.0752096 41.1155 0.605496 0.795688 -0.0159818 506.062 0.0695094 -0.0328687 0.99704 -0.788106\n0.802346 -0.592963 -0.0680837 40.1389 0.59137 0.805213 -0.0437421 505.301 0.0807594 -0.0051663 0.99672 -0.820877\n0.813129 -0.579261 -0.0572524 39.1803 0.577201 0.815111 -0.0493266 504.625 0.0752401 0.00706271 0.99714 -0.913858\n0.826046 -0.561387 -0.0499227 38.21 0.560589 0.827548 -0.0300895 504.018 0.0582052 -0.00313077 0.9983 -0.912673\n0.842001 -0.537716 -0.0435452 37.2272 0.536301 0.843058 -0.0404265 503.374 0.0584491 0.0106858 0.998233 -0.98241\n0.856656 -0.515127 -0.0279972 36.2392 0.514312 0.85702 -0.03163 502.808 0.0402877 0.0126967 0.999107 -0.984586\n0.875129 -0.483881 -0.00276103 35.237 0.483738 0.874985 -0.0199713 502.295 0.0120796 0.0161419 0.999797 -0.989062\n0.892005 -0.451752 -0.0157075 34.2031 0.451534 0.892118 -0.0155967 501.822 0.0210588 0.00681989 0.999755 -1.0122\n0.909079 -0.415373 -0.0322578 33.1353 0.414428 0.909512 -0.0322059 501.362 0.0427163 0.0159091 0.998961 -0.99559\n0.926233 -0.376196 -0.0238408 32.0687 0.375152 0.926139 -0.0390922 500.94 0.0367862 0.0272646 0.998951 -1.01832\n0.941775 -0.335778 -0.0177191 30.9817 0.335113 0.94162 -0.0324224 500.61 0.0275714 0.0245967 0.999317 -1.04152\n0.955871 -0.293138 -0.0195227 29.8596 0.292437 0.955738 -0.0323324 500.308 0.0281364 0.0251965 0.999286 -1.02881\n0.967855 -0.250772 -0.0192255 28.7173 0.249921 0.967509 -0.0382985 500.037 0.028205 0.0322626 0.999081 -1.04568\n0.977899 -0.208225 -0.0188883 27.5528 0.20754 0.977672 -0.0329478 499.845 0.0253271 0.0282996 0.999279 -1.06402\n0.98597 -0.165521 -0.0215866 26.3653 0.164814 0.98583 -0.0312347 499.692 0.0264507 0.0272387 0.999279 -1.06257\n0.99126 -0.129252 -0.02641 25.1547 0.128381 0.991197 -0.032359 499.57 0.03036 0.0286857 0.999127 -1.07591\n0.994567 -0.0999836 -0.0289847 23.916 0.0990822 0.994595 -0.0310285 499.471 0.0319304 0.027988 0.999098 -1.09377\n0.996768 -0.0751305 -0.0284577 22.6641 0.0742446 0.996758 -0.0310059 499.398 0.0306949 0.0287928 0.999114 -1.10559\n0.998253 -0.0511678 -0.0295471 21.4145 0.0501809 0.998187 -0.0332289 499.345 0.0311938 0.0316882 0.999011 -1.1215\n0.999146 -0.0299636 -0.0284543 20.1584 0.0290657 0.999082 -0.0314609 499.335 0.0293709 0.030607 0.9991 -1.14259\n0.999557 -0.0117993 -0.0273403 18.8731 0.0109391 0.999447 -0.031402 499.321 0.0276957 0.031089 0.999133 -1.14569\n0.999573 1.56447e-05 -0.0292174 17.5626 -0.000840857 0.999601 -0.0282318 499.341 0.0292053 0.0282443 0.999174 -1.17337\n0.999418 0.00737013 -0.0333216 16.2492 -0.0082 0.999658 -0.0248372 499.362 0.0331271 0.025096 0.999136 -1.15908\n0.999484 0.0142769 -0.0287733 14.9351 -0.0150881 0.999489 -0.0281778 499.373 0.0283563 0.0285974 0.999189 -1.17024\n0.999449 0.0195687 -0.0267994 13.627 -0.0202476 0.999475 -0.0253021 499.413 0.0262902 0.0258307 0.999321 -1.19128\n0.999267 0.0244168 -0.0294951 12.3066 -0.0251511 0.999376 -0.0247866 499.456 0.0288715 0.0255102 0.999258 -1.18342\n0.999185 0.0295021 -0.0275514 10.9984 -0.0302624 0.999161 -0.0275991 499.486 0.026714 0.0284104 0.999239 -1.19389\n0.999092 0.0315011 -0.0286709 9.66835 -0.0322525 0.999138 -0.0261329 499.543 0.027823 0.0270339 0.999247 -1.19796\n0.998976 0.0326619 -0.0313085 8.36179 -0.033588 0.999 -0.0295264 499.581 0.0303128 0.0305478 0.999074 -1.18999\n0.999058 0.0317918 -0.0295392 7.07421 -0.0327577 0.998925 -0.03281 499.613 0.0284644 0.0337468 0.999025 -1.20187\n0.999179 0.0281692 -0.0291079 5.80292 -0.0289997 0.999173 -0.0285149 499.65 0.0282806 0.0293357 0.999169 -1.21653\n0.999262 0.023896 -0.030088 4.52824 -0.0248131 0.999227 -0.0304883 499.679 0.0293362 0.0312124 0.999082 -1.20494\n0.999471 0.0187397 -0.0265902 3.27104 -0.0195178 0.99938 -0.0293108 499.688 0.0260245 0.0298143 0.999217 -1.21703\n0.999602 0.0135588 -0.0247208 2.02716 -0.0141976 0.999565 -0.0258499 499.714 0.0243596 0.0261906 0.99936 -1.21864\n0.999625 0.00859951 -0.0259988 0.785089 -0.009288 0.999606 -0.0264781 499.713 0.0257608 0.0267097 0.999311 -1.21223\n0.99963 0.0015987 -0.0271512 -0.426117 -0.00225773 0.999703 -0.024259 499.72 0.0271044 0.0243113 0.999337 -1.21684\n0.999628 -0.00713255 -0.0263319 -1.63018 0.00654095 0.999726 -0.0224851 499.701 0.0264851 0.0223045 0.9994 -1.20838\n0.999558 -0.0165897 -0.0246811 -2.83176 0.0160167 0.999602 -0.0232332 499.671 0.0250567 0.0228276 0.999425 -1.21598\n0.999311 -0.0269506 -0.0255263 -4.00742 0.0264761 0.999474 -0.0187492 499.641 0.0260182 0.0180604 0.999498 -1.21686\n0.99882 -0.0413837 -0.0254018 -5.18537 0.0409281 0.998996 -0.0181992 499.574 0.0261294 0.0171381 0.999512 -1.20345\n0.997665 -0.0636522 -0.0247519 -6.35197 0.0633426 0.997906 -0.0131015 499.47 0.025534 0.0115031 0.999608 -1.21874\n0.995368 -0.0917287 -0.0287704 -7.49982 0.0913821 0.995729 -0.0131414 499.346 0.0298529 0.0104515 0.9995 -1.21733\n0.991978 -0.122402 -0.0315907 -8.64119 0.122098 0.992453 -0.0113927 499.169 0.0327468 0.00744416 0.999436 -1.23089\n0.986956 -0.157965 -0.0310662 -9.76287 0.157828 0.987443 -0.00685153 498.963 0.0317584 0.00185906 0.999494 -1.23002\n0.979561 -0.19891 -0.0299248 -10.8414 0.198785 0.980017 -0.00713828 498.699 0.0307467 0.00104379 0.999527 -1.22486\n0.968944 -0.245489 -0.0297247 -11.9119 0.245249 0.969392 -0.0115004 498.352 0.0316381 0.00385332 0.999492 -1.23986\n0.953639 -0.299565 -0.0288604 -12.963 0.299366 0.954074 -0.0110769 497.953 0.0308532 0.00192353 0.999522 -1.25621\n0.932614 -0.359172 -0.0350075 -13.9581 0.358932 0.933271 -0.0131292 497.498 0.0373872 -0.000320853 0.999301 -1.26605\n0.905502 -0.422636 -0.0380277 -14.9388 0.422371 0.906297 -0.0151487 496.974 0.0408668 -0.00234461 0.999162 -1.29353\n0.871622 -0.487993 -0.046234 -15.8539 0.487826 0.872802 -0.0155977 496.389 0.0479646 -0.0089588 0.998809 -1.28693\n0.810382 -0.58507 -0.0312315 -17.1118 0.584391 0.810968 -0.0285977 495.338 0.0420594 0.00492366 0.999103 -1.35227\n0.75974 -0.649153 -0.0373601 -17.9016 0.649019 0.760576 -0.0172479 494.625 0.0396117 -0.0111435 0.999153 -1.32951\n0.677832 -0.734823 -0.0240348 -18.9463 0.734162 0.678249 -0.0313799 493.307 0.0393602 0.00362485 0.999219 -1.38517\n0.615894 -0.787367 -0.0269591 -19.5749 0.786928 0.616466 -0.0267205 492.416 0.0376582 -0.00475784 0.999279 -1.37463\n0.519289 -0.854199 -0.0261456 -20.3758 0.853665 0.519908 -0.0308448 490.937 0.0399409 -0.00630219 0.999182 -1.42395\n0.459064 -0.88821 -0.0185257 -20.8085 0.88751 0.459437 -0.0352528 489.937 0.0398233 -0.000258418 0.999207 -1.40022\n0.409772 -0.912107 -0.0121515 -21.1886 0.911436 0.409939 -0.035145 488.903 0.0370373 0.00332613 0.999308 -1.46644\n0.366364 -0.930285 -0.0186217 -21.587 0.929623 0.366811 -0.03536 487.827 0.0397255 -0.0043565 0.999201 -1.49078\n0.324567 -0.945674 -0.0188999 -21.9337 0.945011 0.325058 -0.0359345 486.728 0.0401259 -0.00619742 0.999175 -1.47769\n0.264406 -0.964252 -0.0175471 -22.3524 0.963512 0.264902 -0.0383571 485.012 0.0416342 -0.00676503 0.99911 -1.55067\n0.224699 -0.974217 -0.0202715 -22.5962 0.973194 0.225412 -0.0456299 483.84 0.0490229 -0.00947512 0.998753 -1.5698\n0.187422 -0.981992 -0.02377 -22.7971 0.981481 0.188191 -0.0357802 482.644 0.0396091 -0.0166238 0.999077 -1.59634\n0.130345 -0.991215 -0.0224214 -23.0144 0.990969 0.130964 -0.0288057 480.821 0.031489 -0.0184643 0.999334 -1.59099\n0.0992675 -0.994806 -0.0225274 -23.1191 0.994074 0.100152 -0.0422616 479.564 0.0442982 -0.0181987 0.998853 -1.63593\n0.0750571 -0.996871 -0.0248083 -23.1911 0.996151 0.0760858 -0.0435103 478.315 0.0452617 -0.0214471 0.998745 -1.65248\n0.0456118 -0.998724 -0.0216952 -23.2647 0.998177 0.0464248 -0.0385769 476.402 0.0395349 -0.0198961 0.99902 -1.67609\n0.0206813 -0.999559 -0.0213086 -23.2973 0.998842 0.0215831 -0.0430006 474.433 0.0434416 -0.0203946 0.998848 -1.72009\n0.00755424 -0.999765 -0.0203088 -23.2942 0.99885 0.008506 -0.0471945 473.101 0.0473561 -0.0199289 0.998679 -1.74533\n-0.00178469 -0.999793 -0.0202774 -23.2855 0.998765 -0.000775151 -0.0496856 471.758 0.0496596 -0.0203411 0.998559 -1.78609\n-0.00702993 -0.999796 -0.0189415 -23.2687 0.99873 -0.00607482 -0.0500184 470.421 0.0498931 -0.0192691 0.998569 -1.81774\n-0.00952215 -0.9998 -0.0176019 -23.2689 0.998784 -0.0086582 -0.0485239 468.417 0.0483617 -0.0180425 0.998667 -1.85277\n-0.0101065 -0.999792 -0.0177423 -23.2557 0.998858 -0.00926525 -0.0468708 467.071 0.0466966 -0.0181957 0.998743 -1.8825\n-0.0109412 -0.999756 -0.0191833 -23.2451 0.998794 -0.0100082 -0.0480748 465.737 0.0478711 -0.0196862 0.99866 -1.90958\n-0.00874213 -0.999775 -0.019325 -23.234 0.998828 -0.00781069 -0.0477592 463.765 0.0475975 -0.0197199 0.998672 -1.94393\n-0.00537923 -0.999747 -0.0218462 -23.2414 0.99878 -0.00429912 -0.049191 462.448 0.0490847 -0.0220841 0.99855 -1.97553\n-0.00190913 -0.999719 -0.0236363 -23.2536 0.998736 -0.000719128 -0.0502527 461.145 0.0502216 -0.0237024 0.998457 -1.98942\n0.00172388 -0.999686 -0.0250179 -23.2601 0.998764 0.00296418 -0.0496243 459.843 0.0496829 -0.0249014 0.998455 -2.02774\n0.00584129 -0.999671 -0.0249811 -23.2865 0.998344 0.00725944 -0.0570608 457.96 0.0572234 -0.0246064 0.998058 -2.06868\n0.00845203 -0.999647 -0.0251862 -23.302 0.998298 0.00988858 -0.0574701 456.795 0.0576988 -0.0246576 0.998029 -2.08494\n0.0110362 -0.999627 -0.0249792 -23.307 0.998383 0.0124086 -0.0554683 455.677 0.0557575 -0.0243266 0.998148 -2.12115\n0.0126544 -0.999644 -0.0234851 -23.3205 0.998286 0.0139723 -0.056831 454.649 0.0571389 -0.0227256 0.998108 -2.14966\n0.0129264 -0.999633 -0.0238238 -23.3475 0.998149 0.014316 -0.0591128 453.227 0.0594321 -0.0230156 0.997967 -2.17715\n0.0127191 -0.999736 -0.0191296 -23.3622 0.998168 0.0138263 -0.058906 451.94 0.0591549 -0.0183453 0.99808 -2.21573\n0.011647 -0.999727 -0.0202509 -23.3803 0.998527 0.0127014 -0.0527401 450.736 0.0529829 -0.0196069 0.998403 -2.24213\n0.0105561 -0.999811 -0.0163153 -23.376 0.998476 0.011423 -0.0539877 449.528 0.0541638 -0.0157205 0.998408 -2.28004\n0.00842234 -0.999888 -0.0123402 -23.3819 0.998518 0.00907295 -0.0536517 448.336 0.0537577 -0.0118701 0.998483 -2.31229\n0.00467598 -0.999902 -0.0132 -23.3984 0.998459 0.00539835 -0.0552307 447.129 0.0552966 -0.0129214 0.998386 -2.33519\n-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\n-0.00618767 -0.999932 -0.00987915 -23.3647 0.998426 -0.00562703 -0.0558026 444.761 0.0557432 -0.0102089 0.998393 -2.41259\n-0.0122362 -0.999897 -0.00752122 -23.3431 0.998346 -0.011794 -0.05626 443.577 0.0561655 -0.00819719 0.998388 -2.44436\n-0.0198243 -0.999781 -0.00664335 -23.3102 0.998066 -0.0193979 -0.0590571 442.43 0.0589153 -0.00780127 0.998232 -2.48788\n-0.0258572 -0.999661 -0.0030059 -23.2748 0.997832 -0.0256276 -0.0606126 441.422 0.0605151 -0.00456666 0.998157 -2.49931\n-0.0326385 -0.999467 -0.00101593 -23.2269 0.997496 -0.0325104 -0.0628022 440.262 0.0627357 -0.00306316 0.998025 -2.53942\n-0.0404543 -0.999177 0.00286654 -23.1708 0.997653 -0.0405508 -0.0551762 439.171 0.055247 0.000627704 0.998473 -2.57014\n-0.0578387 -0.998325 -0.00120453 -23.083 0.996668 -0.0576731 -0.0576758 437.851 0.0575097 -0.00453641 0.998335 -2.6099\n-0.0976305 -0.995222 -0.00133708 -22.9047 0.993961 -0.0974391 -0.0504636 436.64 0.0500922 -0.0062558 0.998725 -2.63368\n-0.178875 -0.983872 -0.000456519 -22.6105 0.983379 -0.178771 -0.0317185 435.615 0.0311253 -0.00612257 0.999497 -2.66327\n-0.3188 -0.947794 -0.00729284 -22.0423 0.947723 -0.318646 -0.0168786 434.319 0.0136736 -0.0122925 0.999831 -2.6456\n-0.449987 -0.893001 0.007816 -21.3408 0.892951 -0.450048 -0.00980162 433.312 0.0122704 0.0025687 0.999921 -2.64529\n-0.587622 -0.809131 0.00273149 -20.497 0.808937 -0.58755 -0.0201447 432.353 0.0179046 -0.00962788 0.999793 -2.64513\n-0.714343 -0.699787 0.00345885 -19.4164 0.699524 -0.714194 -0.0243303 431.505 0.0194964 -0.0149607 0.999698 -2.65464\n-0.795659 -0.60561 0.0128072 -18.4707 0.605104 -0.795611 -0.0291906 430.964 0.0278677 -0.0154761 0.999492 -2.63465\n-0.880167 -0.474379 0.0164741 -17.0644 0.473374 -0.879802 -0.0431855 430.326 0.0349802 -0.030212 0.998931 -2.70491\n-0.938202 -0.345976 0.00882784 -15.5292 0.345425 -0.937678 -0.0379628 429.907 0.0214119 -0.0325675 0.99924 -2.63504\n-0.965378 -0.260767 0.00681087 -14.2528 0.260293 -0.964682 -0.0404416 429.637 0.0171161 -0.0372686 0.999159 -2.66427\n-0.980374 -0.195894 0.0221738 -12.8966 0.195 -0.980101 -0.0371108 429.444 0.0290024 -0.0320586 0.999065 -2.65662\n-0.989192 -0.146126 0.0121412 -11.5036 0.145537 -0.98854 -0.0400959 429.271 0.0178611 -0.0378955 0.999122 -2.62693\n-0.99442 -0.103973 0.0178504 -9.50503 0.103334 -0.994079 -0.0336041 429.113 0.0212386 -0.031572 0.999276 -2.64246\n-0.995963 -0.0897633 0.000396875 -8.51011 0.0896736 -0.995146 -0.0405251 429.025 0.00403262 -0.0403259 0.999178 -2.57744\n-0.996724 -0.0800502 -0.0115314 -7.46337 0.0803846 -0.996242 -0.0322526 428.954 -0.00890627 -0.0330739 0.999413 -2.56614\n-0.996813 -0.0787387 0.0128471 -6.34036 0.0784017 -0.996611 -0.0249046 428.881 0.0147645 -0.023818 0.999607 -2.60492\n-0.995433 -0.0784189 0.0544305 -5.20606 0.0762164 -0.99623 -0.0414277 428.76 0.057474 -0.03709 0.997658 -2.60193\n-0.996145 -0.0800884 0.0357769 -4.10942 0.0788365 -0.996268 -0.0351323 428.688 0.0384571 -0.0321764 0.998742 -2.55401\n-0.996638 -0.0819018 0.0021053 -2.95801 0.081802 -0.996196 -0.0300483 428.597 0.0045583 -0.0297751 0.999546 -2.58523\n-0.996147 -0.0865807 0.0139608 -1.17872 0.0860891 -0.995753 -0.0326341 428.422 0.016727 -0.0313065 0.99937 -2.58129\n-0.995782 -0.0901711 0.0169605 0.0259342 0.0895953 -0.995461 -0.0320976 428.313 0.0197778 -0.0304426 0.999341 -2.57399\n-0.99533 -0.0954743 0.0142483 1.25816 0.09495 -0.994908 -0.0337994 428.176 0.0174027 -0.0322887 0.999327 -2.56764\n-0.994586 -0.102537 0.016898 2.51414 0.101901 -0.994178 -0.0350061 428.039 0.0203891 -0.0330947 0.999244 -2.56454\n-0.993582 -0.111501 0.019014 3.78871 0.110758 -0.993178 -0.0364786 427.88 0.0229517 -0.0341385 0.999154 -2.55654\n-0.992121 -0.124087 0.0172813 5.09294 0.123252 -0.99144 -0.0430794 427.704 0.022479 -0.04061 0.998922 -2.56555\n-0.990419 -0.137023 0.0171984 6.38614 0.136195 -0.98977 -0.04249 427.503 0.0228446 -0.0397405 0.998949 -2.55047\n-0.988304 -0.151619 0.0163572 7.74828 0.150752 -0.987534 -0.0452932 427.275 0.0230206 -0.0422975 0.99884 -2.55433\n-0.985572 -0.168407 0.0169346 9.11451 0.167299 -0.984459 -0.0533989 427.003 0.0256641 -0.0497953 0.99843 -2.54113\n-0.982602 -0.185301 0.0125508 10.5033 0.184513 -0.981664 -0.0478595 426.749 0.0211891 -0.044711 0.998775 -2.54765\n-0.979053 -0.203356 0.0101039 11.8936 0.202684 -0.978135 -0.0465965 426.434 0.0193586 -0.0435726 0.998863 -2.56998\n-0.974564 -0.223738 0.0129067 13.3138 0.222874 -0.973621 -0.0488857 426.104 0.0235039 -0.0447656 0.998721 -2.56679\n-0.969473 -0.244911 0.0118433 14.7197 0.244297 -0.968922 -0.0388421 425.716 0.0209881 -0.0347631 0.999175 -2.53286\n-0.964231 -0.264897 0.00936171 16.1666 0.264277 -0.963492 -0.0428957 425.305 0.0203829 -0.0388873 0.999036 -2.54318\n-0.958661 -0.284234 0.0134297 17.6078 0.283414 -0.957982 -0.0441255 424.847 0.0254074 -0.0384952 0.998936 -2.53891\n-0.952875 -0.30305 0.0137886 19.0424 0.302207 -0.952226 -0.0440143 424.373 0.0264685 -0.0377731 0.998936 -2.5379\n-0.94726 -0.320316 0.00979799 20.4409 0.319614 -0.946529 -0.0439358 423.867 0.0233474 -0.0384871 0.998986 -2.53065\n-0.941895 -0.335845 0.00642312 21.835 0.335325 -0.941218 -0.0408237 423.339 0.019756 -0.0362978 0.999146 -2.51151\n-0.936509 -0.350608 0.0050535 23.2136 0.350168 -0.93589 -0.0386283 422.799 0.0182729 -0.0344062 0.999241 -2.50758\n-0.930806 -0.365504 0.00254188 24.5946 0.365164 -0.930199 -0.0372181 422.244 0.0159678 -0.0337146 0.999304 -2.49057\n-0.924933 -0.380126 0.00196856 25.9748 0.379861 -0.924459 -0.0328911 421.663 0.0143226 -0.0296743 0.999457 -2.47894\n-0.918801 -0.394684 0.00537004 27.3553 0.394345 -0.918435 -0.0311337 421.056 0.01722 -0.026488 0.999501 -2.47708\n-0.91201 -0.410121 0.00618051 28.7308 0.409781 -0.911701 -0.0296758 420.421 0.0178055 -0.024532 0.99954 -2.46044\n-0.904733 -0.425941 0.00571963 30.1082 0.425586 -0.904394 -0.0307856 419.747 0.0182856 -0.0254186 0.99951 -2.45224\n-0.896697 -0.442599 0.00644864 31.4922 0.442205 -0.896358 -0.0315689 419.053 0.0197526 -0.0254561 0.999481 -2.44391\n-0.887182 -0.46137 0.00680524 32.8695 0.460935 -0.886829 -0.0327556 418.328 0.0211475 -0.0259234 0.99944 -2.44626\n-0.87741 -0.47969 0.00706869 34.2459 0.4792 -0.877024 -0.0345926 417.546 0.0227931 -0.0269645 0.999376 -2.43622\n-0.866889 -0.498479 0.0046687 35.6129 0.498068 -0.866489 -0.033527 416.735 0.0207579 -0.0267389 0.999427 -2.43107\n-0.856047 -0.516877 0.00473498 36.9812 0.516464 -0.855667 -0.0331416 415.894 0.0211817 -0.0259253 0.999439 -2.43309\n-0.844601 -0.535374 0.0048384 38.3395 0.53496 -0.844244 -0.0327206 415.015 0.0216026 -0.0250475 0.999453 -2.4261\n-0.833425 -0.552617 0.00422953 39.702 0.552213 -0.833066 -0.0325875 414.102 0.0215319 -0.0248236 0.99946 -2.4212\n-0.817441 -0.576011 0.0015247 41.7171 0.575628 -0.816989 -0.034375 412.668 0.021046 -0.0272218 0.999408 -2.42016\n-0.805949 -0.591977 0.00319811 43.0457 0.591509 -0.805505 -0.0357665 411.679 0.023749 -0.0269342 0.999355 -2.42404\n-0.793516 -0.608532 0.00468294 44.3748 0.607903 -0.793004 -0.0399745 410.632 0.0280394 -0.0288736 0.99919 -2.42741\n-0.780172 -0.625559 0.00276005 45.6867 0.624758 -0.779383 -0.0473279 409.556 0.0317575 -0.0351995 0.998876 -2.45061\n-0.762761 -0.646667 0.0042294 47.6403 0.645895 -0.76214 -0.0443058 407.886 0.0318745 -0.031063 0.999009 -2.4526\n-0.75144 -0.659796 0.00247704 48.9435 0.659515 -0.75122 -0.0266036 406.747 0.0194137 -0.0183573 0.999643 -2.42577\n-0.732535 -0.680617 0.0123869 50.8857 0.680661 -0.732598 -0.000868394 404.997 0.00966565 0.00779515 0.999923 -2.43182\n-0.716718 -0.697236 0.0133099 52.1347 0.697172 -0.716838 -0.00970006 403.76 0.0163043 0.00232709 0.999864 -2.40959\n-0.694989 -0.718756 0.0195148 53.9864 0.718215 -0.695239 -0.0284689 401.818 0.0340296 -0.00576974 0.999404 -2.41461\n-0.678921 -0.734041 0.0158078 55.1865 0.733505 -0.679056 -0.0292134 400.509 0.0321782 -0.00823847 0.999448 -2.42528\n-0.663037 -0.748472 0.0131387 56.368 0.747918 -0.663084 -0.0306311 399.15 0.0316385 -0.0104829 0.999444 -2.45188\n-0.64649 -0.762877 0.00829194 57.5263 0.762416 -0.646419 -0.0294092 397.769 0.0277957 -0.0126908 0.999533 -2.4552\n-0.629622 -0.776862 0.00787152 58.6686 0.776434 -0.629563 -0.0283072 396.34 0.0269464 -0.0117111 0.999568 -2.46119\n-0.612509 -0.790411 0.00914697 59.7821 0.789969 -0.612493 -0.0282973 394.881 0.0279689 -0.0101065 0.999558 -2.47002\n-0.587496 -0.809205 0.00590769 61.4184 0.808799 -0.587408 -0.0282146 392.638 0.0263016 -0.0117978 0.999584 -2.48215\n-0.571042 -0.820917 0.00268943 62.4815 0.820536 -0.570871 -0.0287448 391.102 0.0251324 -0.0142077 0.999583 -2.48783\n-0.548287 -0.836289 0.00136918 64.0243 0.835788 -0.548014 -0.0337529 388.738 0.0289776 -0.0173619 0.999429 -2.48619\n-0.533092 -0.846053 0.00252842 65.0247 0.845573 -0.532886 -0.0322331 387.135 0.0286183 -0.0150453 0.999477 -2.49352\n-0.517968 -0.855393 0.00347025 66.0005 0.854968 -0.517832 -0.0296719 385.517 0.0271781 -0.0124022 0.999554 -2.50657\n-0.502478 -0.864553 0.00798642 66.9554 0.864019 -0.50246 -0.0317033 383.87 0.0314221 -0.00902978 0.999465 -2.52529\n-0.486561 -0.873555 0.0126246 67.8711 0.873006 -0.486706 -0.0312761 382.213 0.0334659 -0.00419633 0.999431 -2.5258\n-0.468169 -0.883519 0.0145706 68.7688 0.883045 -0.468395 -0.0289258 380.518 0.0323813 -0.000675692 0.999475 -2.54604\n-0.449238 -0.893326 0.0124063 69.6137 0.892951 -0.449409 -0.0258896 378.816 0.0287033 -0.00055242 0.999588 -2.55902\n-0.429418 -0.903066 0.00841505 70.4352 0.902796 -0.429497 -0.0222031 377.073 0.0236651 -0.00193737 0.999718 -2.56238\n-0.407091 -0.913376 0.00456897 71.2093 0.913141 -0.407092 -0.0212127 375.314 0.0212352 -0.00446339 0.999765 -2.56962\n-0.384393 -0.92317 0.000228239 71.9297 0.922851 -0.384267 -0.0261581 373.518 0.024236 -0.00984434 0.999658 -2.56782\n-0.361642 -0.932312 -0.00310156 72.6195 0.932004 -0.361432 -0.0271041 371.706 0.0241485 -0.0126927 0.999628 -2.56251\n-0.338923 -0.940803 -0.00457534 73.2685 0.940547 -0.338707 -0.0254608 369.863 0.0224039 -0.0129326 0.999665 -2.55433\n-0.315774 -0.948765 -0.0115084 73.8552 0.948646 -0.315446 -0.0237689 368.032 0.0189208 -0.018423 0.999651 -2.56101\n-0.292199 -0.956188 -0.0179848 74.4075 0.956148 -0.291689 -0.0264418 366.158 0.0200374 -0.0249224 0.999489 -2.54948\n-0.272108 -0.962057 -0.0200737 74.9288 0.962049 -0.271543 -0.0269619 364.265 0.020488 -0.0266484 0.999435 -2.53238\n-0.251036 -0.967843 -0.0161576 75.4208 0.967692 -0.250521 -0.0284913 362.374 0.0235273 -0.0227879 0.999463 -2.52835\n-0.227874 -0.973607 -0.0127227 75.8713 0.97337 -0.227444 -0.028643 360.467 0.0249934 -0.0189109 0.999509 -2.52537\n-0.204444 -0.9788 -0.0123732 76.2613 0.978581 -0.204054 -0.0272116 358.537 0.0241099 -0.0176715 0.999553 -2.52837\n-0.180988 -0.983428 -0.0106119 76.6092 0.983198 -0.180664 -0.026134 356.598 0.0237837 -0.0151635 0.999602 -2.52478\n-0.1571 -0.987549 -0.00811064 76.9234 0.987297 -0.156852 -0.025329 354.642 0.0237415 -0.0119868 0.999646 -2.52577\n-0.132931 -0.991097 -0.00745504 77.1793 0.990863 -0.132719 -0.0239991 352.679 0.022796 -0.0105771 0.999684 -2.52852\n-0.121172 -0.992602 -0.00766914 77.2836 0.992392 -0.120969 -0.0229047 351.683 0.0218076 -0.0103862 0.999708 -2.5248\n-0.109484 -0.993965 -0.00681863 77.3863 0.99374 -0.1093 -0.0231267 350.681 0.0222419 -0.00930793 0.999709 -2.5254\n-0.0801542 -0.996749 -0.00815366 77.6208 0.996449 -0.0799134 -0.026501 347.694 0.0257633 -0.0102489 0.999616 -2.53207\n-0.071563 -0.997397 -0.00881629 77.6835 0.997089 -0.0713023 -0.0270007 346.694 0.0263018 -0.0107229 0.999597 -2.5318\n-0.0637212 -0.997921 -0.00962069 77.7356 0.997646 -0.0634528 -0.0260138 345.683 0.0253493 -0.0112557 0.999615 -2.53315\n-0.0478702 -0.998769 -0.0129838 77.8205 0.998565 -0.0475399 -0.0246557 343.685 0.0240081 -0.0141454 0.999612 -2.53759\n-0.0405807 -0.999057 -0.0154239 77.8637 0.998871 -0.0401821 -0.0253296 342.676 0.0246859 -0.0164344 0.99956 -2.54184\n-0.0340712 -0.999272 -0.0171392 77.8761 0.999072 -0.0336025 -0.0269293 341.662 0.0263337 -0.0180409 0.99949 -2.54269\n-0.0268474 -0.999447 -0.0196206 77.8848 0.999264 -0.0262942 -0.0279252 340.654 0.0273939 -0.0203559 0.999417 -2.54688\n-0.0206387 -0.999544 -0.0220614 77.8869 0.999431 -0.0200377 -0.0271255 339.652 0.026671 -0.0226087 0.999389 -2.5506\n-0.0164182 -0.999523 -0.0261512 77.8942 0.999548 -0.0157482 -0.0256233 338.619 0.0251992 -0.02656 0.99933 -2.55862\n-0.0154717 -0.999465 -0.0288287 77.9024 0.99961 -0.0147909 -0.0236838 337.593 0.0232447 -0.0291839 0.999304 -2.54716\n-0.0160315 -0.999442 -0.0293184 77.9099 0.999636 -0.0153846 -0.022157 336.575 0.0216936 -0.0296629 0.999325 -2.54889\n-0.015007 -0.999478 -0.0286037 77.9237 0.999634 -0.0143531 -0.0229282 335.557 0.0225056 -0.0289373 0.999328 -2.54075\n-0.0137843 -0.99957 -0.0258692 77.9321 0.999629 -0.0131683 -0.0238342 334.534 0.0234833 -0.0261882 0.999381 -2.54097\n-0.0124689 -0.999641 -0.0237056 77.9413 0.999663 -0.0119226 -0.0230483 333.511 0.0227574 -0.023985 0.999453 -2.53671\n-0.0107725 -0.999685 -0.0226591 77.9564 0.999725 -0.0102957 -0.0210554 332.49 0.0208155 -0.0228797 0.999522 -2.53389\n-0.00983103 -0.9997 -0.0224498 77.9548 0.999753 -0.00937896 -0.0201542 331.463 0.0199376 -0.0226424 0.999545 -2.5311\n-0.00852838 -0.999732 -0.021506 77.9673 0.999741 -0.008071 -0.0212653 330.432 0.0210861 -0.0216818 0.999543 -2.52875\n-0.007195 -0.999768 -0.0202926 77.9654 0.999704 -0.0067202 -0.0233697 329.399 0.0232279 -0.0204547 0.999521 -2.5234\n-0.00565489 -0.99979 -0.019677 77.9664 0.999696 -0.00517997 -0.0241037 328.364 0.0239967 -0.0198074 0.999516 -2.52937\n-0.00423743 -0.999824 -0.0182669 77.9691 0.999692 -0.00378902 -0.024513 327.334 0.0244395 -0.0183652 0.999533 -2.51819\n-0.00308647 -0.999877 -0.0153849 77.9722 0.99971 -0.00271747 -0.0239479 326.304 0.0239032 -0.0154543 0.999595 -2.51948\n-0.00177117 -0.999895 -0.0143536 77.9688 0.999757 -0.0014552 -0.0219941 325.26 0.0219709 -0.0143891 0.999655 -2.51989\n-0.000770487 -0.999915 -0.012992 77.9715 0.999785 -0.000501159 -0.0207208 324.225 0.0207125 -0.0130052 0.999701 -2.51611\n0.000551288 -0.999938 -0.0110945 77.9679 0.999768 0.000789916 -0.0215158 323.19 0.0215233 -0.0110801 0.999707 -2.51298\n0.00189445 -0.999936 -0.0111472 77.9555 0.999746 0.00214415 -0.0224314 322.149 0.0224539 -0.0111019 0.999686 -2.52332\n0.00330443 -0.999924 -0.011855 77.9518 0.999726 0.0035779 -0.0231213 321.103 0.023162 -0.0117753 0.999662 -2.52295\n0.00491267 -0.999923 -0.0113579 77.9428 0.999718 0.00517505 -0.0231877 320.058 0.0232447 -0.0112408 0.999667 -2.51317\n0.00597444 -0.99993 -0.0102345 77.9353 0.999714 0.00620954 -0.0230959 319.018 0.0231578 -0.0100936 0.999681 -2.51218\n0.00685998 -0.999936 -0.00903342 77.9237 0.99966 0.00708491 -0.0251069 317.975 0.0251692 -0.00885811 0.999644 -2.52139\n0.00819379 -0.99994 -0.00728232 77.9109 0.999535 0.00840388 -0.029302 316.924 0.0293615 -0.00703884 0.999544 -2.51864\n0.00936092 -0.999935 -0.00647068 77.8974 0.999448 0.00956219 -0.0318067 315.873 0.0318666 -0.00616937 0.999473 -2.53255\n0.0108795 -0.999918 -0.006737 77.8817 0.999435 0.0110881 -0.0317396 314.818 0.0318117 -0.00638788 0.999473 -2.55392\n0.0121971 -0.999887 -0.008819 77.8668 0.999477 0.0124554 -0.0298524 313.768 0.0299589 -0.00845027 0.999515 -2.55871\n0.0130645 -0.999862 -0.0102371 77.8397 0.999482 0.0133593 -0.0292774 312.714 0.0294102 -0.00984926 0.999519 -2.56884\n0.0141428 -0.999828 -0.0119848 77.8177 0.999374 0.0145231 -0.0322675 311.655 0.032436 -0.011521 0.999407 -2.57359\n0.0150343 -0.999805 -0.0127651 77.7851 0.999363 0.0154385 -0.0321749 310.59 0.0323657 -0.0122732 0.999401 -2.57813\n0.014588 -0.999787 -0.0146131 77.7691 0.999528 0.0149762 -0.026819 309.532 0.0270321 -0.0142149 0.999533 -2.57513\n0.0128707 -0.99981 -0.0146336 77.7548 0.99967 0.0131914 -0.0220325 308.477 0.0222213 -0.0143452 0.99965 -2.57055\n0.0114738 -0.999844 -0.0134509 77.7424 0.999729 0.0117429 -0.020108 307.41 0.0202628 -0.0132166 0.999707 -2.56682\n0.00859172 -0.999899 -0.0113323 77.7271 0.999694 0.0088519 -0.0231128 305.284 0.0232108 -0.0111302 0.999669 -2.56198\n0.00726482 -0.999918 -0.0105422 77.7067 0.999682 0.00751679 -0.024061 304.223 0.0241383 -0.0103641 0.999655 -2.56233\n0.00607466 -0.999925 -0.0105941 77.7028 0.999707 0.0063208 -0.0233572 303.157 0.0234224 -0.0104491 0.999671 -2.5582\n0.00440251 -0.999932 -0.0107926 77.6905 0.999747 0.0046394 -0.0220233 302.087 0.0220719 -0.0106929 0.999699 -2.56195\n0.00330843 -0.999944 -0.0100453 77.6917 0.999775 0.00351787 -0.0209038 301.025 0.020938 -0.00997385 0.999731 -2.55605\n0.00199909 -0.999947 -0.0100833 77.6881 0.999794 0.00220238 -0.0201911 299.954 0.0202122 -0.0100408 0.999745 -2.55211\n0.00094548 -0.999954 -0.00950133 77.6902 0.999782 0.00114362 -0.0208707 298.887 0.0208806 -0.00947952 0.999737 -2.55191\n-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\n-0.0010863 -0.99996 -0.008851 77.69 0.999762 -0.000893301 -0.02178 296.771 0.0217713 -0.00887256 0.999724 -2.54419\n-0.00214233 -0.999935 -0.0111656 77.6824 0.999762 -0.00189916 -0.0217441 295.716 0.0217214 -0.0112095 0.999701 -2.5474\n-0.00304246 -0.999931 -0.0113522 77.6831 0.999742 -0.00278606 -0.0225336 294.667 0.0225004 -0.0114179 0.999682 -2.54206\n-0.00333048 -0.999918 -0.0123516 77.6897 0.999722 -0.00304098 -0.0233839 293.619 0.0233444 -0.0124261 0.99965 -2.54572\n-0.00374452 -0.999895 -0.0139771 77.687 0.999725 -0.00341963 -0.0231966 292.57 0.0231463 -0.0140601 0.999633 -2.53511\n-0.00418725 -0.999881 -0.014821 77.6855 0.999727 -0.0038453 -0.023026 291.547 0.0229663 -0.0149134 0.999625 -2.53457\n-0.00391913 -0.999878 -0.0150952 77.6841 0.99974 -0.00357847 -0.0225286 290.509 0.0224719 -0.0151796 0.999632 -2.53166\n-0.00408805 -0.999861 -0.0161882 77.6873 0.999756 -0.00373517 -0.0217692 289.453 0.0217057 -0.0162732 0.999632 -2.52449\n-0.00434718 -0.999841 -0.0172995 77.6837 0.999775 -0.00398648 -0.0208304 288.412 0.0207581 -0.0173862 0.999633 -2.52223\n-0.00462051 -0.99982 -0.0183885 77.6833 0.99976 -0.0042251 -0.0214845 287.376 0.021403 -0.0184833 0.9996 -2.52203\n-0.00483576 -0.999776 -0.0206152 77.6914 0.999729 -0.00436384 -0.0228761 286.34 0.022781 -0.0207203 0.999526 -2.52099\n-0.00514836 -0.99974 -0.0221936 77.6921 0.999692 -0.00460654 -0.0243956 285.308 0.024287 -0.0223123 0.999456 -2.51142\n-0.00524535 -0.999712 -0.0234345 77.693 0.999688 -0.00466974 -0.0245499 284.283 0.0244334 -0.023556 0.999424 -2.50737\n-0.00506662 -0.999721 -0.0230816 77.6961 0.999706 -0.0045168 -0.0238108 283.262 0.0236999 -0.0231954 0.99945 -2.50166\n-0.00443966 -0.999734 -0.0226367 77.6996 0.999756 -0.00394723 -0.0217523 282.242 0.0216571 -0.0227278 0.999507 -2.49872\n-0.00328106 -0.99978 -0.0207114 77.6898 0.999788 -0.00285839 -0.0204044 281.223 0.0203408 -0.020774 0.999577 -2.49248\n-0.00180788 -0.999806 -0.0196311 77.6909 0.999809 -0.00142519 -0.0194905 280.21 0.0194588 -0.0196626 0.999617 -2.49224\n-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\n0.00112993 -0.999824 -0.0187242 77.6954 0.999799 0.00150415 -0.0199838 278.165 0.0200085 -0.0186979 0.999625 -2.47933\n0.00246135 -0.999818 -0.0189421 77.6875 0.999792 0.00284407 -0.0202041 277.151 0.0202543 -0.0188884 0.999616 -2.47499\n0.00393358 -0.999788 -0.0202274 77.6778 0.999772 0.00435604 -0.0208839 276.124 0.0209676 -0.0201407 0.999577 -2.47111\n0.00537425 -0.999768 -0.0208737 77.6669 0.999752 0.0058231 -0.0215021 275.1 0.0216187 -0.0207529 0.999551 -2.46339\n0.00668459 -0.999729 -0.0222945 77.6481 0.999742 0.00716508 -0.0215419 274.086 0.0216958 -0.0221448 0.999519 -2.46401\n0.00789368 -0.99966 -0.0248643 77.6341 0.999734 0.00842822 -0.0214674 273.06 0.0216697 -0.0246882 0.99946 -2.46375\n0.00846454 -0.999609 -0.0266323 77.6191 0.999723 0.00904396 -0.0217119 272.049 0.0219443 -0.0264411 0.999409 -2.456\n0.00928856 -0.999585 -0.0272631 77.6126 0.999715 0.00988273 -0.0217406 271.031 0.022001 -0.0270534 0.999392 -2.45476\n0.00952639 -0.999567 -0.0278431 77.5918 0.999708 0.010139 -0.0219448 270.015 0.0222176 -0.0276259 0.999371 -2.45076\n0.010134 -0.999561 -0.0278342 77.5779 0.999702 0.0107454 -0.0219042 269.013 0.0221936 -0.0276039 0.999373 -2.44886\n0.0104319 -0.999579 -0.0270845 77.5673 0.99969 0.0110378 -0.0223205 268.005 0.02261 -0.0268433 0.999384 -2.43994\n0.0104301 -0.999633 -0.0250156 77.5641 0.999694 0.010985 -0.0221501 266.994 0.0224167 -0.0247769 0.999442 -2.43594\n0.0102613 -0.999687 -0.0228044 77.5453 0.999707 0.0107567 -0.0217049 265.992 0.0219434 -0.022575 0.999504 -2.42816\n0.0101114 -0.999738 -0.0205209 77.5389 0.999727 0.0105396 -0.0208663 264.987 0.0210772 -0.0203043 0.999572 -2.42293\n0.00935528 -0.999863 -0.0136437 77.5109 0.999761 0.0096221 -0.0196236 261.995 0.0197522 -0.0134569 0.999714 -2.40584\n0.00848855 -0.999865 -0.0140973 77.4862 0.9997 0.0088092 -0.0228408 260.023 0.0229618 -0.0138992 0.99964 -2.4047\n0.00816359 -0.999819 -0.0171633 77.4651 0.999666 0.00858114 -0.0243966 258.058 0.0245395 -0.0169584 0.999555 -2.3992\n0.00744734 -0.99977 -0.020102 77.4289 0.999719 0.00789668 -0.0223667 255.134 0.0225203 -0.0199298 0.999548 -2.39203\n0.00748775 -0.999732 -0.0218867 77.4081 0.999707 0.0079878 -0.02285 253.203 0.0230187 -0.0217092 0.999499 -2.37969\n0.00765939 -0.999712 -0.0227327 77.3854 0.999758 0.0081244 -0.0204339 251.29 0.0206127 -0.0225707 0.999533 -2.37363\n0.00807064 -0.999677 -0.0240829 77.3529 0.99976 0.00855718 -0.0201685 248.431 0.0203681 -0.0239143 0.999507 -2.35166\n0.00801365 -0.9997 -0.0231553 77.3396 0.999793 0.00844363 -0.0185317 246.558 0.0187216 -0.023002 0.99956 -2.33998\n0.00687925 -0.99974 -0.0217402 77.3313 0.9998 0.0072847 -0.0186259 244.68 0.0187794 -0.0216077 0.99959 -2.31825\n0.00606643 -0.999795 -0.0193033 77.321 0.999813 0.00641839 -0.0182237 242.818 0.0183439 -0.0191892 0.999648 -2.29757\n0.00458385 -0.999852 -0.0165963 77.3107 0.999794 0.00491079 -0.019713 240.993 0.0197916 -0.0165026 0.999668 -2.28307\n0.00354496 -0.99988 -0.0150635 77.3049 0.99975 0.00387614 -0.0220135 239.182 0.0220693 -0.0149817 0.999644 -2.27374\n0.00272259 -0.999906 -0.0134039 77.2909 0.999683 0.00305707 -0.0249973 237.414 0.0250359 -0.0133316 0.999598 -2.26431\n0.00215047 -0.999914 -0.0129649 77.292 0.999729 0.00245016 -0.0231437 235.704 0.0231735 -0.0129116 0.999648 -2.24571\n0.00157132 -0.999904 -0.0137835 77.29 0.99977 0.00186569 -0.0213698 234.038 0.0213934 -0.0137468 0.999677 -2.23732\n0.00027616 -0.999878 -0.0156115 77.2797 0.99968 0.000670819 -0.0252805 232.412 0.0252878 -0.0155995 0.999558 -2.21974\n-0.000188023 -0.999886 -0.0151228 77.2725 0.999701 0.000181722 -0.0244445 230.847 0.0244444 -0.0151229 0.999587 -2.21229\n-0.00121107 -0.999867 -0.0162705 77.2736 0.999712 -0.00082056 -0.0239862 229.321 0.0239697 -0.0162949 0.99958 -2.20594\n-0.00133846 -0.999851 -0.0172051 77.2722 0.999707 -0.000922051 -0.0241878 227.856 0.0241684 -0.0172325 0.999559 -2.18813\n-0.00196232 -0.999844 -0.0175695 77.273 0.999785 -0.00159924 -0.0206557 226.443 0.0206244 -0.0176062 0.999632 -2.18417\n-0.00323535 -0.999809 -0.0192948 77.2793 0.999767 -0.00282233 -0.0213948 224.425 0.0213363 -0.0193595 0.999585 -2.16528\n-0.0035245 -0.999801 -0.0196123 77.2846 0.99986 -0.00320239 -0.0164312 222.491 0.0163651 -0.0196675 0.999673 -2.14777\n-0.00395744 -0.999777 -0.0207659 77.2781 0.999868 -0.00362904 -0.0158281 221.213 0.0157492 -0.0208258 0.999659 -2.13098\n-0.0039645 -0.999818 -0.0186831 77.2845 0.999915 -0.00373182 -0.0124727 219.954 0.0124007 -0.018731 0.999748 -2.09597\n-0.00338336 -0.999751 -0.0220557 77.2783 0.999845 -0.00376257 0.0171748 218.75 -0.0172535 -0.0219942 0.999609 -2.05208\n-0.00308621 -0.999755 -0.0219032 77.2808 0.999924 -0.00334661 0.011862 217.506 -0.0119324 -0.0218649 0.99969 -2.02178\n-0.00213508 -0.999902 -0.0138526 77.2777 0.997636 -0.00117825 -0.0687161 215.551 0.068693 -0.0139666 0.99754 -2.07096\n0.00101863 -0.999876 -0.0157114 77.2644 0.999456 0.00153604 -0.0329548 214.35 0.0329748 -0.0156693 0.999333 -2.0219\n0.00301775 -0.999911 -0.0130051 77.2688 0.999992 0.00305275 -0.00267216 212.497 0.00271162 -0.0129969 0.999912 -2.04819\n0.00398228 -0.99991 -0.012803 77.2649 0.999707 0.00428657 -0.0238282 211.203 0.023881 -0.0127044 0.999634 -2.04674\n0.00559183 -0.999854 -0.0161407 77.2378 0.999874 0.0058306 -0.014784 209.307 0.014876 -0.016056 0.99976 -2.01464\n0.00630108 -0.999827 -0.017522 77.2254 0.999877 0.00655076 -0.014229 207.374 0.0143413 -0.0174302 0.999745 -1.99572\n0.00687067 -0.999855 -0.0155777 77.2117 0.999838 0.00712824 -0.0165396 206.069 0.0166483 -0.0154615 0.999742 -1.99018\n0.00717094 -0.999836 -0.0166515 77.1939 0.999863 0.00741771 -0.0148054 204.776 0.0149265 -0.0165431 0.999752 -1.96916\n0.00800318 -0.999889 -0.0125903 77.1984 0.999856 0.00818995 -0.0148535 202.823 0.014955 -0.0124696 0.99981 -1.95542\n0.00839418 -0.999864 -0.0141771 77.1736 0.999849 0.00860789 -0.0150806 201.533 0.0152005 -0.0140483 0.999786 -1.94254\n0.00923233 -0.99979 -0.0183161 77.1409 0.999837 0.00951429 -0.0153674 200.237 0.0155384 -0.0181712 0.999714 -1.93343\n0.0100737 -0.99975 -0.0199685 77.1143 0.999809 0.0104047 -0.0165378 198.945 0.0167414 -0.019798 0.999664 -1.91722\n0.0106988 -0.999576 -0.0270807 77.0865 0.999848 0.0110661 -0.0134512 197.008 0.0137452 -0.0269327 0.999543 -1.91456\n0.0116676 -0.999499 -0.029414 77.0564 0.999849 0.0120396 -0.0125018 195.712 0.0128497 -0.0292637 0.999489 -1.88405\n0.0123595 -0.999393 -0.0325708 77.0418 0.999857 0.0127275 -0.0111169 194.428 0.0115247 -0.0324287 0.999408 -1.87379\n0.0125653 -0.999265 -0.0362099 77.0198 0.99986 0.0129579 -0.0106279 193.15 0.0110893 -0.0360712 0.999288 -1.85042\n0.0104152 -0.999248 -0.037336 76.9951 0.999884 0.0108223 -0.0107208 191.866 0.0111168 -0.03722 0.999245 -1.82556\n0.0078466 -0.999284 -0.0370019 76.9979 0.999892 0.00829977 -0.0121096 190.597 0.012408 -0.0369029 0.999242 -1.80566\n0.00540088 -0.999345 -0.0357705 76.9818 0.999899 0.00586737 -0.0129493 189.347 0.0131507 -0.0356969 0.999276 -1.79238\n0.00250684 -0.999368 -0.0354472 76.9789 0.999895 0.00301132 -0.0141856 188.095 0.0142834 -0.035408 0.999271 -1.77014\n0.00050963 -0.999465 -0.0326998 76.9824 0.999862 0.00105234 -0.0165817 186.858 0.0166073 -0.0326869 0.999328 -1.75371\n-0.00321276 -0.999241 -0.0388317 76.9618 0.99972 -0.00229971 -0.0235348 185.031 0.0234276 -0.0388964 0.998969 -1.74105\n-0.00241841 -0.999706 -0.0241363 76.9851 0.999973 -0.00224954 -0.00702154 183.848 0.00696518 -0.0241526 0.999684 -1.6631\n-0.00441883 -0.999736 -0.0225697 77.0081 0.999623 -0.00502734 0.0269761 182.665 -0.0270824 -0.022442 0.999381 -1.67819\n-0.00610494 -0.999721 -0.0228271 76.9978 0.999979 -0.0061498 0.00189552 181.464 -0.00203537 -0.022815 0.999738 -1.67164\n-0.00692608 -0.999825 -0.0173725 77.0227 0.997925 -0.00579877 -0.0641214 180.204 0.0640095 -0.0177806 0.997791 -1.70553\n-0.00762631 -0.999835 -0.0164917 77.0259 0.998771 -0.00680852 -0.0490883 179.037 0.0489679 -0.0168458 0.998658 -1.6277\n-0.00887686 -0.99973 -0.0214817 77.04 0.99996 -0.00889223 0.000620069 177.877 -0.000810921 -0.0214754 0.999769 -1.66133\n-0.00881866 -0.999803 -0.0177663 77.0523 0.999959 -0.00878388 -0.00203466 176.704 0.0018782 -0.0177835 0.99984 -1.66129\n-0.00982114 -0.999881 -0.0119087 77.0645 0.999775 -0.00959491 -0.0189077 175.499 0.0187912 -0.0120917 0.99975 -1.6381\n-0.00950457 -0.999854 -0.0142335 77.0805 0.999925 -0.00939373 -0.00783388 173.722 0.00769903 -0.0143069 0.999868 -1.60948\n-0.00775376 -0.999897 -0.0121038 77.0965 0.999921 -0.00763324 -0.00997165 172.504 0.00987823 -0.0121802 0.999877 -1.59614\n-0.00608816 -0.999918 -0.0112355 77.0934 0.999892 -0.00593703 -0.0134366 171.289 0.0133688 -0.011316 0.999847 -1.58848\n-0.00427731 -0.999918 -0.0120503 77.0961 0.999911 -0.00412474 -0.0126577 170.073 0.012607 -0.0121034 0.999847 -1.57266\n-0.00217378 -0.999938 -0.0109343 77.0915 0.99993 -0.00204666 -0.0116236 168.851 0.0116005 -0.0109588 0.999873 -1.55685\n4.44453e-05 -0.999952 -0.00982362 77.0913 0.999933 0.000158526 -0.0116124 167.614 0.0116134 -0.00982245 0.999884 -1.53446\n0.00298951 -0.999968 -0.00737329 77.0777 0.999939 0.00306764 -0.0106079 165.735 0.0106302 -0.00734113 0.999917 -1.51258\n0.00534003 -0.999964 -0.00657882 77.0685 0.999925 0.00541239 -0.0110296 164.466 0.0110648 -0.00651943 0.999918 -1.50273\n0.00665537 -0.999961 -0.00583244 77.0521 0.999903 0.00672631 -0.0122284 162.542 0.0122671 -0.00575049 0.999908 -1.47711\n0.00756373 -0.999958 -0.00516356 77.0449 0.999916 0.00761756 -0.0104868 161.27 0.0105257 -0.00508381 0.999932 -1.45601\n0.00810198 -0.999963 -0.00293174 77.0361 0.999923 0.00812913 -0.0093701 159.961 0.00939359 -0.00285559 0.999952 -1.44569\n0.00809083 -0.999952 -0.00553815 77.0082 0.999934 0.00813565 -0.0081205 158.651 0.00816516 -0.00547208 0.999952 -1.4333\n0.00930213 -0.999938 -0.00618911 76.9949 0.999927 0.00934907 -0.00759769 157.328 0.00765508 -0.00611798 0.999952 -1.41271\n0.00891361 -0.999916 -0.00936526 76.9739 0.999924 0.00899274 -0.0084417 155.983 0.00852522 -0.0092893 0.999921 -1.39682\n0.0102411 -0.999862 -0.013086 76.9458 0.999909 0.0103548 -0.00864999 154.645 0.0087843 -0.0129963 0.999877 -1.38078\n0.0105552 -0.99982 -0.0157472 76.9215 0.999905 0.010694 -0.00875213 153.287 0.00891895 -0.0156533 0.999838 -1.356\n0.0103343 -0.999773 -0.0186551 76.9059 0.999907 0.0104991 -0.00875502 151.929 0.00894889 -0.0185628 0.999788 -1.3297\n0.00856756 -0.999754 -0.0204626 76.8799 0.99992 0.00875652 -0.00916236 150.547 0.00933928 -0.0203825 0.999749 -1.30854\n0.00791173 -0.999757 -0.0205833 76.877 0.999932 0.00808524 -0.00836026 149.154 0.00852465 -0.0205158 0.999753 -1.28849\n0.00714166 -0.999742 -0.0215723 76.8662 0.999938 0.00732411 -0.00839022 147.768 0.00854605 -0.0215111 0.999732 -1.26564\n0.00584244 -0.99977 -0.0206437 76.8481 0.999932 0.00604999 -0.0100059 146.396 0.0101285 -0.0205839 0.999737 -1.23597\n0.00509275 -0.999765 -0.0210543 76.8432 0.999927 0.00532142 -0.0108193 145.012 0.0109288 -0.0209977 0.99972 -1.22428\n0.00467827 -0.999749 -0.0219005 76.8293 0.99993 0.00491397 -0.0107209 143.675 0.0108258 -0.0218489 0.999703 -1.18572\n0.00348473 -0.999767 -0.0213129 76.8246 0.999949 0.00368557 -0.00939161 142.315 0.00946797 -0.0212791 0.999729 -1.17372\n0.00362105 -0.99979 -0.0201895 76.8288 0.999925 0.00338399 0.0117636 140.995 -0.0116928 -0.0202306 0.999727 -1.10212\n0.00310431 -0.999856 -0.0166735 76.8305 0.999202 0.00243765 0.0398554 139.687 -0.039809 -0.016784 0.999066 -1.05493\n0.00111884 -0.999755 -0.0221053 76.8017 0.9999 0.00142994 -0.0140626 138.26 0.0140908 -0.0220874 0.999657 -1.11893\n0.00211985 -0.999866 -0.0162399 76.8026 0.997568 0.00324586 -0.0696266 136.878 0.06967 -0.0160528 0.997441 -1.0723\n0.00342259 -0.999795 -0.0199346 76.7985 0.999988 0.0033539 0.00347811 135.596 -0.00341054 -0.0199462 0.999795 -1.00954\n0.00342297 -0.999699 -0.0243045 76.7838 0.999803 0.00294654 0.0196113 134.233 -0.0195338 -0.0243668 0.999512 -1.05754\n0.00326913 -0.999806 -0.019429 76.7802 0.999505 0.00387468 -0.0312118 132.782 0.0312811 -0.0193173 0.999324 -1.02096\n0.00480846 -0.999861 -0.0159625 76.7831 0.999886 0.00503574 -0.0142285 131.416 0.014307 -0.0158922 0.999771 -0.958972\n0.00500543 -0.999916 -0.0119499 76.7795 0.999964 0.00492389 0.00684345 130.016 -0.00678404 -0.0119838 0.999905 -0.947008\n0.00567963 -0.999965 -0.00611092 76.768 0.999979 0.00569823 -0.00303156 128.581 0.00306627 -0.00609357 0.999977 -0.925478\n0.00676407 -0.999968 -0.00416883 76.7502 0.999947 0.00679602 -0.00769915 126.424 0.00772724 -0.00411653 0.999962 -0.893155\n0.00743789 -0.999958 -0.00530489 76.7405 0.999959 0.00746494 -0.00509902 124.971 0.0051384 -0.00526675 0.999973 -0.871771\n0.00851406 -0.999931 -0.00813882 76.7228 0.999945 0.00856392 -0.00611105 123.518 0.00618032 -0.00808634 0.999948 -0.853574\n0.00874952 -0.999873 -0.0133538 76.6997 0.99992 0.00887052 -0.00902854 122.046 0.00914585 -0.0132737 0.99987 -0.823478\n0.01226 -0.999776 -0.017263 76.6747 0.999874 0.0124315 -0.00986452 120.566 0.0100769 -0.0171399 0.999802 -0.800444\n0.0157842 -0.999663 -0.0206075 76.6406 0.999836 0.0159623 -0.00850672 119.078 0.0088328 -0.0204699 0.999751 -0.777816\n0.020011 -0.999563 -0.0217734 76.5893 0.999756 0.0202083 -0.00887937 117.591 0.00931549 -0.0215904 0.999724 -0.754268\n0.0273239 -0.999428 -0.0199447 76.5343 0.999579 0.0275124 -0.00923826 116.087 0.0097817 -0.0196839 0.999758 -0.709358\n0.0361206 -0.999187 -0.0178926 76.4846 0.9993 0.0362874 -0.0090857 114.55 0.00972759 -0.0175519 0.999799 -0.708153\n0.0463911 -0.99878 -0.0168969 76.3986 0.99886 0.0465721 -0.0104793 113.021 0.0112534 -0.0163915 0.999802 -0.675391\n0.0614742 -0.998011 -0.0139466 76.2998 0.998074 0.0615821 -0.00744405 111.474 0.0082881 -0.0134622 0.999875 -0.652695\n0.0786596 -0.996798 -0.0143611 76.1493 0.996889 0.0787233 -0.00392391 109.91 0.0050419 -0.0140078 0.999889 -0.636415\n0.097282 -0.99512 -0.01648 75.9967 0.995243 0.0973542 -0.00363372 108.339 0.00522038 -0.0160481 0.999858 -0.604647\n0.115527 -0.993138 -0.0181944 75.7897 0.993278 0.115638 -0.00514208 106.77 0.00721076 -0.0174781 0.999821 -0.581943\n0.135002 -0.990666 -0.0188392 75.5556 0.990811 0.135131 -0.00573671 105.191 0.00822892 -0.0178916 0.999806 -0.561349\n0.164944 -0.98612 -0.0189731 75.1489 0.986268 0.16507 -0.00525172 102.812 0.00831071 -0.0178463 0.999806 -0.516322\n0.185547 -0.982489 -0.016942 74.848 0.982596 0.185667 -0.00577122 101.202 0.00881574 -0.0155764 0.99984 -0.485731\n0.207823 -0.978081 -0.0129232 74.4978 0.978141 0.207894 -0.00441144 99.6098 0.0070014 -0.0117239 0.999907 -0.460205\n0.242103 -0.97019 -0.0108164 73.8905 0.970239 0.242139 -0.00207422 97.1983 0.00463145 -0.00999229 0.999939 -0.42327\n0.265029 -0.964177 -0.0110147 73.4266 0.964223 0.265076 -0.0030075 95.5887 0.0058195 -0.00982355 0.999935 -0.389805\n0.288111 -0.957523 -0.0119224 72.9251 0.957582 0.288153 -0.00195357 93.9901 0.00530606 -0.0108538 0.999927 -0.367862\n0.324857 -0.945644 -0.0150238 72.1015 0.945742 0.324916 -0.00159461 91.5928 0.0063894 -0.0136906 0.999886 -0.323403\n0.349882 -0.93664 -0.0169914 71.4782 0.936759 0.349968 -0.00225339 90.0093 0.00805705 -0.0151284 0.999853 -0.291088\n0.373958 -0.927279 -0.0175649 70.8153 0.927413 0.374036 -0.00128144 88.4258 0.00775818 -0.0158108 0.999845 -0.264737\n0.397941 -0.917228 -0.0183195 70.124 0.91737 0.398032 -0.00145428 86.8486 0.00862565 -0.016227 0.999831 -0.239185\n0.416889 -0.908705 -0.0214107 69.3561 0.908907 0.416998 -0.000708236 85.2716 0.00957179 -0.019165 0.999771 -0.222278\n0.432533 -0.901309 -0.0235981 68.5863 0.901564 0.432645 0.000385218 83.6924 0.00986238 -0.0214418 0.999721 -0.185196\n0.448017 -0.89376 -0.0217534 67.7852 0.89398 0.448106 0.000858263 82.1176 0.00898077 -0.0198317 0.999763 -0.155278\n0.468122 -0.883386 -0.0221516 66.5324 0.883596 0.468249 -0.000627634 79.7609 0.0109269 -0.0192792 0.999754 -0.121272\n0.478356 -0.877927 -0.0204904 65.6684 0.878092 0.478488 -0.00179128 78.1917 0.011377 -0.0171356 0.999788 -0.0897987\n0.493519 -0.869554 -0.0177662 64.324 0.869662 0.49364 -0.00289981 75.8599 0.0112917 -0.0140195 0.999838 -0.0673291\n0.500881 -0.865235 -0.0220387 63.4057 0.865397 0.501072 -0.00380475 74.3053 0.014335 -0.0171665 0.99975 -0.0484548\n0.50561 -0.862446 -0.0233595 62.4833 0.862643 0.505805 -0.00293088 72.768 0.0143431 -0.018669 0.999723 -0.020998\n0.509166 -0.860384 -0.0221238 61.5823 0.860565 0.509335 -0.00243099 71.2586 0.01336 -0.0178012 0.999752 0.00165494\n0.513597 -0.857746 -0.0221317 60.2176 0.857957 0.513722 5.06427e-05 68.9963 0.0113261 -0.019014 0.999755 0.0275294\n0.515076 -0.85677 -0.0253233 58.8324 0.857092 0.515146 0.00419697 66.7247 0.00944938 -0.0238662 0.999671 0.0716462\n0.5139 -0.857422 -0.0271025 57.9184 0.857791 0.51398 0.00446287 65.2178 0.0101036 -0.0255417 0.999623 0.100878\n0.508519 -0.860648 -0.0263457 56.5697 0.861002 0.508578 0.00488786 62.9378 0.00919213 -0.0251693 0.999641 0.146803\n0.503592 -0.863468 -0.028595 55.6709 0.863881 0.503674 0.00479937 61.418 0.0102584 -0.0271196 0.99958 0.169365\n0.497845 -0.866746 -0.0300309 54.7935 0.867191 0.497958 0.00410653 59.8935 0.0113948 -0.028087 0.999541 0.190018\n0.490921 -0.870686 -0.0300474 53.9414 0.871118 0.491066 0.00283729 58.3675 0.0122849 -0.0275677 0.999544 0.224363\n0.483789 -0.874644 -0.0307686 53.0782 0.875073 0.48399 0.00102184 56.8215 0.013998 -0.0274191 0.999526 0.244076\n0.476666 -0.878557 -0.0304693 52.2505 0.878996 0.476823 0.00233145 55.3138 0.0124802 -0.0278937 0.999533 0.277053\n0.468845 -0.882809 -0.0288515 51.4356 0.883234 0.468906 0.00504158 53.8008 0.00907789 -0.0278463 0.999571 0.299324\n0.460651 -0.887101 -0.0291975 50.6501 0.887541 0.460693 0.00568708 52.2915 0.00840605 -0.0285337 0.999557 0.331645\n0.452199 -0.891396 -0.0304811 49.8834 0.89187 0.452264 0.0051216 50.7905 0.00922011 -0.0295011 0.999522 0.361217\n0.44254 -0.896202 -0.0313275 49.1408 0.896697 0.442619 0.0047356 49.2772 0.00962208 -0.0301869 0.999498 0.392611\n0.432104 -0.901261 -0.0318496 48.4021 0.901777 0.43217 0.0051248 47.7812 0.00914566 -0.0309357 0.99948 0.418435\n0.419857 -0.906827 -0.0372202 47.7151 0.907521 0.419978 0.00487205 46.2648 0.0112136 -0.0358236 0.999295 0.432287\n0.405798 -0.913283 -0.0352537 47.0518 0.913916 0.405865 0.00554468 44.7691 0.00924438 -0.0344689 0.999363 0.478146\n0.389849 -0.92021 -0.0350869 46.4222 0.920852 0.389844 0.00727948 43.2735 0.00697974 -0.0351477 0.999358 0.496709\n0.374438 -0.926521 -0.0368239 45.8192 0.927157 0.374673 0.000555308 41.7727 0.0132824 -0.0343495 0.999322 0.536541\n0.357282 -0.933377 -0.0340069 45.2564 0.933924 0.357471 0.000551027 40.2672 0.0116422 -0.0319567 0.999421 0.567374\n0.329622 -0.943664 -0.0291061 44.4739 0.944087 0.329683 0.00281222 38.011 0.00694199 -0.0284056 0.999572 0.61181\n0.309878 -0.95037 -0.0278069 44.0013 0.950739 0.309994 0.000145059 36.5142 0.00848212 -0.0264821 0.999613 0.638011\n0.289977 -0.956707 -0.0250132 43.563 0.956979 0.29014 -0.00306542 35.0211 0.0101901 -0.0230482 0.999682 0.676079\n0.271052 -0.962306 -0.0223289 43.1779 0.962509 0.271213 -0.00446006 33.5612 0.0103478 -0.0202829 0.999741 0.709732\n0.252716 -0.967303 -0.0214456 42.8091 0.967486 0.252875 -0.00501783 32.1318 0.0102768 -0.0194802 0.999757 0.732235\n0.223298 -0.974507 -0.0217868 42.3357 0.974718 0.223416 -0.00312043 30.0319 0.00790839 -0.0205392 0.999758 0.776278\n0.202469 -0.979066 -0.0208689 42.0737 0.979269 0.202554 -0.00201644 28.6751 0.00620129 -0.020028 0.99978 0.808764\n0.182783 -0.982977 -0.0185887 41.8416 0.983139 0.182849 -0.00187413 27.3224 0.00524115 -0.0179327 0.999825 0.835432\n0.164776 -0.986153 -0.0187373 41.63 0.986301 0.164889 -0.00459621 25.9853 0.00762213 -0.0177233 0.999814 0.861815\n0.151324 -0.988321 -0.0179762 41.4406 0.988452 0.151442 -0.00533926 24.6682 0.00799924 -0.0169607 0.999824 0.883728\n0.137837 -0.990125 -0.0255654 41.1685 0.990287 0.137292 0.0219911 22.7522 -0.018264 -0.0283483 0.999431 0.986493\n0.130435 -0.99124 -0.0207562 40.9349 0.991427 0.130564 -0.0049721 20.8642 0.00763855 -0.0199298 0.999772 0.987179\n0.128538 -0.991001 -0.0373492 40.7533 0.991273 0.129502 -0.0246504 19.6631 0.0292654 -0.0338548 0.998998 1.02352\n0.128773 -0.991272 -0.0282518 40.6091 0.99155 0.129155 -0.0121577 18.5076 0.0157005 -0.0264475 0.999527 1.08425\n0.12804 -0.991316 -0.029967 40.387 0.991768 0.127933 0.0054677 16.7709 -0.00158647 -0.0304203 0.999536 1.07384\n0.129748 -0.991052 -0.0313301 40.23 0.991546 0.129716 0.00307091 15.6329 0.00102059 -0.0314637 0.999504 1.13415\n0.13527 -0.990478 -0.0255981 40.0928 0.990796 0.135356 -0.00162176 14.5308 0.00507116 -0.0251431 0.999671 1.15863\n0.157913 -0.987065 -0.0276761 39.7495 0.987435 0.158018 -0.00161523 12.4967 0.00596766 -0.0270733 0.999616 1.21733\n0.186459 -0.982197 -0.0228594 39.4731 0.98246 0.18646 0.00211143 11.1122 0.00218853 -0.0228522 0.999736 1.2524\n0.221892 -0.974747 -0.0251385 39.1581 0.975029 0.222047 -0.00354769 9.86262 0.00904004 -0.0237236 0.999678 1.27867\n0.262037 -0.964756 -0.0241379 38.8266 0.965001 0.262212 -0.00431865 8.7627 0.0104957 -0.0221614 0.999699 1.31048\n0.320119 -0.947112 -0.0224388 38.3538 0.947329 0.320252 -0.00248511 7.51508 0.00953973 -0.0204614 0.999745 1.33455\n0.379867 -0.924755 -0.0230074 37.8526 0.925028 0.379877 0.00410622 6.45328 0.00494273 -0.0228423 0.999727 1.36452\n0.444482 -0.895522 -0.0218159 37.2909 0.895772 0.444485 0.00499342 5.46713 0.00522513 -0.0217616 0.99975 1.38479\n0.516001 -0.856305 -0.0220347 36.6286 0.856587 0.51587 0.0116739 4.5206 0.0013706 -0.0248984 0.999689 1.40531\n0.609001 -0.792857 -0.0222432 35.631 0.793098 0.609083 0.00367646 3.3574 0.010633 -0.01988 0.999746 1.43016\n0.692634 -0.720795 -0.0267057 34.6501 0.72114 0.692769 0.00529913 2.49369 0.0146813 -0.0229289 0.999629 1.43272\n0.755444 -0.654617 -0.0279553 33.7932 0.654983 0.755622 0.00569877 1.88139 0.0173931 -0.0226153 0.999593 1.42286\n0.815183 -0.578279 -0.0327081 32.8258 0.578798 0.815426 0.00863192 1.31942 0.0216793 -0.0259679 0.999428 1.43524\n0.870115 -0.49204 -0.028229 31.7665 0.492488 0.870242 0.0115884 0.812309 0.0188641 -0.0239857 0.999534 1.41142\n0.929033 -0.368643 -0.0316094 30.1415 0.369303 0.929129 0.0182706 0.282744 0.0226339 -0.0286474 0.999333 1.4208\n0.970354 -0.240048 -0.0281174 28.3324 0.24079 0.970206 0.0268339 -0.0808593 0.0208382 -0.0328088 0.999244 1.38706\n0.98976 -0.139569 -0.0299184 26.3059 0.140599 0.989425 0.0356424 -0.294155 0.0246275 -0.0394839 0.998917 1.38204\n0.993811 -0.105724 -0.0340968 25.2237 0.106971 0.99357 0.0370843 -0.378433 0.0299569 -0.0405021 0.99873 1.35984\n0.996554 -0.0757561 -0.0337883 24.131 0.0771373 0.99615 0.0416422 -0.419705 0.0305035 -0.044105 0.998561 1.36246\n0.998672 -0.0455504 -0.0240846 22.4247 0.0465672 0.997968 0.0434935 -0.473812 0.0220545 -0.0445573 0.998763 1.35578\n0.999065 -0.0328085 -0.0281722 21.2333 0.0340424 0.998429 0.0444988 -0.497782 0.026668 -0.0454162 0.998612 1.33661\n0.999213 -0.0217368 -0.0331692 20.0302 0.023092 0.998891 0.0410368 -0.52197 0.0322404 -0.0417705 0.998607 1.351\n0.999649 -0.00704273 -0.025546 18.158 0.00803381 0.999211 0.038903 -0.518597 0.0252519 -0.0390946 0.998916 1.32868\n0.999665 0.00201893 -0.0257875 16.1871 -0.000942563 0.99913 0.0416842 -0.502004 0.0258492 -0.0416459 0.998798 1.31278\n0.99962 0.00396251 -0.027263 14.1744 -0.00280707 0.999101 0.0422898 -0.48391 0.027406 -0.0421973 0.998733 1.29947\n0.99955 0.00197139 -0.0299438 12.1004 -0.000621763 0.998986 0.0450146 -0.470716 0.0300022 -0.0449758 0.998537 1.28795\n0.999648 0.000254519 -0.0265348 10.6803 0.000897076 0.999058 0.0433785 -0.465597 0.0265208 -0.0433871 0.998706 1.28363\n0.999646 -0.0015048 -0.0265717 9.23925 0.00263968 0.999083 0.0427271 -0.46407 0.0264831 -0.0427821 0.998733 1.26615\n0.999536 -0.00271179 -0.030355 7.77586 0.00399769 0.999093 0.0423817 -0.47213 0.0302125 -0.0424834 0.99864 1.26783\n0.999643 -0.0034889 -0.0264887 6.30045 0.00454262 0.999196 0.039825 -0.467842 0.0263285 -0.0399311 0.998856 1.26672\n0.999686 -0.00420077 -0.0247045 4.04938 0.00516984 0.999214 0.0392943 -0.472864 0.0245201 -0.0394097 0.998922 1.25279\n0.999633 -0.00521463 -0.0265927 1.7529 0.0062425 0.999231 0.0387168 -0.483443 0.0263704 -0.0388686 0.998896 1.24513\n0.999531 -0.00569562 -0.0300751 0.209662 0.00687771 0.999202 0.0393486 -0.483577 0.029827 -0.039537 0.998773 1.22631\n0.999456 -0.00619097 -0.0323787 -1.34424 0.00746815 0.999193 0.0394741 -0.494936 0.0321082 -0.0396944 0.998696 1.22259\n0.999642 -0.00686729 -0.025869 -2.90328 0.0078661 0.99922 0.0387085 -0.497204 0.025583 -0.0388981 0.998916 1.21676\n0.999602 -0.00785292 -0.0270807 -5.30729 0.00888916 0.999224 0.0383592 -0.510671 0.0267585 -0.0385846 0.998897 1.2019\n0.999508 -0.00609719 -0.0307522 -7.74889 0.00727903 0.999233 0.0384669 -0.515667 0.0304941 -0.0386718 0.998787 1.18708\n0.999649 -0.00310343 -0.0263013 -10.2206 0.00410009 0.999272 0.037925 -0.523826 0.0261645 -0.0380195 0.998934 1.18692\n0.999628 -0.00128348 -0.0272429 -11.8757 0.00229537 0.999307 0.0371447 -0.521009 0.0271763 -0.0371934 0.998938 1.17973\n0.999613 0.00141518 -0.0277856 -14.4018 -0.000424718 0.999365 0.0356203 -0.51781 0.0278184 -0.0355947 0.998979 1.16667\n0.99956 0.00438194 -0.0293282 -17.8088 -0.0032905 0.999304 0.0371601 -0.495284 0.0294706 -0.0370473 0.998879 1.14519\n0.99966 0.00351757 -0.0258541 -19.5282 -0.00254247 0.999288 0.0376521 -0.473576 0.0259681 -0.0375735 0.998956 1.13515\n0.999672 0.00264501 -0.0254914 -22.1487 -0.00169066 0.999299 0.0373872 -0.468674 0.0255724 -0.0373318 0.998976 1.1317\n0.999592 0.000531092 -0.0285586 -24.8228 0.000566195 0.999262 0.0384006 -0.455755 0.0285579 -0.0384011 0.998854 1.11682\n0.999609 -0.000184137 -0.0279624 -26.6107 0.00129784 0.999206 0.0398157 -0.44677 0.0279329 -0.0398364 0.998816 1.10897\n0.999696 -0.00161761 -0.0246193 -29.3027 0.00258061 0.999231 0.0391342 -0.449584 0.0245371 -0.0391858 0.998931 1.11414\n0.999589 -0.00287929 -0.0285083 -31.1219 0.00395352 0.999281 0.0376968 -0.448907 0.0283793 -0.0377941 0.998882 1.10921\n0.999607 -0.00382066 -0.0277688 -33.8468 0.00477353 0.999399 0.0343295 -0.459853 0.027621 -0.0344485 0.999025 1.09959\n0.999626 -0.00426285 -0.0270275 -36.5801 0.00522582 0.99935 0.0356591 -0.455796 0.0268579 -0.035787 0.998998 1.07968\n0.999449 -0.0043974 -0.032886 -38.4225 0.00559374 0.999323 0.0363756 -0.463441 0.0327037 -0.0365395 0.998797 1.07341\n0.999638 -0.00339495 -0.0266859 -41.168 0.00438708 0.999298 0.0372079 -0.470114 0.0265408 -0.0373115 0.998951 1.05785\n0.999655 -0.0028095 -0.0261182 -43.0193 0.00382085 0.999242 0.0387533 -0.46582 0.0259895 -0.0388397 0.998907 1.05734\n0.999377 -0.00187832 -0.0352413 -45.8031 0.00324757 0.99924 0.0388367 -0.462931 0.0351416 -0.038927 0.998624 1.03941\n0.999319 -0.000631342 -0.0368838 -47.6382 0.00203873 0.999271 0.0381323 -0.466039 0.0368328 -0.0381816 0.998592 1.03553\n0.999697 0.00152421 -0.0245571 -49.4424 -0.000648761 0.999365 0.0356183 -0.454262 0.0245958 -0.0355916 0.999064 1.02427\n0.999617 0.00426441 -0.027357 -51.2672 -0.00323906 0.999295 0.0374156 -0.439536 0.0274972 -0.0373126 0.998925 1.00584\n0.999399 0.00864196 -0.0335689 -53.9889 -0.0073064 0.999185 0.0397065 -0.410226 0.0338847 -0.0394374 0.998647 1.00486\n0.999545 0.0113684 -0.0279264 -55.7837 -0.0102593 0.999165 0.0395418 -0.386031 0.0283526 -0.0392373 0.998828 0.998285\n0.999524 0.0143723 -0.0272926 -58.447 -0.0133033 0.999152 0.0389558 -0.343773 0.0278294 -0.0385742 0.998868 0.979257\n0.999342 0.0152077 -0.0329249 -60.2278 -0.0138866 0.999104 0.0399888 -0.309483 0.0335035 -0.0395053 0.998658 0.978981\n0.999512 0.0165483 -0.0264764 -62.8571 -0.0155109 0.999122 0.0389185 -0.267842 0.0270972 -0.0384888 0.998892 0.976213\n0.999538 0.0154084 -0.0261811 -65.4785 -0.0144481 0.99923 0.0364809 -0.227769 0.026723 -0.0360858 0.998991 0.968438\n0.999463 0.0150239 -0.0291231 -67.2312 -0.0139706 0.999253 0.0360402 -0.203537 0.0296428 -0.035614 0.998926 0.965374\n0.999468 0.0134165 -0.0297384 -69.7977 -0.0124107 0.999353 0.0337525 -0.167284 0.030172 -0.0333655 0.998988 0.968633\n0.999415 0.0129333 -0.031672 -71.4931 -0.0119131 0.999411 0.0321895 -0.150115 0.0320696 -0.0317933 0.99898 0.952562\n0.999484 0.0122734 -0.0296953 -73.1785 -0.011376 0.999479 0.0302023 -0.134729 0.0300505 -0.0298489 0.999103 0.953741\n0.999454 0.0112418 -0.0310829 -74.8479 -0.0103219 0.999509 0.0295974 -0.109816 0.0314003 -0.0292604 0.999078 0.929135\n0.999132 0.00998819 -0.0404389 -78.1646 -0.00863053 0.999398 0.0336097 -0.065438 0.0407502 -0.0332315 0.998617 0.904128\n0.999711 0.00888233 -0.0223301 -79.8008 -0.00818163 0.999477 0.0312769 -0.0587126 0.0225963 -0.0310852 0.999261 0.906533\n0.999724 0.00855604 -0.0218954 -81.4192 -0.00794093 0.999576 0.0280274 -0.0377311 0.0221259 -0.0278458 0.999367 0.906386\n0.999501 0.0110469 -0.0295904 -83.0391 -0.0103378 0.999658 0.0240102 -0.0296125 0.0298455 -0.0236923 0.999274 0.903023\n0.999872 0.0148931 0.00587919 -85.3936 -0.0151144 0.999102 0.039576 0.0352204 -0.0052845 -0.0396598 0.999199 0.931102\n0.999513 0.0173847 -0.0259095 -87.0001 -0.0163939 0.999144 0.0379758 0.080214 0.0265475 -0.0375326 0.998943 0.928838\n0.998142 0.0199412 -0.0575694 -89.4321 -0.0183317 0.99943 0.0283518 0.108539 0.0581019 -0.0272438 0.997939 0.887514\n0.999656 0.022994 -0.0126548 -91.7699 -0.0226487 0.999385 0.0267866 0.187782 0.0132629 -0.0264907 0.999561 0.872577\n0.999351 0.0258499 -0.0250846 -93.4075 -0.0251546 0.999301 0.0276482 0.235679 0.0257818 -0.0269993 0.999303 0.865368\n0.999211 0.0294852 -0.0266268 -95.0413 -0.0287094 0.999165 0.0290611 0.293394 0.0274615 -0.0282737 0.999223 0.8646\n0.999072 0.0348694 -0.0252901 -96.6617 -0.0340603 0.998915 0.031746 0.358444 0.0263696 -0.0308551 0.999176 0.85904\n0.998822 0.0406813 -0.0264463 -98.331 -0.0398864 0.998756 0.0299204 0.430081 0.0276306 -0.0288303 0.999202 0.857022\n0.998486 0.048357 -0.0262289 -100.853 -0.0474733 0.998316 0.0333284 0.576766 0.0277964 -0.0320327 0.9991 0.835065\n0.99826 0.0543766 -0.0227968 -102.564 -0.0536403 0.998055 0.0317527 0.662132 0.0244791 -0.0304746 0.999236 0.847714\n0.998002 0.0597516 -0.0205432 -104.277 -0.059153 0.997839 0.028605 0.770089 0.022208 -0.0273327 0.99938 0.841828\n0.997563 0.0642761 -0.0271254 -105.98 -0.0635055 0.997578 0.0283729 0.896175 0.0288834 -0.0265812 0.999229 0.829707\n0.996868 0.0731765 -0.0299843 -108.62 -0.072408 0.997037 0.0259623 1.08019 0.0317952 -0.0237099 0.999213 0.824015\n0.996773 0.078425 -0.0171342 -110.35 -0.0780194 0.996682 0.0231818 1.22325 0.0188954 -0.0217701 0.999584 0.81559\n0.996275 0.0836684 -0.0208725 -112.112 -0.0832073 0.996288 0.0220617 1.3849 0.0226409 -0.0202428 0.999539 0.812425\n0.9955 0.0897982 -0.0302684 -113.909 -0.0890531 0.99571 0.0251287 1.56019 0.0323951 -0.0223201 0.999226 0.79744\n0.99503 0.0955948 -0.0278893 -115.672 -0.0948937 0.995162 0.0254652 1.73909 0.0301887 -0.0226921 0.999287 0.799701\n0.994591 0.102899 -0.0141796 -118.367 -0.102524 0.994415 0.0250351 2.03475 0.0166765 -0.0234459 0.999586 0.796714\n0.993919 0.108716 -0.0174779 -120.198 -0.108226 0.993762 0.0269105 2.23679 0.0202945 -0.0248553 0.999485 0.809274\n0.993857 0.109913 -0.0129268 -122.014 -0.109569 0.993667 0.0249026 2.44353 0.015582 -0.0233332 0.999606 0.828444\n0.993577 0.111012 -0.0219338 -123.845 -0.11037 0.99348 0.0285803 2.66288 0.0249635 -0.0259759 0.999351 0.833345\n0.992895 0.11444 -0.0326138 -126.596 -0.113483 0.993092 0.029828 2.97104 0.035802 -0.0259149 0.999023 0.795703\n0.992924 0.116317 -0.0239066 -128.443 -0.115593 0.992852 0.0297349 3.19386 0.0271943 -0.0267611 0.999272 0.788608\n0.992685 0.117881 -0.026092 -130.324 -0.117137 0.992709 0.0284126 3.42677 0.0292511 -0.0251484 0.999256 0.777565\n0.992418 0.118852 -0.0313025 -132.204 -0.11787 0.992528 0.0315456 3.6534 0.0348178 -0.0276168 0.999012 0.756072\n0.992463 0.119643 -0.0265075 -134.075 -0.118754 0.992378 0.0329078 3.90154 0.0302427 -0.0295119 0.999107 0.748988\n0.992304 0.120381 -0.0290052 -136.927 -0.119397 0.992279 0.0335801 4.26282 0.0328236 -0.0298585 0.999015 0.72274\n0.992158 0.121015 -0.0312551 -138.839 -0.120014 0.992251 0.0321512 4.4895 0.0349037 -0.028148 0.998994 0.702758\n0.992149 0.121485 -0.0296984 -140.744 -0.120554 0.992211 0.0313562 4.72601 0.0332764 -0.0275298 0.999067 0.689568\n0.992288 0.121359 -0.0252318 -143.618 -0.120563 0.992221 0.0310087 5.08172 0.0287987 -0.0277276 0.999201 0.659813\n0.992294 0.121225 -0.0256337 -145.56 -0.120444 0.992265 0.0300727 5.33713 0.029081 -0.0267535 0.999219 0.650932\n0.992186 0.121282 -0.0292943 -147.464 -0.12043 0.992289 0.0292979 5.56237 0.0326218 -0.0255411 0.999141 0.644657\n0.992253 0.121447 -0.0261604 -149.394 -0.120702 0.992282 0.0284165 5.79904 0.0294097 -0.0250388 0.999254 0.627615\n0.9923 0.121322 -0.0249175 -151.319 -0.120603 0.992289 0.0286029 6.03211 0.0281955 -0.0253776 0.99928 0.619051\n0.992422 0.120635 -0.0233568 -153.253 -0.119882 0.992293 0.0313383 6.28454 0.0269573 -0.0283007 0.999236 0.60821\n0.992656 0.118037 -0.0264913 -156.167 -0.117095 0.992515 0.0346703 6.64245 0.0303854 -0.0313137 0.999048 0.593303\n0.993002 0.115227 -0.0258764 -159.076 -0.114293 0.992828 0.0350605 6.99238 0.0297307 -0.0318576 0.99905 0.590447\n0.993201 0.11333 -0.0266012 -161.014 -0.112389 0.993064 0.0345375 7.21505 0.0303308 -0.031313 0.999049 0.579633\n0.993299 0.111916 -0.0288563 -162.908 -0.110918 0.993243 0.0341253 7.42945 0.0324805 -0.0306959 0.999001 0.562055\n0.993434 0.110328 -0.0302893 -165.764 -0.109324 0.993457 0.033033 7.75693 0.0337356 -0.0295048 0.998995 0.546645\n0.993373 0.11135 -0.028465 -167.656 -0.110413 0.99335 0.0326313 7.96791 0.0319092 -0.0292721 0.999062 0.527984\n0.993279 0.112657 -0.0265641 -169.541 -0.11175 0.993173 0.033449 8.18091 0.030151 -0.0302556 0.999087 0.512313\n0.993056 0.115081 -0.0244271 -172.328 -0.114225 0.992874 0.0339576 8.51239 0.028161 -0.0309316 0.999125 0.518824\n0.992582 0.117968 -0.0293899 -175.125 -0.117094 0.992671 0.0298858 8.84238 0.0327001 -0.0262227 0.999121 0.509\n0.992332 0.120231 -0.0286615 -176.969 -0.119426 0.992439 0.0283287 9.05899 0.0318507 -0.0246886 0.999188 0.501349\n0.992173 0.12216 -0.0258876 -178.811 -0.121488 0.992249 0.0261306 9.29834 0.028879 -0.022781 0.999323 0.495108\n0.991945 0.124363 -0.0240451 -180.622 -0.123826 0.992045 0.0226873 9.51659 0.0266753 -0.0195272 0.999453 0.484417\n0.991799 0.125853 -0.0222741 -182.449 -0.125489 0.991948 0.0170511 9.74406 0.0242407 -0.0141161 0.999606 0.491559\n0.991972 0.126416 -0.00334636 -185.134 -0.126278 0.991615 0.0274438 10.1243 0.00678764 -0.0268009 0.999618 0.518202\n0.989615 0.126524 -0.0682233 -187.892 -0.124309 0.991597 0.0358015 10.4862 0.0721798 -0.026949 0.997027 0.469983\n0.991816 0.127674 0.00101147 -190.513 -0.127659 0.991501 0.0250806 10.8294 0.00219927 -0.0250045 0.999685 0.437699\n0.991661 0.127267 -0.020312 -192.323 -0.126596 0.991455 0.0314665 11.0848 0.024143 -0.0286326 0.999298 0.449511\n0.991085 0.128819 -0.034003 -195.022 -0.127822 0.991342 0.030038 11.4249 0.037578 -0.0254239 0.99897 0.42551\n0.991589 0.126837 -0.025747 -197.703 -0.125838 0.99135 0.0372772 11.7931 0.0302524 -0.0337237 0.998973 0.407423\n0.991965 0.125266 -0.0176965 -200.365 -0.124544 0.991514 0.0372674 12.1341 0.0222147 -0.034764 0.999149 0.404061\n0.991987 0.124349 -0.022356 -202.163 -0.123428 0.991598 0.0387145 12.3733 0.0269823 -0.035645 0.999 0.401753\n0.991945 0.123667 -0.0274017 -204.828 -0.122678 0.99182 0.0352639 12.686 0.0315386 -0.0316183 0.999002 0.391023\n0.991921 0.123846 -0.0274718 -207.464 -0.122908 0.991851 0.0335354 13.0118 0.0314012 -0.029888 0.99906 0.370988\n0.991905 0.124591 -0.0245118 -210.064 -0.123731 0.991735 0.0339575 13.3414 0.02854 -0.0306498 0.999123 0.364018\n0.991915 0.124426 -0.0249638 -211.789 -0.123533 0.991738 0.0345763 13.5652 0.0290597 -0.0312128 0.99909 0.354946\n0.991812 0.12457 -0.0281431 -213.504 -0.123555 0.99171 0.0353004 13.7851 0.0323071 -0.0315341 0.99898 0.353175\n0.991969 0.124566 -0.0219253 -216.044 -0.123803 0.991748 0.0332984 14.1068 0.0258923 -0.0303166 0.999205 0.344831\n0.991745 0.124799 -0.0294414 -218.561 -0.123726 0.99167 0.0358269 14.4351 0.0336673 -0.0318885 0.998924 0.332787\n0.992237 0.124362 0.000129851 -221.01 -0.124291 0.991634 0.0348349 14.7375 0.00420339 -0.0345806 0.999393 0.340759\n0.992024 0.122915 -0.0279153 -223.5 -0.121798 0.991799 0.0387208 15.0704 0.0324458 -0.035012 0.99886 0.355846\n0.990041 0.122048 -0.0701703 -225.182 -0.119485 0.992044 0.0396508 15.2772 0.0744513 -0.0308716 0.996747 0.311446\n0.992519 0.122081 -0.00162687 -227.6 -0.121958 0.99197 0.033492 15.5676 0.00570254 -0.033043 0.999438 0.301989\n0.992163 0.12076 -0.032075 -230.144 -0.119509 0.992089 0.0384212 15.8924 0.0364611 -0.0342869 0.998747 0.278165\n0.992304 0.120273 -0.0294431 -231.819 -0.119048 0.992069 0.0403296 16.098 0.0340602 -0.0365141 0.998753 0.277097\n0.99272 0.119529 -0.0147985 -234.348 -0.118968 0.992305 0.0343047 16.3997 0.0187851 -0.0322944 0.999302 0.284305\n0.992681 0.119069 -0.0201865 -236.063 -0.118409 0.992473 0.0312547 16.5958 0.023756 -0.0286357 0.999308 0.278947\n0.992541 0.118418 -0.0289688 -237.784 -0.117534 0.9926 0.0305045 16.8071 0.0323667 -0.0268722 0.999115 0.273603\n0.992747 0.117756 -0.0242192 -240.355 -0.117036 0.992697 0.0292326 17.1155 0.0274847 -0.0261861 0.999279 0.259399\n0.992627 0.118011 -0.0276604 -242.966 -0.117162 0.992643 0.030532 17.4271 0.03106 -0.0270661 0.999151 0.249615\n0.992544 0.118404 -0.0289386 -244.685 -0.117435 0.992527 0.03316 17.6438 0.0326486 -0.0295144 0.999031 0.234829\n0.992566 0.119852 -0.0211653 -247.253 -0.119124 0.992336 0.0328518 17.9572 0.0249405 -0.0300863 0.999236 0.237097\n0.992541 0.119609 -0.0235996 -249.773 -0.118834 0.992402 0.0319018 18.2697 0.027236 -0.0288594 0.999212 0.235228\n0.992342 0.120822 -0.0256908 -252.281 -0.119964 0.992241 0.0326544 18.5829 0.0294368 -0.0293223 0.999136 0.233239\n0.992165 0.122098 -0.0264752 -254.744 -0.121163 0.992034 0.0344646 18.8914 0.0304723 -0.0309867 0.999055 0.224806\n0.992028 0.122982 -0.0275089 -256.365 -0.121962 0.991878 0.0360901 19.092 0.0317239 -0.0324473 0.99897 0.218986\n0.992052 0.124009 -0.0213446 -258.786 -0.123192 0.991727 0.0360588 19.3983 0.0256396 -0.0331427 0.999122 0.213384\n0.991878 0.124751 -0.0248139 -260.38 -0.123775 0.991597 0.0376171 19.6101 0.0292982 -0.0342402 0.998984 0.21057\n0.992045 0.12588 0.000684721 -263.547 -0.125824 0.991405 0.0358283 20.0089 0.00383123 -0.0356294 0.999358 0.245561\n0.992221 0.12306 0.0188001 -265.84 -0.123794 0.991294 0.0448452 20.3476 -0.0131178 -0.0468237 0.998817 0.305634\n0.991201 0.121108 -0.0534128 -267.497 -0.118627 0.991807 0.0473997 20.5455 0.0587156 -0.0406464 0.997447 0.284753\n0.99034 0.118732 -0.071623 -269.137 -0.11542 0.99212 0.0487403 20.7346 0.0768456 -0.0400028 0.99624 0.234735\n0.993228 0.115922 0.00774006 -271.517 -0.116122 0.99264 0.0343848 21.0146 -0.00369713 -0.0350507 0.999379 0.234089\n0.99342 0.11419 -0.00881251 -273.175 -0.113791 0.992811 0.0371093 21.2177 0.0129867 -0.0358623 0.999272 0.231236\n0.993553 0.111509 -0.0204597 -275.673 -0.110631 0.993059 0.0399451 21.4956 0.0247719 -0.0374241 0.998992 0.244823\n0.994031 0.108428 -0.012084 -277.33 -0.107877 0.99338 0.039477 21.69 0.0162844 -0.0379377 0.999147 0.25025\n0.994348 0.105468 -0.0121615 -278.996 -0.104935 0.99374 0.0383266 21.8612 0.0161276 -0.0368338 0.999191 0.272883\n0.994866 0.100606 -0.0109239 -281.526 -0.100164 0.994341 0.0353874 22.1163 0.0144223 -0.0341116 0.999314 0.287783\n0.995106 0.0975719 -0.0155829 -283.232 -0.0969959 0.994696 0.0342153 22.2754 0.0188387 -0.0325363 0.999293 0.300158\n0.995419 0.0940766 -0.0170714 -284.932 -0.0934527 0.995034 0.0342597 22.4337 0.0202097 -0.0325073 0.999267 0.303748\n0.995938 0.0885485 -0.0163241 -287.51 -0.0879591 0.995549 0.0338508 22.6722 0.0192489 -0.0322775 0.999294 0.312661\n0.996257 0.0849594 -0.0159382 -289.223 -0.0843801 0.995852 0.0340502 22.8187 0.018765 -0.0325779 0.999293 0.323755\n0.996833 0.0774974 -0.0178629 -292.599 -0.0768798 0.996494 0.0329951 23.0851 0.0203573 -0.0315173 0.999296 0.344898\n0.997073 0.0741571 -0.018588 -294.261 -0.0734985 0.996719 0.0339127 23.2136 0.0210419 -0.0324473 0.999252 0.350303\n0.997507 0.0684482 -0.0171513 -296.764 -0.067888 0.9972 0.0313517 23.3864 0.0192493 -0.0301092 0.999361 0.374438\n0.997894 0.0642245 -0.00912071 -298.4 -0.0639138 0.997473 0.0310244 23.5051 0.0110902 -0.0303761 0.999477 0.39096\n0.998009 0.0606393 -0.0173725 -300.033 -0.0600782 0.997707 0.031177 23.6013 0.0192232 -0.0300712 0.999363 0.41149\n0.998404 0.0547973 -0.0136492 -302.434 -0.0543642 0.998061 0.0303058 23.7339 0.0152834 -0.0295154 0.999447 0.438439\n0.99861 0.0515507 -0.0110154 -304.002 -0.0512018 0.998241 0.0299045 23.8142 0.0125376 -0.0292989 0.999492 0.454368\n0.998844 0.0459559 -0.0141168 -306.345 -0.0455276 0.998534 0.0292895 23.9249 0.0154421 -0.0286129 0.999471 0.485415\n0.999003 0.0423068 -0.0142638 -307.904 -0.0418787 0.998699 0.0290842 23.9938 0.0154757 -0.0284579 0.999475 0.498161\n0.9992 0.0371357 -0.0148707 -309.468 -0.0366328 0.998791 0.0327726 24.058 0.0160698 -0.0322016 0.999352 0.499812\n0.999498 0.0299146 -0.0104387 -311.762 -0.0295971 0.999131 0.029345 24.1249 0.0113075 -0.0290213 0.999515 0.557501\n0.999657 0.0261528 -0.00109554 -313.298 -0.026114 0.999299 0.0268189 24.1581 0.00179616 -0.0267811 0.99964 0.568232\n0.999334 0.0243943 0.0271327 -315.579 -0.025089 0.999358 0.0255628 24.2128 -0.0264917 -0.0262265 0.999305 0.661358\n0.99955 0.023504 0.018649 -317.132 -0.0239629 0.999406 0.024774 24.2522 -0.0180556 -0.0252097 0.999519 0.706026\n0.997595 0.029134 -0.0628854 -320.305 -0.0273988 0.999224 0.0282817 24.3469 0.0636606 -0.0264907 0.99762 0.670666\n0.999369 0.0305266 0.0181698 -322.611 -0.0309558 0.999237 0.0238262 24.4203 -0.0174286 -0.0243736 0.999551 0.691945\n0.999504 0.0303183 -0.00852577 -325.024 -0.0301687 0.999398 0.0171565 24.4766 0.00904079 -0.0168907 0.999816 0.740148\n0.999565 0.0294543 0.00160165 -327.434 -0.0294776 0.999416 0.0172965 24.5571 -0.00109125 -0.0173362 0.999849 0.770994\n0.999496 0.028717 -0.0135297 -329.895 -0.0284482 0.999402 0.0196554 24.6351 0.0140861 -0.0192606 0.999715 0.806037\n0.99954 0.0282116 -0.0111178 -331.539 -0.0279839 0.999406 0.0201282 24.6886 0.0116791 -0.0198078 0.999736 0.825449\n0.999593 0.0267237 -0.00996595 -334.082 -0.0264576 0.999313 0.0259404 24.7767 0.0106523 -0.0256662 0.999614 0.844923\n0.999661 0.0255248 -0.00515552 -336.651 -0.0253679 0.999272 0.0285031 24.8603 0.0058793 -0.0283627 0.99958 0.88025\n0.999737 0.0228756 0.00139897 -338.385 -0.0229055 0.999359 0.0275261 24.8973 -0.000768393 -0.027551 0.99962 0.930565\n0.999797 0.019977 -0.00268281 -341.022 -0.0199172 0.999587 0.0207058 24.9532 0.00309534 -0.0206481 0.999782 0.978167\n0.999855 0.0157109 -0.00663881 -343.693 -0.0155796 0.999691 0.0193853 24.9913 0.00694132 -0.019279 0.99979 1.01343\n0.999895 0.0143633 -0.00205703 -345.46 -0.0143207 0.99971 0.0193815 25.0193 0.00233482 -0.01935 0.99981 1.04275\n0.999928 0.0119012 -0.00149879 -347.264 -0.0118723 0.999767 0.0180211 25.05 0.00171292 -0.018002 0.999836 1.08007\n0.999949 0.00949292 -0.00337679 -349.065 -0.00943409 0.99981 0.0170312 25.0689 0.00353783 -0.0169985 0.999849 1.11607\n0.999955 0.0058448 -0.00753622 -351.841 -0.0056981 0.999797 0.0193422 25.0882 0.00764774 -0.0192984 0.999785 1.15805\n0.999973 0.00304673 -0.00665815 -353.698 -0.00291616 0.999805 0.019532 25.1063 0.00671636 -0.0195121 0.999787 1.18987\n0.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\n0.999959 -0.00163332 -0.00887036 -358.352 0.00180562 0.999809 0.0194504 25.0924 0.0088369 -0.0194656 0.999771 1.27635\n0.999957 -0.00490232 -0.00790947 -361.113 0.00505922 0.999788 0.01994 25.0876 0.00781005 -0.0199792 0.99977 1.32503\n0.999983 -0.00566245 -0.00109184 -363.825 0.00568162 0.999814 0.0184349 25.0604 0.000987246 -0.0184408 0.999829 1.38784\n0.999955 -0.00618292 -0.00715033 -366.552 0.0063065 0.999829 0.0173917 25.0749 0.00704158 -0.017436 0.999823 1.43543\n0.999964 -0.00564585 -0.00634923 -369.264 0.00576863 0.999793 0.019488 25.0612 0.00623789 -0.019524 0.99979 1.48164\n0.999983 -0.00575017 0.000853569 -371.045 0.00573208 0.999786 0.0198697 25.0484 -0.000967641 -0.0198645 0.999802 1.52729\n0.999131 -0.00705057 0.0410898 -373.703 0.0060836 0.999703 0.0236108 25.0421 -0.041244 -0.0233403 0.998876 1.64374\n0.998278 -0.00995584 -0.0578082 -377.416 0.0113323 0.999659 0.0235322 25.0082 0.0575542 -0.0241468 0.99805 1.68177\n0.999032 -0.00983989 -0.0428701 -379.166 0.010785 0.999703 0.0218701 25.0127 0.0426421 -0.0223113 0.998841 1.72402\n0.999778 -0.013301 0.0163483 -381.869 0.0129274 0.999658 0.0227499 24.966 -0.0166453 -0.0225335 0.999608 1.73717\n0.999782 -0.0139745 -0.0155314 -384.678 0.0143492 0.999602 0.0242789 24.9442 0.0151859 -0.0244965 0.999585 1.7994\n0.999877 -0.0152573 0.00373039 -386.506 0.0151606 0.99958 0.0247067 24.926 -0.00410578 -0.0246471 0.999688 1.83835\n0.999841 -0.0173512 -0.00402572 -389.306 0.0174415 0.999569 0.0235957 24.8812 0.00361457 -0.0236622 0.999713 1.89057\n0.999814 -0.0185302 -0.00527802 -392.103 0.0186522 0.999535 0.0241027 24.837 0.00482894 -0.0241967 0.999696 1.95473\n0.999796 -0.0201831 0.00057899 -393.915 0.0201636 0.999515 0.0237341 24.7987 -0.00105774 -0.0237176 0.999718 1.99927\n0.999739 -0.0222626 -0.00521892 -396.687 0.0223878 0.999429 0.0252981 24.7466 0.00465274 -0.0254083 0.999666 2.05466\n0.999684 -0.0245464 -0.00546775 -398.499 0.024697 0.999262 0.0294202 24.7067 0.00474156 -0.0295459 0.999552 2.10405\n0.99956 -0.0294245 0.00366955 -401.173 0.0293092 0.99917 0.0282728 24.6325 -0.00449842 -0.0281529 0.999594 2.18162\n0.999485 -0.0320842 -0.000116935 -402.946 0.032075 0.999094 0.0279865 24.5787 -0.000781097 -0.0279759 0.999608 2.22155\n0.999346 -0.0360622 -0.00283087 -405.561 0.0361284 0.998938 0.0285845 24.4924 0.00179705 -0.0286681 0.999587 2.29262\n0.999147 -0.0412857 4.14073e-05 -408.164 0.041266 0.998701 0.0298904 24.3937 -0.0012754 -0.0298632 0.999553 2.35737\n0.998882 -0.0469268 0.00575776 -410.76 0.0467419 0.998489 0.0288785 24.2841 -0.00710424 -0.0285771 0.999566 2.41511\n0.99845 -0.0516623 0.0207126 -413.319 0.0508469 0.997978 0.0381302 24.1663 -0.0226407 -0.0370179 0.999058 2.55147\n0.997981 -0.0574325 -0.0271178 -415.969 0.0581762 0.997928 0.0274818 24.0148 0.0254833 -0.0290039 0.999254 2.57617\n0.998188 -0.0601172 -0.00242976 -418.524 0.0601613 0.997813 0.027388 23.8612 0.000777954 -0.0274845 0.999622 2.62522\n0.997865 -0.0650063 0.00631239 -421.145 0.064804 0.997499 0.028213 23.7 -0.00813063 -0.0277437 0.999582 2.67893\n0.99769 -0.0671383 -0.0103945 -422.909 0.0674016 0.997347 0.02749 23.592 0.00852134 -0.0281271 0.999568 2.71808\n0.997437 -0.0711674 -0.00740176 -425.522 0.0713607 0.996989 0.0303461 23.405 0.00521981 -0.0307965 0.999512 2.75957\n0.997141 -0.0747929 -0.0107704 -428.137 0.0750983 0.996679 0.0314737 23.2287 0.00838061 -0.0321926 0.999447 2.81222\n0.996974 -0.0775109 -0.00586713 -429.859 0.0776619 0.996445 0.032657 23.0893 0.003315 -0.0330138 0.999449 2.86069\n0.996598 -0.0816133 0.0114513 -432.445 0.0812069 0.996173 0.0323378 22.8882 -0.0140466 -0.0312978 0.999411 2.94063\n0.996436 -0.0843032 -0.0030391 -434.18 0.0843543 0.99608 0.0266449 22.7428 0.000780933 -0.0268063 0.99964 2.99036\n0.996172 -0.0870504 -0.00802828 -435.889 0.0872307 0.995854 0.0258132 22.5963 0.00574794 -0.0264147 0.999635 3.02332\n0.995883 -0.0905818 -0.00332594 -437.602 0.0906376 0.995549 0.0258191 22.4451 0.000972397 -0.0260143 0.999661 3.05477\n0.995159 -0.0982066 -0.00362444 -440.155 0.0982689 0.99479 0.0271251 22.2007 0.000941686 -0.02735 0.999625 3.11755\n0.994631 -0.103447 0.0027378 -441.857 0.103342 0.994307 0.025981 22.0205 -0.00540988 -0.0255586 0.999659 3.16754\n0.993836 -0.110847 -0.00177387 -444.411 0.110853 0.993439 0.0281195 21.745 -0.00135473 -0.0281428 0.999603 3.23396\n0.992769 -0.117799 0.0230724 -446.905 0.116976 0.992544 0.0342429 21.447 -0.0269342 -0.0312963 0.999147 3.32362\n0.992016 -0.125437 0.0130368 -449.44 0.124928 0.991569 0.034401 21.1706 -0.017242 -0.0324976 0.999323 3.44843\n0.989299 -0.133161 -0.0596349 -452.053 0.134673 0.990644 0.0220872 20.8211 0.0561358 -0.029882 0.997976 3.45349\n0.989892 -0.140528 0.0191221 -454.532 0.139927 0.989714 0.0297864 20.4547 -0.0231112 -0.0268097 0.999373 3.50286\n0.9893 -0.145163 -0.0146324 -457.187 0.145512 0.989 0.0265551 20.0962 0.0106166 -0.0284002 0.99954 3.56133\n0.988686 -0.149858 -0.00650181 -459.774 0.149984 0.988274 0.0286308 19.7002 0.002135 -0.029282 0.999569 3.59908\n0.988185 -0.15306 -0.00793637 -461.491 0.153226 0.987789 0.0282058 19.4538 0.00352226 -0.0290886 0.999571 3.63819\n0.987715 -0.15577 -0.0124748 -463.192 0.156054 0.987395 0.0264258 19.1787 0.00820122 -0.0280479 0.999573 3.68215\n0.987295 -0.158841 -0.00414347 -464.873 0.158895 0.986906 0.0277164 18.9001 -0.000313294 -0.0280226 0.999607 3.71295\n0.986884 -0.161298 -0.00660495 -466.527 0.161416 0.986545 0.0259536 18.625 0.00232982 -0.0266794 0.999641 3.75459\n0.986101 -0.165941 -0.00832262 -469.022 0.166099 0.985796 0.0248325 18.2153 0.00408369 -0.0258697 0.999657 3.80843\n0.985397 -0.169968 -0.0101606 -471.472 0.170171 0.985111 0.0244448 17.8014 0.00585447 -0.0258169 0.99965 3.85973\n0.984948 -0.172681 -0.00771752 -473.096 0.17282 0.984657 0.0241501 17.5142 0.00342883 -0.0251204 0.999679 3.90182\n0.984522 -0.175002 -0.00947717 -475.514 0.175187 0.984226 0.0246914 17.0886 0.00500661 -0.0259696 0.99965 3.94995\n0.984006 -0.177959 0.00793513 -477.848 0.177745 0.983821 0.0224158 16.6479 -0.0117958 -0.0206468 0.999717 4.0088\n0.982997 -0.18038 0.0343461 -479.381 0.179202 0.983197 0.0347811 16.381 -0.0400428 -0.0280348 0.998805 4.10431\n0.982579 -0.1823 -0.0361205 -481.794 0.183314 0.982682 0.0270722 15.9562 0.0305597 -0.033222 0.998981 4.14648\n0.980832 -0.184732 -0.0619922 -483.392 0.186184 0.982341 0.0184842 15.6384 0.0574829 -0.0296718 0.997905 4.12267\n0.98212 -0.188196 0.00482605 -485.689 0.187946 0.981644 0.0324288 15.2046 -0.0108404 -0.0309419 0.999462 4.16434\n0.981704 -0.190216 -0.00867023 -487.295 0.190383 0.981339 0.0269665 14.9052 0.00337898 -0.0281238 0.999599 4.18878\n0.981102 -0.192818 -0.0160984 -488.871 0.193194 0.980803 0.0264813 14.589 0.0106833 -0.029091 0.99952 4.22536\n0.980597 -0.196013 -0.00287081 -491.174 0.19602 0.98025 0.0261913 14.1281 -0.00231972 -0.0262459 0.999653 4.28774\n0.979575 -0.201059 -0.00290335 -493.494 0.201073 0.979309 0.0228807 13.6537 -0.00175711 -0.0229972 0.999734 4.34931\n0.978951 -0.203992 -0.00652203 -495.017 0.204089 0.978679 0.0231235 13.3342 0.00166597 -0.0239678 0.999711 4.38809\n0.977935 -0.208892 -0.00255512 -497.29 0.208895 0.977667 0.0230138 12.8497 -0.00230934 -0.0230398 0.999732 4.44865\n0.977195 -0.212324 -0.00288227 -498.794 0.212335 0.976948 0.0220456 12.5298 -0.00186498 -0.0221549 0.999753 4.48834\n0.976285 -0.216303 -0.00896461 -501.041 0.216444 0.976085 0.020235 12.0303 0.00437334 -0.0216955 0.999755 4.54269\n0.975731 -0.218794 -0.00885645 -502.519 0.218937 0.975504 0.0213982 11.7008 0.00395771 -0.0228179 0.999732 4.58038\n0.974982 -0.222119 -0.00857852 -504.721 0.222254 0.974758 0.0211915 11.2002 0.00365496 -0.0225679 0.999739 4.61996\n0.974711 -0.223227 -0.010402 -506.189 0.223413 0.974457 0.0228229 10.8668 0.00504158 -0.0245696 0.999685 4.65429\n0.974342 -0.224903 -0.00878071 -508.365 0.22505 0.974065 0.0234304 10.3626 0.00328344 -0.0248053 0.999687 4.70445\n0.974006 -0.226364 -0.00842699 -510.55 0.226503 0.973726 0.0235374 9.86273 0.00287756 -0.0248343 0.999687 4.74482\n0.973667 -0.227779 -0.0094546 -512.712 0.227947 0.973357 0.0248285 9.36888 0.00354729 -0.0263298 0.999647 4.7995\n0.973123 -0.23014 -0.00821582 -515.56 0.230278 0.972779 0.0259557 8.70132 0.00201873 -0.02715 0.999629 4.86491\n0.972813 -0.231251 -0.0125416 -516.985 0.231506 0.972498 0.0255373 8.37043 0.00629119 -0.0277464 0.999595 4.8903\n0.972266 -0.233444 -0.0142354 -518.4 0.233752 0.971938 0.0263872 8.03121 0.00767598 -0.028983 0.99955 4.92187\n0.971148 -0.238265 -0.0100618 -520.472 0.238453 0.970786 0.0267248 7.51318 0.00340027 -0.028353 0.999592 4.96143\n0.969958 -0.242588 -0.0182311 -522.553 0.243054 0.969538 0.0303371 7.00865 0.0103163 -0.0338568 0.999373 5.00231\n0.969303 -0.245402 -0.0151188 -523.926 0.24577 0.968818 0.031453 6.65923 0.00692874 -0.0342033 0.999391 5.03115\n0.968671 -0.248172 -0.00933906 -525.286 0.248346 0.968115 0.032817 6.3185 0.000897026 -0.0341082 0.999418 5.06405\n0.968052 -0.250749 0.00107882 -527.315 0.250656 0.967793 0.0234028 5.77552 -0.0069123 -0.0223847 0.999726 5.14803\n0.967589 -0.252404 -0.00793092 -529.328 0.252521 0.967335 0.0222802 5.25403 0.00204824 -0.0235608 0.99972 5.18019\n0.967247 -0.253715 -0.00788434 -531.313 0.253815 0.967112 0.0164952 4.71509 0.00343996 -0.0179561 0.999833 5.23154\n0.965959 -0.258511 -0.00975336 -533.287 0.258671 0.965692 0.0229841 4.19529 0.0034771 -0.0247246 0.999688 5.26071\n0.964361 -0.264591 1.77277e-05 -535.878 0.264478 0.963949 0.029229 3.48094 -0.00775084 -0.0281826 0.999573 5.33391\n0.961745 -0.272349 0.0295366 -537.779 0.270795 0.961447 0.0478558 2.96858 -0.0414314 -0.0380267 0.998417 5.46505\n0.961067 -0.276314 0.00106827 -539.081 0.276131 0.960559 0.0328195 2.59443 -0.0100946 -0.0312467 0.999461 5.49416\n0.958678 -0.280905 -0.0450363 -540.428 0.281957 0.959241 0.0188862 2.19998 0.0378955 -0.0308041 0.998807 5.49052\n0.957985 -0.284056 -0.0397118 -541.708 0.284902 0.958398 0.0174556 1.81186 0.0331013 -0.0280362 0.999059 5.52533\n0.957045 -0.289926 0.00281715 -543.558 0.289709 0.956627 0.03054 1.24978 -0.0115493 -0.028412 0.99953 5.54742\n0.956236 -0.29253 -0.0062757 -544.812 0.292592 0.955865 0.0266685 0.873219 -0.00180263 -0.0273376 0.999625 5.58018\n0.955445 -0.294995 -0.0101159 -546.675 0.295167 0.955007 0.0289674 0.307383 0.00111548 -0.0306626 0.999529 5.63096\n0.954969 -0.296676 -0.00424691 -548.478 0.296666 0.954503 0.0302126 -0.249534 -0.00490967 -0.030112 0.999534 5.69379\n0.95483 -0.297145 -0.00212314 -550.257 0.297081 0.954419 0.0287696 -0.821451 -0.00652237 -0.0281008 0.999584 5.74884\n0.955275 -0.295708 -0.00266752 -551.425 0.295668 0.954899 0.0273557 -1.17149 -0.00554207 -0.0269209 0.999622 5.79722\n0.957655 -0.287913 -0.00162944 -553.138 0.287882 0.957431 0.0212168 -1.68271 -0.00454852 -0.0207874 0.999774 5.85474\n0.964984 -0.262264 -0.00492988 -555.293 0.26231 0.964845 0.0163473 -2.26499 0.000469252 -0.0170681 0.999854 5.93423\n0.971277 -0.237898 -0.00505828 -556.313 0.237945 0.971192 0.01297 -2.49445 0.00182703 -0.013801 0.999903 5.95001\n0.983214 -0.182392 -0.00486583 -557.776 0.182442 0.983124 0.0134663 -2.72394 0.00232757 -0.014128 0.999897 5.99324\n0.994587 -0.103296 -0.0112754 -559.261 0.103454 0.99453 0.0144268 -2.80855 0.00972352 -0.0155152 0.999832 5.99769\n0.999971 -0.00471001 -0.00599295 -560.696 0.00488331 0.999561 0.0292392 -2.72253 0.0058526 -0.0292676 0.999554 6.02173\n0.993989 0.10883 -0.0118797 -562.096 -0.108307 0.993383 0.0382018 -2.45868 0.0159586 -0.0366855 0.999199 6.03157\n0.971352 0.237529 -0.00746513 -563.429 -0.23692 0.970359 0.0476638 -2.01597 0.0185654 -0.0445297 0.998836 6.04912\n0.928686 0.370798 0.00716294 -564.667 -0.370746 0.927716 0.0434795 -1.40426 0.00947695 -0.0430344 0.999029 6.06765\n0.861374 0.507965 -0.0024877 -565.822 -0.507392 0.860616 0.0435038 -0.626334 0.0242394 -0.0362108 0.99905 6.05808\n0.768888 0.639318 -0.00917871 -566.816 -0.638378 0.768404 0.0450416 0.337394 0.0358489 -0.0287725 0.998943 6.04308\n0.656681 0.754118 -0.00867261 -567.635 -0.753116 0.656329 0.0452734 1.45563 0.0398336 -0.0231987 0.998937 5.99045\n0.479676 0.877435 -0.00439832 -568.435 -0.876323 0.479309 0.0481786 3.18259 0.0443817 -0.0192558 0.998829 5.94198\n0.344415 0.938691 0.015445 -568.834 -0.938147 0.3435 0.0434553 4.63099 0.0354857 -0.0294563 0.998936 5.8996\n0.239136 0.97088 0.0143429 -569.144 -0.970697 0.238678 0.0279422 6.11682 0.0237052 -0.0206046 0.999507 5.91995\n0.185094 0.98251 0.020351 -569.292 -0.982688 0.184879 0.0120184 7.16684 0.00804574 -0.0222232 0.999721 5.90536\n0.137762 0.99004 0.0290431 -569.378 -0.990465 0.137678 0.00486129 8.20873 0.000814272 -0.0294359 0.999566 5.90777\n0.0774608 0.996651 0.0262201 -569.453 -0.996306 0.0764028 0.0391974 9.86849 0.0370628 -0.0291595 0.998887 5.87854\n0.0378791 0.99875 0.0326271 -569.496 -0.997331 0.0357455 0.0636657 11.6091 0.0624198 -0.0349516 0.997438 5.87768\n0.0190418 0.999393 0.029176 -569.518 -0.998911 0.0177729 0.0431483 12.7502 0.0426035 -0.0299658 0.998643 5.85198\n0.00916548 0.999399 0.0334382 -569.495 -0.999703 0.00840267 0.0228821 13.902 0.0225874 -0.0336379 0.999179 5.82486\n-0.000250877 0.999627 0.0273238 -569.507 -0.999472 -0.00113818 0.0324631 15.7514 0.0324821 -0.0273012 0.999099 5.80554\n-0.00435624 0.999578 0.0287165 -569.471 -0.999502 -0.00524993 0.0311194 17.6125 0.0312571 -0.0285666 0.999103 5.8017\n-0.00583657 0.999654 0.0256456 -569.45 -0.999507 -0.00662308 0.0306912 19.524 0.0308504 -0.0254538 0.9992 5.7673\n-0.00796209 0.999631 0.0259834 -569.416 -0.999401 -0.00882982 0.0334535 21.4733 0.0336706 -0.0257015 0.999102 5.75569\n-0.00937688 0.999538 0.028927 -569.386 -0.999524 -0.0102196 0.0291224 23.4789 0.0294046 -0.0286401 0.999157 5.71595\n-0.0093671 0.999256 0.037418 -569.341 -0.999399 -0.0106045 0.0330101 25.5281 0.0333824 -0.0370863 0.998754 5.70438\n-0.00878897 0.999197 0.0391031 -569.314 -0.999374 -0.0101169 0.0338928 26.9525 0.0342612 -0.0387808 0.99866 5.68576\n-0.00809979 0.999025 0.0434098 -569.259 -0.999464 -0.00946451 0.0313255 29.1094 0.0317058 -0.0431328 0.998566 5.66575\n-0.00859079 0.999049 0.0427429 -569.252 -0.999346 -0.0100795 0.0347372 30.5719 0.035135 -0.0424165 0.998482 5.65398\n-0.00723438 0.998999 0.0441324 -569.238 -0.999547 -0.00851396 0.0288755 32.8219 0.0292223 -0.0439035 0.998608 5.61998\n-0.00680394 0.99897 0.0448582 -569.198 -0.999641 -0.00795695 0.0255751 34.3662 0.0259056 -0.0446681 0.998666 5.61378\n-0.00590974 0.998884 0.0468684 -569.191 -0.999656 -0.00709869 0.025242 35.9119 0.0255466 -0.0467031 0.998582 5.60155\n-0.00481732 0.998694 0.0508579 -569.16 -0.999705 -0.00601999 0.0235211 38.2796 0.0237966 -0.0507296 0.998429 5.58967\n-0.00427881 0.998493 0.0547074 -569.146 -0.999672 -0.00565149 0.0249612 39.8988 0.0252328 -0.0545827 0.99819 5.59022\n-0.00387743 0.998398 0.056456 -569.122 -0.999689 -0.00526061 0.0243721 41.5044 0.0246301 -0.0563439 0.998108 5.5818\n-0.00313651 0.998227 0.0594335 -569.1 -0.999681 -0.00461966 0.0248338 43.1587 0.0250643 -0.0593366 0.997923 5.57473\n-0.00305477 0.997471 0.0710069 -569.061 -0.999493 -0.00529675 0.0314073 45.6333 0.031704 -0.0708749 0.996981 5.55352\n-0.00219363 0.997099 0.0760821 -569.034 -0.999541 -0.00448469 0.0299552 47.3096 0.0302095 -0.0759815 0.996651 5.54753\n-0.000861037 0.996834 0.0795058 -569.015 -0.999559 -0.00321906 0.0295351 48.9993 0.0296975 -0.0794453 0.996397 5.54404\n0.00131952 0.996933 0.078245 -569.023 -0.999543 -0.001048 0.030209 50.691 0.0301984 -0.0782491 0.996476 5.5348\n0.00190664 0.997747 0.067055 -569.027 -0.999645 0.000119681 0.0266431 53.2084 0.0265751 -0.067082 0.997393 5.54219\n0.00205102 0.998122 0.0612182 -569.031 -0.999432 -1.39478e-05 0.0337118 54.8971 0.0336494 -0.0612525 0.997555 5.53657\n0.00162857 0.998427 0.0560408 -569.039 -0.999354 -0.000387349 0.0359427 57.4472 0.0359079 -0.0560631 0.997781 5.50253\n0.000778352 0.998513 0.0545138 -569.032 -0.999317 -0.00123721 0.0369299 59.1216 0.0369424 -0.0545053 0.99783 5.47277\n0.000532481 0.998549 0.0538454 -569.019 -0.999343 -0.0014193 0.036203 60.7649 0.0362269 -0.0538294 0.997893 5.46938\n0.000613485 0.998688 0.0512103 -569.014 -0.99938 -0.00118994 0.0351781 62.4265 0.0351928 -0.0512001 0.998068 5.45086\n0.000590679 0.998489 0.0549415 -569.001 -0.999244 -0.00154612 0.0388417 64.0634 0.0388679 -0.0549229 0.997734 5.42115\n-0.000653438 0.998184 0.0602367 -569.004 -0.999405 -0.00272922 0.0343846 66.4566 0.0344866 -0.0601784 0.997592 5.40434\n0.000141602 0.998069 0.062108 -568.982 -0.999497 -0.00182788 0.0316526 68.0592 0.0317051 -0.0620813 0.997567 5.39173\n0.00121243 0.998034 0.0626608 -568.965 -0.999168 -0.0013447 0.0407508 69.6266 0.0407549 -0.0626581 0.997203 5.36614\n0.000703197 0.998468 0.0553287 -568.979 -0.998981 -0.00179578 0.0451033 71.1205 0.0451335 -0.055304 0.997449 5.40359\n0.00072545 0.99884 0.0481566 -568.99 -0.999712 -0.000429763 0.023974 72.5792 0.0239669 -0.0481601 0.998552 5.38315\n-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\n0.000874616 0.99925 0.0387205 -568.992 -0.999458 -0.000401045 0.0329253 76.046 0.0329161 -0.0387283 0.998707 5.36294\n0.000386348 0.999137 0.0415384 -568.967 -0.999682 -0.000662265 0.0252277 78.0452 0.0252334 -0.0415349 0.998818 5.36022\n0.00113285 0.999167 0.0407919 -568.974 -0.999647 4.89315e-05 0.0265631 79.3788 0.026539 -0.0408076 0.998815 5.35357\n0.000202282 0.998877 0.04737 -568.973 -0.999999 0.000123279 0.00167071 80.6667 0.00166299 -0.0473703 0.998876 5.38485\n0.00227565 0.998753 0.0498646 -568.935 -0.999645 0.000948046 0.0266316 82.6861 0.0265511 -0.0499075 0.998401 5.32998\n0.00387565 0.998604 0.0526848 -568.927 -0.997095 -0.000148677 0.0761674 84.0392 0.0760689 -0.0528269 0.995702 5.31881\n0.00138086 0.998409 0.0563699 -568.925 -0.999046 -0.00108263 0.0436483 85.3217 0.0436399 -0.0563764 0.997455 5.37409\n-0.000926067 0.998518 0.0544159 -568.919 -0.999961 -0.00140265 0.00872057 86.6467 0.00878397 -0.0544057 0.99848 5.31879\n-0.000600425 0.9985 0.0547536 -568.91 -0.999527 -0.00228223 0.0306586 88.0502 0.0307376 -0.0547093 0.998029 5.29375\n-0.00100997 0.998557 0.0536988 -568.903 -0.999524 -0.00266439 0.0307467 90.103 0.0308454 -0.0536421 0.998084 5.29672\n-0.00121846 0.998489 0.0549315 -568.902 -0.999709 -0.00253865 0.02397 91.488 0.0240733 -0.0548864 0.998202 5.28172\n-0.000689193 0.998696 0.0510435 -568.907 -0.999587 -0.00215483 0.0286639 92.9035 0.0287366 -0.0510027 0.998285 5.28115\n-0.000230422 0.998819 0.0485793 -568.908 -0.999626 -0.00155813 0.0272947 94.2952 0.0273381 -0.0485548 0.998446 5.28027\n-0.000171665 0.998963 0.0455312 -568.903 -0.999657 -0.00136385 0.0261541 95.6943 0.0261891 -0.0455111 0.99862 5.27016\n0.000234362 0.998925 0.0463491 -568.892 -0.99957 -0.00112466 0.0292933 97.0958 0.0293139 -0.046336 0.998496 5.24581\n0.000461175 0.998866 0.0476084 -568.89 -0.999467 -0.00109355 0.0326254 98.4676 0.0326404 -0.047598 0.998333 5.25558\n0.000845986 0.998747 0.0500414 -568.878 -0.999535 -0.000680031 0.0304702 99.8397 0.0304661 -0.0500439 0.998282 5.23394\n0.000316358 0.998632 0.0522912 -568.872 -0.99955 -0.00125201 0.0299575 101.869 0.0299819 -0.0522771 0.998182 5.2402\n-0.000386352 0.998691 0.0511515 -568.867 -0.999536 -0.00194327 0.0303912 103.248 0.0304508 -0.051116 0.998228 5.22684\n-0.00161372 0.99883 0.0483387 -568.869 -0.999538 -0.003079 0.0302537 104.612 0.0303671 -0.0482676 0.998373 5.23986\n-0.00234222 0.999015 0.0443011 -568.859 -0.999512 -0.00371881 0.0310167 105.983 0.0311509 -0.0442068 0.998537 5.22603\n-0.0027358 0.999037 0.0437928 -568.86 -0.999529 -0.0040698 0.0304016 107.364 0.0305505 -0.043689 0.998578 5.20975\n-0.00354773 0.998953 0.0456027 -568.851 -0.999897 -0.00417755 0.013723 108.71 0.0138991 -0.0455493 0.998865 5.19832\n-0.00407342 0.998483 0.054908 -568.825 -0.999936 -0.00348566 -0.010796 110.063 -0.0105882 -0.0549484 0.998433 5.25919\n-0.00171532 0.999196 0.0400488 -568.845 -0.999715 -0.0026669 0.0237192 111.503 0.0238069 -0.0399967 0.998916 5.2224\n0.00174254 0.999244 0.0388364 -568.831 -0.997185 -0.00117469 0.0749666 112.934 0.0749556 -0.0388577 0.996429 5.19537\n-0.000376636 0.999323 0.0367943 -568.835 -0.999486 -0.00155577 0.0320233 114.219 0.0320588 -0.0367633 0.99881 5.2718\n0.000101892 0.999499 0.031659 -568.846 -0.999997 0.000181114 -0.00249952 115.617 -0.002504 -0.0316586 0.999496 5.20019\n0.000289135 0.999419 0.0340905 -568.834 -0.999717 -0.000522741 0.023804 117.063 0.023808 -0.0340877 0.999135 5.18427\n-0.000365855 0.999202 0.0399283 -568.817 -0.999655 -0.0014143 0.0262332 118.437 0.0262687 -0.0399049 0.998858 5.21425\n-0.00112576 0.998949 0.045818 -568.803 -0.999845 -0.00192866 0.0174831 119.845 0.0175531 -0.0457912 0.998797 5.20218\n-0.00147551 0.997933 0.0642524 -568.772 -0.999619 -0.00324341 0.0274193 122.613 0.027571 -0.0641875 0.997557 5.20014\n-0.00229993 0.997705 0.0676764 -568.749 -0.999699 -0.00394697 0.0242133 123.978 0.0244249 -0.0676003 0.997413 5.20817\n-0.00253771 0.997677 0.0680728 -568.725 -0.999654 -0.00431316 0.0259474 126.032 0.0261808 -0.0679834 0.997343 5.21525\n-0.00327657 0.997734 0.0672014 -568.727 -0.999819 -0.00452852 0.0184859 127.373 0.0187483 -0.0671287 0.997568 5.21248\n-0.000950308 0.997766 0.0667919 -568.717 -0.999697 -0.00259037 0.0244725 129.423 0.0245908 -0.0667484 0.997467 5.22158\n0.00289788 0.997724 0.0673654 -568.709 -0.998205 -0.00114395 0.0598827 130.817 0.0598235 -0.067418 0.99593 5.17521\n0.00259492 0.996941 0.0781088 -568.68 -0.99953 0.000198626 0.0306711 132.803 0.0305618 -0.0781516 0.996473 5.16182\n0.00352975 0.99661 0.0821973 -568.676 -0.99943 0.00075621 0.0337492 134.154 0.0335727 -0.0822695 0.996044 5.13371\n0.00325845 0.996124 0.087904 -568.656 -0.999207 -0.000243972 0.0398036 135.485 0.0396708 -0.087964 0.995333 5.13119\n-0.00159921 0.995352 0.0962943 -568.635 -0.999576 -0.0043923 0.0288008 137.473 0.0290899 -0.0962074 0.994936 5.11703\n-0.00507597 0.994863 0.101106 -568.616 -0.999539 -0.00807531 0.0292782 138.809 0.0299442 -0.100911 0.994445 5.10528\n-0.0114637 0.994672 0.102448 -568.599 -0.999575 -0.0141437 0.025472 140.095 0.0267853 -0.102112 0.994412 5.13158\n-0.0209828 0.994505 0.102568 -568.546 -0.999621 -0.022695 0.0155552 141.406 0.0177975 -0.102203 0.994604 5.10613\n-0.0370943 0.993665 0.106081 -568.435 -0.998709 -0.0405502 0.030608 143.358 0.0347157 -0.104808 0.993886 5.11341\n-0.0577576 0.99322 0.100889 -568.351 -0.997713 -0.0609802 0.0291529 144.616 0.0351075 -0.098975 0.99447 5.10382\n-0.0873696 0.990927 0.102126 -568.22 -0.995732 -0.0899304 0.020737 145.858 0.0297331 -0.0998785 0.994555 5.08467\n-0.125437 0.986915 0.101312 -568.031 -0.991645 -0.127821 0.0173674 147.077 0.03009 -0.0982867 0.994703 5.09234\n-0.167757 0.981196 0.0954577 -567.814 -0.985345 -0.169918 0.0149123 148.305 0.0308518 -0.0915571 0.995322 5.07554\n-0.21235 0.972386 0.0968132 -567.515 -0.976636 -0.214532 0.0125971 149.506 0.0330187 -0.0918762 0.995223 5.05119\n-0.283036 0.955444 0.0837672 -566.979 -0.958802 -0.284076 0.000516312 151.257 0.0242895 -0.08017 0.996485 5.07124\n-0.376144 0.923214 0.0786828 -566.021 -0.926131 -0.3772 -0.00155496 153.548 0.0282436 -0.0734554 0.996898 5.01218\n-0.424506 0.902119 0.0773008 -565.46 -0.904908 -0.425599 -0.00256828 154.644 0.0305822 -0.0710404 0.997005 5.04475\n-0.471205 0.878911 0.0740356 -564.844 -0.881419 -0.472329 -0.00262433 155.729 0.0326626 -0.066493 0.997252 4.98811\n-0.518082 0.851199 0.083973 -564.149 -0.854406 -0.519585 -0.00455407 156.766 0.0397547 -0.0741065 0.996458 4.95396\n-0.565206 0.820417 0.0863638 -563.394 -0.824068 -0.566339 -0.0131232 157.759 0.0381447 -0.078587 0.996177 4.96652\n-0.61064 0.788342 0.0750701 -562.608 -0.790934 -0.611843 -0.00844547 158.737 0.0392732 -0.0645326 0.997142 4.95538\n-0.652371 0.754045 0.0763438 -561.738 -0.756447 -0.654043 -0.0039989 159.697 0.0469168 -0.0603588 0.997074 4.87556\n-0.693989 0.715409 0.0810494 -560.808 -0.718235 -0.695747 -0.00868359 160.57 0.0501775 -0.0642388 0.996672 4.87103\n-0.734623 0.674561 0.0727745 -559.825 -0.676902 -0.735992 -0.0109354 161.416 0.0461849 -0.0572946 0.997288 4.83455\n-0.788824 0.610084 0.074523 -558.238 -0.612468 -0.790399 -0.0123442 162.586 0.0513719 -0.0553804 0.997143 4.76372\n-0.821103 0.565916 0.0743512 -557.122 -0.56833 -0.822669 -0.0147442 163.303 0.0528225 -0.0543625 0.997123 4.73086\n-0.848316 0.524307 0.0738985 -555.951 -0.526469 -0.850109 -0.012093 163.976 0.0564813 -0.0491639 0.997192 4.68351\n-0.870806 0.485763 0.075701 -554.732 -0.488042 -0.872708 -0.0140158 164.603 0.0592565 -0.0491503 0.997032 4.62918\n-0.891141 0.44767 0.0738878 -553.494 -0.449768 -0.893038 -0.0138129 165.186 0.059801 -0.0455416 0.997171 4.57394\n-0.907463 0.412761 0.0783518 -552.197 -0.415111 -0.909634 -0.015778 165.732 0.0647589 -0.0468427 0.996801 4.50062\n-0.920813 0.380665 0.0848372 -550.894 -0.383391 -0.923412 -0.0179204 166.224 0.071518 -0.0490271 0.996234 4.44367\n-0.93269 0.350687 0.08431 -549.575 -0.353417 -0.935263 -0.0195015 166.697 0.072013 -0.0479855 0.996249 4.38422\n-0.942425 0.324758 0.0797996 -548.259 -0.327351 -0.944658 -0.0215248 167.128 0.0683929 -0.0464079 0.996579 4.32236\n-0.949301 0.303696 0.0812231 -546.904 -0.306537 -0.951534 -0.0248496 167.523 0.0697398 -0.0484876 0.996386 4.24293\n-0.956726 0.279899 0.0795769 -544.917 -0.282387 -0.959054 -0.0217251 168.094 0.0702378 -0.0432565 0.996592 4.18212\n-0.961138 0.265707 0.0749212 -543.57 -0.267733 -0.963321 -0.0182433 168.461 0.0673258 -0.0375932 0.997023 4.10127\n-0.96427 0.254233 0.0744916 -542.199 -0.256109 -0.966505 -0.0166547 168.802 0.0677623 -0.0351376 0.997083 4.04141\n-0.966757 0.249181 0.05735 -540.148 -0.250222 -0.968119 -0.0116336 169.348 0.0526227 -0.0255971 0.998286 3.97513\n-0.968046 0.24669 0.0450689 -538.777 -0.247632 -0.968712 -0.0165967 169.673 0.0395645 -0.0272268 0.998846 3.945\n-0.965748 0.245549 0.0838806 -537.3 -0.246266 -0.9692 0.00185281 170.031 0.0817521 -0.0188676 0.996474 3.81768\n-0.965159 0.244473 0.0932794 -535.923 -0.244926 -0.969518 0.00673065 170.411 0.0920815 -0.0163504 0.995617 3.77936\n-0.968591 0.24425 0.0466207 -534.556 -0.244722 -0.969582 -0.00461839 170.753 0.0440746 -0.0158825 0.998902 3.72577\n-0.967719 0.244544 0.0609725 -533.096 -0.244964 -0.969532 0.000593179 171.099 0.0592599 -0.0143621 0.998139 3.6748\n-0.966536 0.246704 0.0703151 -531.613 -0.247373 -0.96892 -0.000830988 171.482 0.0679247 -0.0181973 0.997524 3.60051\n-0.966136 0.250734 0.0609428 -530.145 -0.251599 -0.967807 -0.00683728 171.852 0.0572666 -0.0219389 0.998118 3.55524\n-0.964136 0.257798 0.0631012 -527.885 -0.258693 -0.965939 -0.00630089 172.451 0.0593276 -0.0223988 0.997987 3.47561\n-0.962818 0.262495 0.0638652 -526.356 -0.263333 -0.964693 -0.00492662 172.87 0.060317 -0.0215612 0.997946 3.41415\n-0.961597 0.267013 0.0635189 -524.825 -0.267819 -0.963459 -0.00436655 173.298 0.060032 -0.0212104 0.997971 3.35885\n-0.960797 0.270184 0.0622034 -523.293 -0.270844 -0.962621 -0.00227811 173.733 0.0592628 -0.0190362 0.998061 3.30954\n-0.960566 0.271556 0.0597474 -521.738 -0.272191 -0.96224 -0.00260379 174.173 0.0567842 -0.0187638 0.99821 3.25247\n-0.960556 0.271564 0.0598717 -520.159 -0.272235 -0.962226 -0.00318447 174.612 0.0567453 -0.019358 0.998201 3.19629\n-0.960722 0.270586 0.0616165 -516.132 -0.271172 -0.96253 -0.00119613 175.743 0.0589841 -0.0178578 0.998099 3.02616\n-0.960802 0.269554 0.0648147 -514.491 -0.270266 -0.962783 -0.00231154 176.198 0.0617794 -0.0197381 0.997895 2.96062\n-0.960875 0.26838 0.0684888 -512.859 -0.269346 -0.96303 -0.00510614 176.652 0.0645864 -0.0233535 0.997639 2.89699\n-0.961714 0.266444 0.0641446 -510.379 -0.267365 -0.963576 -0.00606322 177.339 0.0601927 -0.0229811 0.997922 2.81036\n-0.96207 0.265111 0.0643144 -508.693 -0.265923 -0.963985 -0.00425302 177.799 0.0608706 -0.0211944 0.997921 2.73769\n-0.962417 0.263961 0.0638672 -506.995 -0.264725 -0.964317 -0.00365583 178.26 0.0606232 -0.0204257 0.997952 2.68455\n-0.962169 0.264407 0.0657193 -505.273 -0.265394 -0.964117 -0.00662349 178.733 0.0616098 -0.0238144 0.997816 2.61061\n-0.961108 0.268178 0.0659609 -503.535 -0.269199 -0.963059 -0.00694448 179.207 0.0616619 -0.024431 0.997798 2.54178\n-0.959943 0.272829 0.0638228 -501.79 -0.273703 -0.9618 -0.00521262 179.704 0.0599626 -0.0224723 0.997948 2.4765\n-0.958518 0.277545 0.0648971 -500.044 -0.278494 -0.96042 -0.00587732 180.212 0.0606972 -0.0237069 0.997875 2.41188\n-0.956564 0.283386 0.0683953 -498.266 -0.284552 -0.958629 -0.00773884 180.733 0.0633727 -0.0268647 0.997628 2.33862\n-0.954855 0.289304 0.0674964 -496.486 -0.290664 -0.956761 -0.0110764 181.259 0.0613735 -0.0301951 0.997658 2.26713\n-0.952893 0.295728 0.067379 -494.702 -0.297041 -0.954811 -0.0101638 181.811 0.0613285 -0.0296994 0.997676 2.19273\n-0.949876 0.30695 0.0593087 -492.002 -0.308144 -0.951265 -0.011929 182.684 0.0527567 -0.0296067 0.998168 2.09245\n-0.947342 0.314006 0.0627882 -490.192 -0.315405 -0.948861 -0.0135136 183.278 0.0553339 -0.0326058 0.997935 2.04252\n-0.945132 0.319685 0.0672837 -488.416 -0.321433 -0.946785 -0.0167101 183.873 0.0583613 -0.0374204 0.997594 1.99674\n-0.942389 0.329149 0.059708 -485.766 -0.330841 -0.943457 -0.0208233 184.792 0.049478 -0.0393775 0.997999 1.92392\n-0.93861 0.339181 0.0629951 -483.103 -0.340749 -0.940022 -0.0157598 185.753 0.0538714 -0.0362578 0.997889 1.87254\n-0.93591 0.346185 0.0650291 -481.333 -0.347663 -0.937535 -0.0126124 186.411 0.0566008 -0.0344123 0.997804 1.8138\n-0.933467 0.353847 0.0585864 -479.566 -0.355038 -0.934787 -0.0109977 187.087 0.0508743 -0.0310664 0.998222 1.7443\n-0.928558 0.366535 0.0585785 -476.931 -0.367625 -0.929933 -0.00867641 188.129 0.0512939 -0.0295915 0.998245 1.67382\n-0.925066 0.374841 0.0612222 -475.179 -0.376024 -0.92657 -0.00866353 188.842 0.0534792 -0.0310353 0.998087 1.63104\n-0.920015 0.38764 0.0575111 -472.539 -0.3887 -0.921329 -0.0080955 189.951 0.0498485 -0.0298025 0.998312 1.55218\n-0.916553 0.395795 0.0572389 -470.784 -0.396915 -0.917809 -0.00925544 190.712 0.0488711 -0.0312021 0.998318 1.50485\n-0.912693 0.404225 0.0599575 -469.023 -0.405383 -0.914111 -0.00807317 191.492 0.0515445 -0.0316741 0.998168 1.44543\n-0.907427 0.416655 0.0545355 -466.399 -0.417661 -0.908567 -0.00802632 192.692 0.0462049 -0.0300607 0.99848 1.3789\n-0.90318 0.42545 0.0570774 -464.647 -0.426575 -0.904411 -0.00862649 193.52 0.0479513 -0.032139 0.998332 1.33915\n-0.898431 0.43509 0.0593119 -462.896 -0.436342 -0.899732 -0.00941732 194.365 0.0492674 -0.0343411 0.998195 1.28941\n-0.893892 0.444918 0.0548234 -461.166 -0.44617 -0.89486 -0.0125584 195.227 0.0434718 -0.0356864 0.998417 1.25422\n-0.88887 0.455014 0.0535981 -459.412 -0.456178 -0.889818 -0.0112541 196.129 0.0425718 -0.0344537 0.998499 1.22159\n-0.883276 0.465444 0.0564356 -457.66 -0.466601 -0.884426 -0.00863299 197.058 0.0458949 -0.0339582 0.998369 1.17511\n-0.877808 0.475838 0.0550567 -455.93 -0.476935 -0.878902 -0.00803683 197.997 0.0445652 -0.0333133 0.998451 1.14899\n-0.872114 0.486741 0.0500079 -454.191 -0.487618 -0.873034 -0.00632421 198.973 0.0405804 -0.0299002 0.998729 1.10837\n-0.865939 0.497151 0.0546865 -452.445 -0.498073 -0.867127 -0.00379978 199.967 0.0455311 -0.0305283 0.998496 1.06901\n-0.856706 0.512431 0.058893 -449.839 -0.513754 -0.857891 -0.00894584 201.511 0.0459396 -0.0379205 0.998224 0.993044\n-0.850952 0.521954 0.0586886 -448.123 -0.523436 -0.851975 -0.0124006 202.56 0.0435287 -0.0412721 0.998199 0.948108\n-0.843283 0.534298 0.0583017 -445.587 -0.535915 -0.844127 -0.0156671 204.145 0.0408431 -0.0444566 0.998176 0.910519\n-0.838735 0.541343 0.0589167 -443.903 -0.542987 -0.839599 -0.0154515 205.223 0.0411018 -0.0449506 0.998143 0.880228\n-0.831902 0.552293 0.0539627 -441.445 -0.553739 -0.832541 -0.0157541 206.854 0.0362253 -0.0429872 0.998419 0.84226\n-0.82479 0.563246 0.0497409 -438.98 -0.564385 -0.82543 -0.0116287 208.521 0.0345078 -0.0376642 0.998694 0.812051\n-0.819671 0.570741 0.0489356 -437.365 -0.571687 -0.820443 -0.00683607 209.655 0.0362473 -0.0335792 0.998779 0.786303\n-0.81239 0.581148 0.0478565 -434.953 -0.581781 -0.813345 0.000863356 211.391 0.0394256 -0.0271406 0.998854 0.730965\n-0.80531 0.591174 0.0445893 -432.553 -0.591599 -0.806221 0.00440072 213.145 0.0385504 -0.022835 0.998996 0.689668\n-0.800199 0.598154 0.0435177 -430.963 -0.598561 -0.801065 0.00440553 214.334 0.0374957 -0.0225227 0.999043 0.653325\n-0.792535 0.608354 0.0423475 -428.598 -0.608844 -0.793288 0.00164556 216.138 0.0345949 -0.0244789 0.999102 0.611172\n-0.783496 0.619975 0.0420165 -426.297 -0.620491 -0.784213 0.000967611 217.953 0.0335498 -0.0253127 0.999116 0.595619\n-0.771624 0.63442 0.0459058 -423.268 -0.634821 -0.772626 0.00710671 220.401 0.0399767 -0.0236583 0.99892 0.566456\n-0.762728 0.644689 0.0512057 -421.071 -0.64517 -0.763989 0.0087009 222.246 0.04473 -0.0264 0.99865 0.515913\n-0.754056 0.653959 0.0611306 -418.981 -0.654866 -0.755716 0.00656411 224.027 0.05049 -0.0350826 0.998108 0.470869\n-0.746416 0.662948 0.0579928 -417.009 -0.663957 -0.747766 0.0024502 225.775 0.0449894 -0.0366759 0.998314 0.481316\n-0.736754 0.674344 0.0495249 -415.152 -0.674962 -0.737832 0.00549989 227.512 0.0402499 -0.0293753 0.998758 0.43022\n-0.729952 0.681484 0.0524387 -413.952 -0.681999 -0.731283 0.010139 228.636 0.0452571 -0.0283622 0.998573 0.421308\n-0.723871 0.688172 0.0492908 -412.821 -0.688572 -0.725084 0.0110555 229.727 0.0433481 -0.0259375 0.998723 0.413176\n-0.71421 0.698553 0.0439091 -411.217 -0.698774 -0.715231 0.012639 231.306 0.0402342 -0.0216557 0.998956 0.394309\n-0.704821 0.708055 0.043414 -409.703 -0.708188 -0.705867 0.0148969 232.833 0.0411924 -0.0202456 0.998946 0.373061\n-0.695084 0.717498 0.0453333 -408.222 -0.717725 -0.696187 0.013969 234.358 0.0415832 -0.0228272 0.998874 0.333774\n-0.686535 0.72557 0.0470925 -406.782 -0.725762 -0.687759 0.0160353 235.884 0.044023 -0.0231691 0.998762 0.300918\n-0.678985 0.732553 0.0484279 -405.363 -0.732792 -0.680264 0.0159966 237.416 0.0446621 -0.0246261 0.998699 0.270761\n-0.671429 0.739506 0.0480905 -403.98 -0.739956 -0.672563 0.0111469 238.95 0.0405871 -0.0281006 0.998781 0.240412\n-0.665084 0.746354 0.0248691 -402.645 -0.746293 -0.66548 0.0135226 240.497 0.0266425 -0.00956596 0.999599 0.256532\n-0.658354 0.752501 0.0176615 -401.754 -0.752683 -0.658342 -0.00731372 241.521 0.0061237 -0.0181085 0.999817 0.254201\n-0.648722 0.758485 0.062132 -400.329 -0.756403 -0.651614 0.0570407 243.183 0.0837506 -0.00999323 0.996437 0.138948\n-0.642371 0.762051 0.0814746 -399.42 -0.760207 -0.647055 0.0583507 244.212 0.0971847 -0.0244548 0.994966 0.0930214\n-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\n-0.632028 0.772699 0.0589647 -396.665 -0.773743 -0.633455 0.00750604 247.542 0.0431514 -0.0408795 0.998232 0.0017772\n-0.628038 0.775809 0.0607286 -395.714 -0.776983 -0.629489 0.00638874 248.705 0.0431844 -0.0431728 0.998134 -0.0225678\n-0.624033 0.779194 0.0586423 -394.76 -0.780299 -0.625377 0.00609439 249.909 0.0414222 -0.0419554 0.99826 -0.0493731\n-0.616534 0.785894 0.0474935 -393.336 -0.78666 -0.617374 0.00396136 251.744 0.0324345 -0.0349189 0.998864 -0.0525101\n-0.611822 0.790152 0.0365241 -392.376 -0.790592 -0.612333 0.00367123 253.011 0.0252657 -0.0266295 0.999326 -0.064127\n-0.604624 0.795704 0.0358406 -390.926 -0.79567 -0.605441 0.0187112 254.945 0.036588 -0.017204 0.999182 -0.0927086\n-0.600549 0.798882 0.0335861 -389.446 -0.798952 -0.601217 0.0146307 256.913 0.0318807 -0.0180472 0.999329 -0.135961\n-0.597264 0.801311 0.0343009 -388.447 -0.801405 -0.59795 0.0143728 258.24 0.0320273 -0.0189045 0.999308 -0.133784\n-0.592624 0.80462 0.0371821 -386.935 -0.804742 -0.593425 0.015374 260.278 0.034435 -0.0208109 0.99919 -0.172714\n-0.590786 0.805703 0.0425894 -385.91 -0.805853 -0.591844 0.0179313 261.679 0.0396536 -0.0237272 0.998932 -0.202273\n-0.586736 0.808489 0.0456745 -384.364 -0.808799 -0.587867 0.0160304 263.774 0.0398109 -0.0275358 0.998828 -0.244652\n-0.583322 0.81103 0.044337 -383.329 -0.811302 -0.584403 0.0161902 265.224 0.0390415 -0.0265266 0.998885 -0.271296\n-0.576808 0.815724 0.0434444 -381.776 -0.815665 -0.578034 0.0238139 267.426 0.0445379 -0.0217001 0.998772 -0.316715\n-0.572288 0.81904 0.040737 -380.727 -0.819023 -0.573352 0.021641 268.928 0.0410815 -0.0209797 0.998936 -0.362125\n-0.563947 0.824407 0.0481306 -379.142 -0.824426 -0.565418 0.024974 271.218 0.0478026 -0.0255961 0.998529 -0.440054\n-0.55704 0.828941 0.0506269 -378.082 -0.829352 -0.558429 0.0182244 272.784 0.0433785 -0.0318359 0.998551 -0.482308\n-0.546452 0.835855 0.0523072 -376.492 -0.83668 -0.547605 0.00980815 275.176 0.0368419 -0.0384047 0.998583 -0.533248\n-0.537664 0.840943 0.0610934 -375.432 -0.842114 -0.539193 0.0107366 276.815 0.04197 -0.0456749 0.998074 -0.554304\n-0.523963 0.849803 0.057428 -373.882 -0.851007 -0.525119 0.00611671 279.315 0.0353546 -0.0456667 0.998331 -0.586626\n-0.51326 0.85621 0.0588933 -372.854 -0.857258 -0.514738 0.0123479 281.037 0.040887 -0.0441491 0.998188 -0.611618\n-0.496372 0.866455 0.0535667 -371.345 -0.866991 -0.49792 0.0200657 283.666 0.044058 -0.0364817 0.998363 -0.65316\n-0.48568 0.873244 0.0394987 -370.385 -0.873389 -0.486638 0.0193928 285.45 0.0361562 -0.0250791 0.999031 -0.682743\n-0.480309 0.876454 0.0336392 -369.906 -0.876484 -0.481055 0.0190233 286.359 0.0328554 -0.0203472 0.999253 -0.705173\n-0.469237 0.882547 0.0304525 -368.931 -0.882509 -0.46989 0.0195254 288.187 0.0315414 -0.0177125 0.999345 -0.731836\n-0.463942 0.885277 0.0322865 -368.455 -0.885231 -0.464681 0.0209346 289.114 0.0335358 -0.0188686 0.999259 -0.752314\n-0.453266 0.890604 0.0370749 -367.498 -0.890545 -0.454247 0.0242801 290.964 0.0384651 -0.0220115 0.999017 -0.772249\n-0.443637 0.895415 0.0376712 -366.59 -0.895399 -0.444629 0.0237753 292.807 0.0380384 -0.0231831 0.999007 -0.787851\n-0.439654 0.89738 0.0375999 -366.141 -0.897364 -0.440645 0.023856 293.727 0.0379761 -0.0232524 0.999008 -0.806204\n-0.432788 0.900472 0.0429491 -365.243 -0.900513 -0.43405 0.0260324 295.572 0.0420835 -0.0274097 0.998738 -0.850337\n-0.425675 0.903325 0.0529603 -364.347 -0.903765 -0.427323 0.0245714 297.414 0.0448271 -0.0374043 0.998294 -0.867326\n-0.422291 0.90479 0.0550038 -363.905 -0.905419 -0.423936 0.0222423 298.336 0.0434427 -0.0404087 0.998238 -0.874\n-0.415192 0.907987 0.0563519 -363.053 -0.90882 -0.416754 0.0190317 300.19 0.0407654 -0.0433119 0.99823 -0.894195\n-0.407718 0.91196 0.0457713 -362.241 -0.912561 -0.408696 0.0141431 302.046 0.0316045 -0.0360027 0.998852 -0.886051\n-0.403514 0.914035 0.0414282 -361.834 -0.914486 -0.404363 0.0143557 302.997 0.0298737 -0.0320928 0.999038 -0.903059\n-0.395143 0.917919 0.035876 -361.025 -0.918054 -0.395969 0.0196425 304.897 0.0322361 -0.0251745 0.999163 -0.921957\n-0.386868 0.921508 0.0339924 -360.224 -0.921743 -0.387515 0.014864 306.786 0.0268698 -0.0255819 0.999312 -0.924449\n-0.378412 0.924953 0.0355937 -359.428 -0.925304 -0.379028 0.0122636 308.695 0.0248343 -0.0282943 0.999291 -0.94013\n-0.374529 0.926355 0.0399276 -359.042 -0.926739 -0.37537 0.0159059 309.658 0.0297221 -0.0310453 0.999076 -0.963692\n-0.365672 0.929335 0.0511846 -358.252 -0.929917 -0.367111 0.0219823 311.598 0.0392194 -0.0395591 0.998447 -0.936328\n-0.358117 0.932603 0.0447564 -357.527 -0.933186 -0.359071 0.0152069 313.516 0.0302527 -0.0363202 0.998882 -0.94747\n-0.344388 0.937744 0.0450848 -356.43 -0.937915 -0.345773 0.0275045 316.465 0.0413813 -0.0328135 0.998604 -0.989291\n-0.335965 0.941004 0.0404759 -355.744 -0.941185 -0.337053 0.0238008 318.405 0.0360392 -0.0300991 0.998897 -1.00158\n-0.33174 0.9426 0.038137 -355.407 -0.94306 -0.332397 0.0122368 319.374 0.024211 -0.031906 0.999198 -0.985361\n-0.320675 0.946396 0.0387421 -354.723 -0.947034 -0.321094 0.00495333 321.365 0.0171277 -0.0351016 0.999237 -1.01224\n-0.30771 0.95044 0.0444886 -354.068 -0.951105 -0.308565 0.0136643 323.359 0.0267147 -0.0381087 0.998916 -0.993969\n-0.301767 0.952277 0.0458722 -353.749 -0.953022 -0.302625 0.0129186 324.344 0.0261841 -0.0398188 0.998864 -0.997092\n-0.289995 0.955815 0.0481648 -353.133 -0.956814 -0.290628 0.00656286 326.339 0.0202709 -0.0441816 0.998818 -0.991141\n-0.278435 0.959279 0.0475056 -352.557 -0.960301 -0.278935 0.00409587 328.343 0.01718 -0.0444793 0.998863 -0.973051\n-0.266204 0.962725 0.0479165 -351.987 -0.963704 -0.26686 0.00774385 330.374 0.0202422 -0.0441159 0.998821 -0.961042\n-0.260059 0.964366 0.048658 -351.706 -0.965312 -0.260869 0.011 331.39 0.0233014 -0.0441095 0.998755 -0.969456\n-0.248778 0.967475 0.0458456 -351.199 -0.968351 -0.24943 0.00900107 333.378 0.0201436 -0.0421553 0.998908 -0.914849\n-0.237227 0.970458 0.043985 -350.71 -0.971364 -0.237579 0.00288659 335.38 0.0132512 -0.0420407 0.999028 -0.912954\n-0.225222 0.972728 0.0554522 -350.217 -0.974045 -0.226116 0.0103425 337.399 0.0225991 -0.0516836 0.998408 -0.922845\n-0.213934 0.97577 0.0458822 -349.798 -0.976452 -0.214948 0.0184002 339.392 0.0278166 -0.0408654 0.998777 -0.874937\n-0.201571 0.978619 0.0409128 -349.399 -0.97935 -0.202035 0.00749873 341.377 0.0156042 -0.0385564 0.999135 -0.896563\n-0.187514 0.981433 0.0403416 -349.019 -0.982118 -0.188031 0.00941382 343.38 0.0168245 -0.037855 0.999142 -0.835827\n-0.180371 0.982885 0.0374686 -348.845 -0.983444 -0.180887 0.0108437 344.391 0.0174357 -0.0348923 0.999239 -0.845921\n-0.163426 0.985808 0.0384119 -348.502 -0.986426 -0.163912 0.00986467 346.399 0.0160209 -0.0362784 0.999213 -0.820838\n-0.145404 0.988726 0.0357628 -348.208 -0.989032 -0.146208 0.020962 348.436 0.0259544 -0.0323226 0.99914 -0.811433\n-0.126572 0.991312 0.0357666 -347.943 -0.991596 -0.127417 0.0224243 350.45 0.0267868 -0.0326277 0.999109 -0.836122\n-0.0996678 0.994102 0.0427607 -347.589 -0.994833 -0.100392 0.0151436 353.482 0.0193471 -0.0410304 0.998971 -0.790071\n-0.092645 0.994923 0.0393103 -347.521 -0.995576 -0.0931831 0.0120802 354.503 0.0156819 -0.0380172 0.999154 -0.772196\n-0.0772523 0.996557 0.0300962 -347.374 -0.99701 -0.0772635 -0.000790452 356.508 0.00153761 -0.0300673 0.999547 -0.74105\n-0.0705495 0.996997 0.0319181 -347.298 -0.997505 -0.0704311 -0.00481935 357.526 -0.00255685 -0.0321785 0.999479 -0.720295\n-0.0569275 0.997856 0.0322933 -347.174 -0.998372 -0.05701 0.00164013 359.589 0.00347766 -0.0321474 0.999477 -0.675168\n-0.0500197 0.998237 0.0319582 -347.126 -0.998692 -0.0503316 0.00903149 360.624 0.0106241 -0.0314646 0.999448 -0.668925\n-0.0368865 0.998688 0.0355219 -347.036 -0.999107 -0.0375882 0.0192937 362.663 0.0206036 -0.0347785 0.999183 -0.636198\n-0.032314 0.999003 0.030815 -347.018 -0.999295 -0.0328817 0.0180951 363.682 0.0190903 -0.0302085 0.999361 -0.605864\n-0.023903 0.99947 0.0220764 -346.979 -0.999679 -0.0240807 0.0078191 365.726 0.00834658 -0.0218825 0.999726 -0.598268\n-0.0202976 0.999604 0.0194869 -346.956 -0.999766 -0.0204402 0.00714457 366.749 0.00754006 -0.0193373 0.999785 -0.592589\n-0.0129908 0.999753 0.0180492 -346.923 -0.999824 -0.0132313 0.0132689 368.805 0.0135045 -0.0178736 0.999749 -0.563252\n-0.0108416 0.999808 0.0163304 -346.914 -0.999819 -0.0110946 0.0154863 369.821 0.0156645 -0.0161595 0.999747 -0.562435\n-0.00789124 0.999875 0.0137349 -346.891 -0.999853 -0.00809881 0.0151234 371.85 0.0152327 -0.0136135 0.999791 -0.52786\n-0.00698128 0.99988 0.0138516 -346.885 -0.999888 -0.00716385 0.0131748 372.862 0.0132724 -0.0137581 0.999817 -0.534871\n-0.00742854 0.999888 0.0130162 -346.866 -0.999902 -0.00758165 0.0117539 374.856 0.0118513 -0.0129276 0.999846 -0.501295\n-0.0123636 0.999842 0.0128081 -346.839 -0.999848 -0.0125193 0.0121496 376.837 0.0123081 -0.0126559 0.999844 -0.480527\n-0.0178436 0.999716 0.0157737 -346.805 -0.999761 -0.0180397 0.0123762 378.822 0.0126572 -0.0155491 0.999799 -0.430987\n-0.0233914 0.999486 0.0219412 -346.739 -0.999716 -0.0234841 0.00397409 380.786 0.00448732 -0.021842 0.999751 -0.381342\n-0.0323074 0.999209 0.0231889 -346.646 -0.999478 -0.0323042 -0.000515511 383.703 0.000233996 -0.0231935 0.999731 -0.311528\n-0.0423902 0.998853 0.0222575 -346.536 -0.99896 -0.0427476 0.0158342 386.62 0.0167674 -0.0215632 0.999627 -0.251043\n-0.0512261 0.998479 0.0203782 -346.387 -0.998566 -0.0515275 0.014548 389.424 0.0155759 -0.0196037 0.999686 -0.208392\n-0.0681803 0.997628 0.00951416 -346.223 -0.997672 -0.0681658 -0.00183211 392.131 -0.00117922 -0.00961693 0.999953 -0.129763\n-0.0857747 0.996312 0.00213568 -346.069 -0.996312 -0.0857691 -0.0025698 393.922 -0.00237714 -0.00234822 0.999994 -0.098648\n-0.106834 0.994256 0.00647136 -345.863 -0.994253 -0.106874 0.00617184 395.701 0.00682801 -0.00577481 0.99996 -0.0567023\n-0.129407 0.991559 0.00807856 -345.625 -0.991492 -0.129505 0.0130973 397.459 0.0140329 -0.00631495 0.999882 -0.025314\n-0.151528 0.988377 0.0122909 -345.342 -0.988293 -0.151715 0.0160958 399.197 0.0177734 -0.00970805 0.999795 -0.0202483\n-0.186481 0.982361 0.0138344 -344.851 -0.982437 -0.186553 0.00404043 401.734 0.00655 -0.0128379 0.999896 0.0386244\n-0.21186 0.977165 0.0162204 -344.471 -0.977241 -0.212 0.00745913 403.421 0.0107275 -0.014271 0.999841 0.0598835\n-0.240575 0.970482 0.0170083 -344.034 -0.970452 -0.24083 0.014967 405.082 0.0186213 -0.0129051 0.999743 0.0772916\n-0.270575 0.962443 0.0221781 -343.564 -0.962635 -0.270751 0.0053114 406.686 0.0111167 -0.0199123 0.99974 0.106447\n-0.312872 0.949205 0.0334853 -342.764 -0.949599 -0.313329 0.0092663 409.064 0.0192875 -0.0288985 0.999396 0.149432\n-0.359626 0.932391 0.0362869 -341.859 -0.93277 -0.360257 0.0124733 411.387 0.0247026 -0.0293616 0.999264 0.164953\n-0.406734 0.912904 0.0342674 -340.857 -0.913341 -0.407152 0.00592827 413.608 0.019364 -0.0288866 0.999395 0.19592\n-0.43754 0.898596 0.0329252 -340.156 -0.898957 -0.437978 0.00716729 415.014 0.020861 -0.0264624 0.999432 0.198828\n-0.467152 0.883012 0.0453677 -339.401 -0.883603 -0.468082 0.0120079 416.344 0.031839 -0.0344775 0.998898 0.210749\n-0.499771 0.865688 0.0285206 -338.669 -0.866 -0.500037 0.0025744 417.581 0.01649 -0.0234123 0.99959 0.242324\n-0.528766 0.848008 0.0358933 -337.908 -0.848521 -0.529159 0.0017232 418.739 0.0204546 -0.0295451 0.999354 0.220186\n-0.558873 0.827813 0.0488423 -337.14 -0.828235 -0.560136 0.0165675 419.812 0.0410731 -0.0311937 0.998669 0.239649\n-0.604598 0.795069 0.0482329 -335.987 -0.795751 -0.605576 0.00756912 421.256 0.0352267 -0.0338051 0.998807 0.222373\n-0.653254 0.755998 0.0415342 -334.827 -0.756693 -0.653769 -0.00155623 422.563 0.0259773 -0.0324453 0.999136 0.227054\n-0.701185 0.712076 0.0358812 -333.587 -0.712682 -0.701458 -0.00642298 423.752 0.0205955 -0.0300756 0.999335 0.221437\n-0.745619 0.66518 0.0398488 -332.289 -0.665841 -0.746079 -0.00469795 424.856 0.0266054 -0.0300359 0.999195 0.231651\n-0.786899 0.616242 0.0321755 -330.954 -0.616492 -0.787357 0.0026636 425.882 0.026975 -0.01774 0.999479 0.246881\n-0.823956 0.566389 0.0173284 -329.581 -0.566654 -0.823548 -0.0259292 426.757 -0.00041523 -0.0311837 0.999514 0.229789\n-0.857399 0.511799 0.0541134 -328.117 -0.512173 -0.858847 0.00776525 427.572 0.0504494 -0.0210575 0.998505 0.23106\n-0.885861 0.459672 0.0628649 -326.63 -0.460306 -0.887747 0.00486467 428.301 0.0580443 -0.0246277 0.99801 0.184774\n-0.907834 0.418271 0.0297679 -325.679 -0.418786 -0.907983 -0.013614 428.687 0.0213344 -0.0248256 0.999464 0.194892\n-0.925241 0.378507 0.0257394 -324.624 -0.378875 -0.92538 -0.011185 429.066 0.0195851 -0.0201008 0.999606 0.135691\n-0.93887 0.341403 0.0443632 -323.561 -0.342204 -0.939553 -0.0116864 429.405 0.0376919 -0.0261533 0.998947 0.138445\n-0.958251 0.283756 0.0351713 -321.948 -0.284266 -0.958689 -0.0103585 429.851 0.0307791 -0.019924 0.999328 0.126702\n-0.973247 0.22667 0.0375756 -320.272 -0.227273 -0.97375 -0.0125826 430.193 0.0337371 -0.0207859 0.999215 0.0935861\n-0.98469 0.170333 0.0370511 -318.541 -0.170936 -0.985187 -0.0137168 430.454 0.0341658 -0.0198401 0.999219 0.0720024\n-0.992039 0.119627 0.0393409 -316.75 -0.12036 -0.992588 -0.0168089 430.609 0.0370385 -0.0214101 0.999084 0.0382554\n-0.997011 0.0662259 0.0397823 -314.301 -0.0671061 -0.99752 -0.0212131 430.747 0.0382788 -0.0238193 0.998983 0.00980817\n-0.998447 0.0375179 0.0411717 -312.388 -0.0383463 -0.999074 -0.01952 430.793 0.0404013 -0.0210685 0.998961 -0.0290521\n-0.99891 0.0214534 0.0414598 -310.442 -0.022197 -0.999599 -0.0175593 430.826 0.0410665 -0.0184604 0.998986 -0.0535458\n-0.999052 0.00867405 0.0426655 -308.452 -0.00930429 -0.99985 -0.0145953 430.831 0.0425325 -0.0149785 0.998983 -0.105808\n-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\n-0.99904 -0.00599227 0.0434022 -304.308 0.00482989 -0.999628 -0.0268373 430.785 0.0435468 -0.0266019 0.998697 -0.203029\n-0.999074 -0.00913861 0.0420369 -302.16 0.00783437 -0.999486 -0.0310869 430.736 0.0422994 -0.0307287 0.998632 -0.231218\n-0.999168 -0.0103859 0.0394362 -300.699 0.00914868 -0.999464 -0.0314247 430.707 0.0397414 -0.0310378 0.998728 -0.26124\n-0.999288 -0.00795551 0.036879 -299.208 0.00663638 -0.999339 -0.0357546 430.687 0.0371391 -0.0354844 0.99868 -0.27965\n-0.999394 -0.00483385 0.034467 -297.685 0.0036509 -0.999405 -0.0343018 430.674 0.0346123 -0.0341552 0.998817 -0.304836\n-0.999347 -0.000454754 0.0361418 -295.369 -0.000900404 -0.999297 -0.0374705 430.658 0.0361334 -0.0374785 0.998644 -0.342243\n-0.99929 0.00210573 0.0376249 -293.793 -0.00344112 -0.999365 -0.0354629 430.657 0.0375263 -0.0355672 0.998662 -0.376759\n-0.999041 0.00597537 0.0433849 -291.389 -0.00739045 -0.999443 -0.0325301 430.69 0.0431664 -0.0328195 0.998529 -0.360281\n-0.999257 0.00585616 0.0381047 -288.936 -0.00697288 -0.999548 -0.0292399 430.727 0.0379162 -0.0294839 0.998846 -0.419335\n-0.999428 0.00502039 0.0334336 -287.264 -0.00596382 -0.999585 -0.0281784 430.736 0.0332783 -0.0283617 0.999044 -0.445932\n-0.999301 0.00520134 0.0370298 -284.723 -0.00623287 -0.999594 -0.0277959 430.743 0.0368702 -0.0280072 0.998928 -0.474948\n-0.999346 0.00505458 0.035812 -282.137 -0.00586975 -0.999725 -0.0226942 430.764 0.0356875 -0.0228896 0.999101 -0.504726\n-0.999389 0.0051447 0.0345829 -279.523 -0.00597446 -0.999696 -0.0239332 430.748 0.0344493 -0.0241252 0.999115 -0.552249\n-0.999355 0.00440124 0.0356307 -276.861 -0.00540942 -0.999586 -0.0282484 430.742 0.0354917 -0.0284229 0.998966 -0.583409\n-0.999263 0.00350727 0.038234 -275.057 -0.00465944 -0.999536 -0.0300875 430.747 0.0381107 -0.0302435 0.998816 -0.605687\n-0.99933 0.000331842 0.0366023 -272.323 -0.00143895 -0.999542 -0.0302246 430.751 0.0365755 -0.030257 0.998873 -0.640452\n-0.999311 -0.00215364 0.0370422 -270.468 0.00104458 -0.999551 -0.029934 430.743 0.03709 -0.0298747 0.998865 -0.666402\n-0.999274 -0.0058178 0.037644 -267.634 0.00481321 -0.999631 -0.0267224 430.733 0.0377856 -0.0265218 0.998934 -0.708838\n-0.999221 -0.00827948 0.0385972 -265.714 0.00729068 -0.999643 -0.0256891 430.701 0.0387962 -0.0253877 0.998925 -0.747116\n-0.999109 -0.0112999 0.0406663 -263.782 0.0102825 -0.999631 -0.0251409 430.671 0.0409353 -0.0247004 0.998856 -0.77531\n-0.999101 -0.0143364 0.0399057 -260.884 0.0133295 -0.999589 -0.0253853 430.622 0.0402533 -0.0248305 0.998881 -0.829333\n-0.999036 -0.0142023 0.0415469 -258.933 0.0131262 -0.999574 -0.0260611 430.6 0.0418993 -0.0254906 0.998797 -0.863759\n-0.999151 -0.0137541 0.0388466 -256 0.0127906 -0.999607 -0.0249411 430.553 0.0391744 -0.0244231 0.998934 -0.89997\n-0.999013 -0.0118364 0.0428037 -253.032 0.0108075 -0.999649 -0.0241893 430.53 0.043075 -0.0237028 0.998791 -0.948568\n-0.999176 -0.00863944 0.039669 -250.122 0.00772692 -0.999703 -0.0230995 430.518 0.0398568 -0.022774 0.998946 -0.974487\n-0.998987 -0.00559102 0.0446585 -247.254 0.00427751 -0.999557 -0.029454 430.484 0.0448034 -0.0292331 0.998568 -1.01771\n-0.999169 -0.00215864 0.0407084 -244.402 0.000861025 -0.999492 -0.0318665 430.469 0.0407565 -0.0318049 0.998663 -1.05466\n-0.999343 0.00155007 0.0362041 -241.567 -0.00279366 -0.999407 -0.0343243 430.446 0.0361294 -0.0344029 0.998755 -1.08865\n-0.999203 0.00396813 0.0397211 -238.747 -0.00539844 -0.999338 -0.0359665 430.453 0.0395521 -0.0361523 0.998563 -1.11247\n-0.999182 0.00540628 0.0400792 -236.852 -0.00677622 -0.999395 -0.0341242 430.467 0.0398705 -0.0343679 0.998614 -1.13207\n-0.999129 0.00471816 0.0414667 -234.053 -0.00606373 -0.999457 -0.0323836 430.484 0.0412914 -0.0326068 0.998615 -1.18524\n-0.999127 0.0025856 0.0416855 -230.319 -0.00389795 -0.999498 -0.0314319 430.481 0.0415833 -0.0315669 0.998636 -1.24526\n-0.999231 0.00192689 0.0391596 -228.467 -0.00318945 -0.999476 -0.0322046 430.473 0.039077 -0.0323048 0.998714 -1.27939\n-0.999096 0.00118574 0.0424857 -225.691 -0.00258299 -0.999457 -0.0328478 430.469 0.0424237 -0.0329279 0.998557 -1.3049\n-0.999205 0.00127668 0.0398515 -223.875 -0.00261463 -0.999434 -0.0335393 430.471 0.0397861 -0.0336168 0.998643 -1.33444\n-0.99901 0.00139709 0.0444736 -221.183 -0.00299886 -0.999348 -0.03597 430.463 0.0443943 -0.0360678 0.998363 -1.37737\n-0.9989 0.00114476 0.0468801 -219.437 -0.0028616 -0.999327 -0.0365711 430.461 0.0468067 -0.036665 0.998231 -1.39895\n-0.998991 0.00111378 0.044905 -216.93 -0.00258672 -0.99946 -0.0327565 430.462 0.0448443 -0.0328395 0.998454 -1.4214\n-0.998971 0.00145832 0.0453405 -215.312 -0.00274421 -0.999595 -0.0283113 430.472 0.0452809 -0.0284066 0.99857 -1.4456\n-0.998919 0.00159422 0.0464506 -212.952 -0.00268092 -0.999724 -0.0233418 430.475 0.0464006 -0.0234411 0.998648 -1.48199\n-0.999089 0.0011795 0.0426506 -210.722 -0.00209088 -0.99977 -0.0213303 430.481 0.0426157 -0.0214 0.998862 -1.51281\n-0.999298 0.000435836 0.0374519 -209.266 -0.0011481 -0.999819 -0.0189988 430.473 0.0374369 -0.0190284 0.999118 -1.52396\n-0.999327 0.0011435 0.0366636 -207.054 -0.00204281 -0.999698 -0.0245007 430.471 0.0366245 -0.0245591 0.999027 -1.56022\n-0.999234 0.000682233 0.0391165 -205.587 -0.00175539 -0.999623 -0.0274072 430.465 0.039083 -0.0274549 0.998859 -1.58389\n-0.999601 0.000518554 0.0282275 -202.732 -0.00140438 -0.999507 -0.0313708 430.461 0.0281973 -0.0313979 0.999109 -1.60941\n-0.998366 0.00109616 0.0571325 -201.27 -0.00272218 -0.999593 -0.0283904 430.465 0.0570782 -0.0284996 0.997963 -1.64934\n-0.998391 -0.000566238 0.0567043 -199.884 -0.000667495 -0.999764 -0.021736 430.458 0.0567032 -0.0217389 0.998154 -1.62007\n-0.999695 0.000467345 0.0246901 -197.752 -0.000870483 -0.999866 -0.0163197 430.458 0.0246792 -0.0163362 0.999562 -1.67272\n-0.999236 0.000422933 0.03908 -196.301 -0.000818493 -0.999949 -0.0101064 430.469 0.0390737 -0.0101306 0.999185 -1.69741\n-0.999167 0.000435924 0.0407998 -194.863 -0.000752617 -0.99997 -0.00774708 430.476 0.0407952 -0.00777133 0.999137 -1.70257\n-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\n-0.999352 -0.00161746 0.0359616 -191.28 0.00125535 -0.999948 -0.0100899 430.457 0.035976 -0.0100382 0.999302 -1.75828\n-0.999329 -0.00586557 0.0361508 -189.109 0.00519678 -0.999814 -0.0185665 430.435 0.036253 -0.0183662 0.999174 -1.79769\n-0.999105 -0.0103092 0.0410338 -186.928 0.00926983 -0.999633 -0.0254397 430.383 0.041281 -0.0250365 0.998834 -1.81697\n-0.999104 -0.0145645 0.0397403 -184.724 0.0134222 -0.999493 -0.0288613 430.345 0.0401405 -0.0283021 0.998793 -1.85533\n-0.999101 -0.0185138 0.0381383 -182.512 0.0174987 -0.999488 -0.0267803 430.309 0.0386146 -0.0260888 0.998914 -1.88658\n-0.999332 -0.0202433 0.0304174 -179.555 0.0191838 -0.999213 -0.0347274 430.23 0.0310964 -0.0341207 0.998934 -1.91867\n-0.999206 -0.0194832 0.0347411 -178.04 0.0184339 -0.999372 -0.0302743 430.224 0.0353092 -0.0296098 0.998938 -1.92994\n-0.999075 -0.0171711 0.0394273 -175.822 0.0162194 -0.999572 -0.0243323 430.196 0.0398282 -0.0236703 0.998926 -1.95217\n-0.999175 -0.0176424 0.0365822 -173.609 0.0169267 -0.999661 -0.0197826 430.16 0.0369188 -0.0191471 0.999135 -1.98425\n-0.998998 -0.0183987 0.0408095 -172.138 0.0176212 -0.999658 -0.0193297 430.124 0.0411511 -0.0185912 0.99898 -2.0361\n-0.999042 -0.0202859 0.0387647 -169.97 0.0192295 -0.999439 -0.0274319 430.069 0.0392994 -0.0266602 0.998872 -2.02633\n-0.998935 -0.0192427 0.0419415 -168.527 0.0181521 -0.999491 -0.0262308 430.037 0.0424249 -0.0254415 0.998776 -2.06353\n-0.999008 -0.0188689 0.0403382 -166.396 0.0176805 -0.999405 -0.029619 429.993 0.040873 -0.0288764 0.998747 -2.09047\n-0.999028 -0.0150552 0.0414272 -164.288 0.0139035 -0.999513 -0.0279493 429.961 0.0418278 -0.0273461 0.998751 -2.11428\n-0.999207 -0.0102076 0.0384776 -162.214 0.00918307 -0.999601 -0.0267097 429.933 0.0387349 -0.0263352 0.998902 -2.14044\n-0.999344 -0.00742991 0.0354571 -160.851 0.00650443 -0.999637 -0.0261458 429.93 0.0356384 -0.025898 0.999029 -2.15796\n-0.999408 -0.00531427 0.033994 -158.799 0.00445718 -0.999671 -0.0252395 429.915 0.034117 -0.025073 0.999103 -2.18543\n-0.999988 -0.0030821 -0.00391367 -156.087 0.00322788 -0.99928 -0.0378044 429.902 -0.00379434 -0.0378166 0.999277 -2.14792\n-0.998643 -0.00336944 0.0519728 -154.645 0.00161741 -0.99943 -0.0337158 429.902 0.0520568 -0.033586 0.998079 -2.16471\n-0.996418 -0.00282019 0.0845168 -153.267 -0.000528078 -0.999217 -0.0395681 429.868 0.0845622 -0.039471 0.995636 -2.2344\n-0.999541 -0.0027865 0.0301573 -151.229 0.001456 -0.999028 -0.0440511 429.86 0.0302507 -0.043987 0.998574 -2.24778\n-0.99967 -0.00215641 0.0256038 -149.041 0.00117723 -0.99927 -0.0381972 429.863 0.0256675 -0.0381544 0.998942 -2.28566\n-0.99922 -0.000608003 0.0394915 -146.847 -0.000809832 -0.999356 -0.0358764 429.859 0.0394879 -0.0358804 0.998576 -2.28637\n-0.999557 -0.00104484 0.0297394 -145.362 0.000110286 -0.999507 -0.031409 429.863 0.0297576 -0.0313918 0.999064 -2.29959\n-0.999514 -0.000470205 0.0311759 -143.85 -0.000454833 -0.99956 -0.0296578 429.855 0.0311762 -0.0296576 0.999074 -2.33014\n-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\n-0.999499 0.00105011 0.0316415 -140.04 -0.00196553 -0.99958 -0.0289135 429.851 0.0315979 -0.0289612 0.999081 -2.35972\n-0.999442 0.000515658 0.0333996 -138.475 -0.00144939 -0.999609 -0.0279382 429.84 0.0333721 -0.027971 0.999052 -2.38131\n-0.999537 -0.00221398 0.0303322 -136.142 0.00129596 -0.999541 -0.030252 429.828 0.0303852 -0.0301987 0.999082 -2.39437\n-0.999591 -0.00434429 0.0282729 -134.563 0.00354917 -0.999598 -0.028113 429.806 0.0283837 -0.0280012 0.999205 -2.39811\n-0.999387 -0.00757164 0.034182 -132.136 0.00678342 -0.99971 -0.0231166 429.795 0.0343471 -0.0228706 0.999148 -2.41285\n-0.999341 -0.00972647 0.0349577 -130.509 0.00890604 -0.999683 -0.0235488 429.77 0.0351757 -0.0232219 0.999111 -2.42932\n-0.999385 -0.0134527 0.0323805 -128.067 0.0126643 -0.999621 -0.0244299 429.726 0.0326969 -0.0240048 0.999177 -2.44241\n-0.999375 -0.0147859 0.0321163 -126.41 0.0138974 -0.999519 -0.0277147 429.683 0.0325106 -0.027251 0.9991 -2.47095\n-0.999336 -0.0143737 0.0334795 -123.887 0.0134356 -0.999515 -0.0280779 429.638 0.0338669 -0.0276095 0.999045 -2.49126\n-0.999441 -0.0130281 0.0307884 -122.189 0.0121522 -0.999521 -0.0284658 429.609 0.0311445 -0.0280757 0.999121 -2.51522\n-0.999493 -0.0124129 0.0293194 -120.471 0.0116005 -0.999548 -0.027719 429.59 0.0296502 -0.0273648 0.999186 -2.52223\n-0.999518 -0.0105437 0.0291876 -117.86 0.00977819 -0.999608 -0.0262461 429.556 0.0294529 -0.025948 0.999229 -2.54297\n-0.999536 -0.00945629 0.0289458 -116.11 0.00870632 -0.999626 -0.0259267 429.534 0.0291802 -0.0256627 0.999245 -2.55707\n-0.99944 -0.00603561 0.0328988 -112.595 0.00516956 -0.99964 -0.0263466 429.498 0.033046 -0.0261618 0.999111 -2.56814\n-0.99944 -0.00426368 0.0331789 -110.805 0.00345983 -0.9997 -0.0242475 429.478 0.0332723 -0.0241192 0.999155 -2.58966\n-0.99953 -0.00276282 0.0305312 -109.013 0.0020934 -0.999757 -0.0219359 429.48 0.0305844 -0.0218617 0.999293 -2.60724\n-0.99953 -0.00129836 0.0306377 -107.197 0.000685039 -0.999799 -0.0200204 429.468 0.0306575 -0.01999 0.99933 -2.6247\n-0.999455 0.000250105 0.0330069 -105.411 -0.000907356 -0.999802 -0.019899 429.464 0.0329954 -0.0199181 0.999257 -2.63429\n-0.999511 0.000597418 0.0312524 -103.595 -0.0012448 -0.999785 -0.0206994 429.472 0.0312333 -0.0207282 0.999297 -2.6382\n-0.999547 0.00176253 0.0300332 -101.776 -0.00242794 -0.999752 -0.0221337 429.463 0.0299868 -0.0221966 0.999304 -2.64886\n-0.999498 0.00232348 0.0315843 -99.9646 -0.00315542 -0.999649 -0.0263161 429.466 0.031512 -0.0264025 0.999155 -2.65628\n-0.999451 0.00236168 0.0330474 -98.1151 -0.0033866 -0.999514 -0.0309921 429.459 0.0329582 -0.031087 0.998973 -2.67517\n-0.999533 0.00376728 0.0303199 -96.2903 -0.00470669 -0.999509 -0.0309717 429.449 0.0301883 -0.0310999 0.99906 -2.67765\n-0.999635 0.00448365 0.0266583 -94.4617 -0.005273 -0.999548 -0.0296138 429.452 0.0265134 -0.0297435 0.999206 -2.67585\n-0.999543 0.00630047 0.0295668 -91.6979 -0.00704422 -0.99966 -0.0251186 429.462 0.0293985 -0.0253154 0.999247 -2.68727\n-0.999569 0.00819468 0.0281834 -88.9421 -0.00886555 -0.999678 -0.0237616 429.478 0.0279796 -0.0240012 0.99932 -2.68767\n-0.999568 0.00825989 0.0282183 -87.0968 -0.00889656 -0.999707 -0.0225119 429.494 0.0280241 -0.0227532 0.999348 -2.6907\n-0.999502 0.00931498 0.0301561 -85.2744 -0.0100318 -0.999669 -0.0237061 429.498 0.0299253 -0.0239968 0.999264 -2.7017\n-0.999461 0.0100136 0.0312648 -83.4677 -0.0107464 -0.999669 -0.0233571 429.507 0.0310206 -0.0236805 0.999238 -2.69794\n-0.999263 0.0097595 0.0371234 -81.6622 -0.0104673 -0.999766 -0.0189199 429.518 0.03693 -0.0192945 0.999132 -2.72877\n-0.999627 0.00853195 0.0259325 -79.0033 -0.00903342 -0.999773 -0.0192822 429.543 0.0257621 -0.0195093 0.999478 -2.73394\n-0.999723 0.00816971 0.0220641 -77.2498 -0.00879701 -0.999556 -0.0284849 429.526 0.0218216 -0.0286711 0.999351 -2.73357\n-0.999399 0.00701901 0.0339381 -74.6281 -0.00811647 -0.999445 -0.0323082 429.541 0.0336925 -0.0325643 0.998902 -2.72993\n-0.9995 0.00458161 0.0312772 -72.898 -0.0054944 -0.99956 -0.0291609 429.553 0.0311298 -0.0293182 0.999085 -2.72475\n-0.999471 0.00341807 0.0323392 -71.1955 -0.00411655 -0.999759 -0.0215566 429.553 0.0322577 -0.0216783 0.999244 -2.73296\n-0.999435 0.000946839 0.0335961 -69.4824 -0.00159742 -0.999812 -0.0193433 429.562 0.0335715 -0.0193861 0.999248 -2.73885\n-0.999608 -0.0010144 0.0279703 -66.0939 0.000504122 -0.999833 -0.0182444 429.549 0.0279842 -0.0182232 0.999442 -2.75274\n-0.999667 -0.000770131 0.0258059 -64.4135 0.000356247 -0.999871 -0.0160391 429.542 0.0258149 -0.0160246 0.999538 -2.75869\n-0.999661 -0.000550228 0.0260198 -62.7479 0.000100431 -0.999851 -0.0172849 429.538 0.0260254 -0.0172764 0.999512 -2.75786\n-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\n-0.9996 0.000341283 0.0282811 -59.45 -0.000878132 -0.99982 -0.0189724 429.523 0.0282695 -0.0189896 0.99942 -2.76867\n-0.999532 -0.000498998 0.030572 -57.8268 -0.000145846 -0.999778 -0.0210868 429.517 0.0305757 -0.0210814 0.99931 -2.76225\n-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\n-0.999512 -0.00103515 0.0312159 -54.6513 0.000295312 -0.999719 -0.023696 429.508 0.0312317 -0.0236752 0.999232 -2.76129\n-0.999547 -0.00112583 0.0300664 -53.1252 0.000457543 -0.999753 -0.0222247 429.499 0.0300839 -0.0222009 0.999301 -2.76065\n-0.999598 -0.00130508 0.0283126 -51.6311 0.00073193 -0.999795 -0.0202444 429.499 0.0283332 -0.0202156 0.999394 -2.75336\n-0.999683 -0.00175901 0.0251025 -50.1353 0.00127077 -0.99981 -0.0194525 429.498 0.0251319 -0.0194144 0.999496 -2.74389\n-0.9997 -0.00170338 0.0244339 -48.6676 0.00125691 -0.999832 -0.0182762 429.494 0.0244609 -0.01824 0.999534 -2.74049\n-0.999638 -0.00253303 0.0267859 -46.4768 0.00220985 -0.999924 -0.012088 429.494 0.0268145 -0.0120245 0.999568 -2.73819\n-0.999679 -0.00265982 0.0251807 -45.0458 0.00235991 -0.999926 -0.0119325 429.493 0.0252105 -0.0118693 0.999612 -2.73255\n-0.999561 -0.00276172 0.0295035 -43.649 0.00253322 -0.999967 -0.0077796 429.489 0.029524 -0.00770145 0.999534 -2.71882\n-0.999434 -0.00281893 0.0335166 -42.2887 0.00259242 -0.999974 -0.00679971 429.483 0.0335349 -0.00670898 0.999415 -2.73134\n-0.999397 -0.00386108 0.0345053 -40.3225 0.00345057 -0.999923 -0.0119486 429.48 0.0345487 -0.0118223 0.999333 -2.75096\n-0.999599 -0.00516261 0.0278472 -39.0453 0.00474099 -0.999873 -0.0151854 429.461 0.027922 -0.0150473 0.999497 -2.73682\n-0.999553 -0.00591629 0.0293191 -37.7641 0.00565217 -0.999943 -0.00908308 429.459 0.0293711 -0.0089133 0.999529 -2.74986\n-0.99954 -0.00593728 0.0297385 -36.4974 0.00538769 -0.999814 -0.0185269 429.44 0.0298429 -0.0183582 0.999386 -2.75431\n-0.999951 -0.00720943 -0.00672312 -34.6809 0.00728568 -0.999909 -0.011387 429.443 -0.00664041 -0.0114354 0.999913 -2.69215\n-0.999962 -0.00674938 -0.00554604 -33.427 0.00686517 -0.999753 -0.0211311 429.403 -0.00540205 -0.0211683 0.999761 -2.68639\n-0.999038 -0.00734239 0.0432377 -32.1463 0.00647776 -0.999777 -0.0201033 429.394 0.0433756 -0.0198039 0.998863 -2.68532\n-0.998899 -0.00729042 0.0463473 -30.9374 0.0065595 -0.999852 -0.0159029 429.392 0.0464564 -0.0155814 0.998799 -2.68686\n-0.999867 -0.00847317 0.0139543 -29.1512 0.00808512 -0.999585 -0.0276339 429.363 0.0141826 -0.0275174 0.999521 -2.68801\n-0.999529 -0.00949281 0.0291706 -27.3317 0.00864901 -0.999544 -0.0289176 429.343 0.0294318 -0.0286517 0.999156 -2.69048\n-0.9997 -0.0101508 0.022274 -25.5623 0.00946242 -0.999481 -0.0307938 429.316 0.022575 -0.0305738 0.999278 -2.6735\n-0.999721 -0.0101814 0.0212923 -23.8062 0.00958929 -0.999569 -0.0277285 429.312 0.0215655 -0.0275166 0.999389 -2.67043\n-0.999678 -0.0105399 0.0230786 -22.0875 0.00993602 -0.999609 -0.0261245 429.288 0.0233449 -0.0258868 0.999392 -2.65953\n-0.999599 -0.0109129 0.0261108 -20.3938 0.0102342 -0.99961 -0.0259871 429.257 0.0263842 -0.0257094 0.999321 -2.64977\n-0.999555 -0.0120137 0.0272906 -19.2828 0.011275 -0.99957 -0.0270619 429.233 0.027604 -0.0267422 0.999261 -2.65529\n-0.99941 -0.0124694 0.0320025 -18.1663 0.0114305 -0.999408 -0.0324433 429.218 0.0323881 -0.0320583 0.998961 -2.66548\n-0.99918 -0.0134543 0.0381915 -17.0657 0.0121101 -0.999306 -0.0352125 429.204 0.0386388 -0.0347211 0.99865 -2.65836\n-0.999859 -0.0141326 0.00910506 -15.4693 0.0138597 -0.999473 -0.0293601 429.181 0.00951519 -0.0292298 0.999527 -2.63124\n-0.999876 -0.0141306 0.00700192 -14.3958 0.0138796 -0.999302 -0.034694 429.146 0.00748728 -0.0345925 0.999373 -2.63565\n-0.999416 -0.0160286 0.0301772 -13.2996 0.0151337 -0.999446 -0.0296536 429.134 0.0306357 -0.0291796 0.999105 -2.61784\n-0.999473 -0.01715 0.0275591 -12.2454 0.0162637 -0.999353 -0.032067 429.125 0.0280912 -0.0316019 0.999106 -2.61564\n-0.999725 -0.0186495 0.0141951 -11.21 0.0181287 -0.999188 -0.0359735 429.098 0.0148544 -0.0357063 0.999252 -2.60422\n-0.999563 -0.0200023 0.0217857 -9.62557 0.0193198 -0.999329 -0.0311014 429.071 0.0223932 -0.0306669 0.999279 -2.6013\n-0.999752 -0.0209253 0.00764603 -8.60939 0.0206461 -0.999176 -0.0349326 429.032 0.00837071 -0.0347661 0.99936 -2.54392\n-0.999667 -0.0238827 -0.00973781 -7.58928 0.0242168 -0.999067 -0.03577 429.022 -0.00887444 -0.0359939 0.999313 -2.52315\n-0.998967 -0.0317121 0.0325336 -5.91614 0.0307126 -0.999054 -0.0307728 428.948 0.0334787 -0.0297418 0.998997 -2.5858\n-0.998208 -0.0423643 0.0422664 -4.34655 0.0408563 -0.998519 -0.0359253 428.883 0.0437258 -0.0341341 0.99846 -2.51956\n-0.998632 -0.0510977 0.011085 -3.2986 0.050754 -0.99828 -0.0293394 428.832 0.0125651 -0.0287367 0.999508 -2.51906\n-0.998084 -0.0615782 0.00605625 -2.19584 0.0613578 -0.997616 -0.0315733 428.747 0.00798604 -0.0311412 0.999483 -2.54663\n-0.997154 -0.0734153 0.0171575 -1.06646 0.0728415 -0.996831 -0.0319675 428.653 0.0194501 -0.0306267 0.999342 -2.5276\n-0.996214 -0.0854816 0.0158187 0.051566 0.0849644 -0.995905 -0.0309005 428.541 0.0183954 -0.0294394 0.999397 -2.51707\n-0.995102 -0.0979503 0.0133271 1.18852 0.0974748 -0.994705 -0.0325812 428.421 0.0164478 -0.0311226 0.99938 -2.51174\n-0.993664 -0.110937 0.0179983 2.34997 0.110261 -0.993286 -0.0350031 428.27 0.0217606 -0.0327968 0.999225 -2.50638\n-0.992123 -0.123658 0.0200289 3.5258 0.122868 -0.991741 -0.0367857 428.104 0.0244123 -0.034035 0.999122 -2.50965\n-0.990417 -0.136981 0.0176448 4.71179 0.136171 -0.989839 -0.0409431 427.921 0.0230739 -0.038148 0.999006 -2.51464\n-0.988471 -0.150311 0.0182212 5.91875 0.14935 -0.987716 -0.0459482 427.716 0.0249039 -0.0426972 0.998778 -2.51486\n-0.98513 -0.171057 0.0160803 7.76121 0.170078 -0.984173 -0.0497761 427.38 0.0243404 -0.046301 0.998631 -2.52723\n-0.982771 -0.184228 0.0148901 9.02264 0.183095 -0.981397 -0.0577655 427.119 0.0252551 -0.0540439 0.998219 -2.52482\n-0.978864 -0.204124 0.0126331 10.9146 0.203267 -0.977847 -0.0499716 426.723 0.0225536 -0.0463474 0.998671 -2.51918\n-0.97616 -0.216745 0.0115156 12.1781 0.215843 -0.974951 -0.0536858 426.409 0.0228632 -0.0499204 0.998491 -2.53481\n-0.97262 -0.232049 0.0127662 13.4667 0.231041 -0.971406 -0.0546899 426.101 0.0250919 -0.050243 0.998422 -2.52478\n-0.968658 -0.248146 0.0111824 14.748 0.247493 -0.96799 -0.0417369 425.761 0.0211813 -0.0376612 0.999066 -2.51947\n-0.96408 -0.265438 0.00962068 16.0548 0.264682 -0.963101 -0.0487764 425.369 0.0222128 -0.0444779 0.998763 -2.51634\n-0.955728 -0.293971 0.01285 18.0293 0.293203 -0.955097 -0.0426851 424.749 0.0248212 -0.0370277 0.999006 -2.50434\n-0.947486 -0.319699 0.00795762 20.0079 0.319047 -0.946672 -0.0449542 424.05 0.0219051 -0.0400546 0.998957 -2.49338\n-0.941453 -0.337103 0.00524482 21.3371 0.336606 -0.940715 -0.0418429 423.564 0.0190393 -0.0376277 0.99911 -2.48572\n-0.935184 -0.354141 0.00376512 22.6611 0.353735 -0.934528 -0.0391061 423.049 0.0173677 -0.0352395 0.999228 -2.48426\n-0.928973 -0.370142 0.00185783 23.998 0.369786 -0.928279 -0.0394622 422.502 0.0163312 -0.0359723 0.999219 -2.47526\n-0.922371 -0.386303 0.00105514 25.3343 0.386028 -0.921811 -0.0352978 421.936 0.0146083 -0.0321504 0.999376 -2.45617\n-0.915793 -0.401635 0.00344007 26.6771 0.401321 -0.915357 -0.0326097 421.332 0.0162461 -0.0284831 0.999462 -2.45144\n-0.904791 -0.425824 0.00512246 28.685 0.425462 -0.904407 -0.0321092 420.381 0.0183056 -0.0268727 0.999471 -2.43753\n-0.897068 -0.44186 0.00541194 30.0206 0.441461 -0.896664 -0.0332643 419.706 0.0195509 -0.0274511 0.999432 -2.4251\n-0.88897 -0.457932 0.00550067 31.3587 0.457513 -0.888563 -0.0337097 418.997 0.0203244 -0.0274503 0.999417 -2.42032\n-0.876858 -0.480711 0.00618073 33.3588 0.480171 -0.87636 -0.0378127 417.888 0.0235935 -0.0301886 0.999266 -2.42119\n-0.864363 -0.502853 0.00405276 35.3454 0.502372 -0.863839 -0.03748 416.706 0.0223479 -0.0303603 0.999289 -2.39886\n-0.851225 -0.524791 0.00324444 37.3356 0.52435 -0.850733 -0.0361893 415.466 0.021752 -0.029104 0.99934 -2.39999\n-0.826734 -0.562582 0.00351908 40.6197 0.562143 -0.826306 -0.0348207 413.268 0.0224974 -0.0268093 0.999387 -2.38327\n-0.810214 -0.58613 0.00215425 42.5779 0.585639 -0.809676 -0.038094 411.844 0.0240723 -0.0296027 0.999272 -2.38712\n-0.792879 -0.60937 0.00339843 44.5122 0.608661 -0.792202 -0.0441456 410.339 0.0295932 -0.0329336 0.999019 -2.39296\n-0.774998 -0.631953 0.00361941 46.4238 0.630855 -0.773965 -0.0547713 408.74 0.0374142 -0.0401643 0.998492 -2.41049\n-0.75701 -0.653404 0.000356804 48.3077 0.652973 -0.756531 -0.0358718 407.12 0.0237087 -0.0269223 0.999356 -2.40284\n-0.744458 -0.667571 0.0114218 49.5837 0.667505 -0.744546 -0.0094357 406.017 0.0148031 0.000599636 0.99989 -2.37532\n-0.732067 -0.681157 0.0102064 50.8151 0.681158 -0.732126 -0.00391903 404.869 0.0101418 0.00408314 0.99994 -2.39514\n-0.717906 -0.696007 0.0135746 52.0193 0.695874 -0.718035 -0.0136399 403.677 0.0192405 -0.000345967 0.999815 -2.37016\n-0.705668 -0.70818 0.0226413 53.2124 0.707644 -0.706022 -0.0277908 402.455 0.0356661 -0.00358909 0.999357 -2.39274\n-0.692641 -0.721041 0.0186896 54.3543 0.720318 -0.692822 -0.0337662 401.229 0.0372953 -0.00992536 0.999255 -2.38925\n-0.679064 -0.733912 0.0156698 55.4834 0.733209 -0.679143 -0.0341961 400.009 0.035739 -0.0117321 0.999292 -2.41117\n-0.658885 -0.752166 0.0108177 57.0757 0.751488 -0.6588 -0.0353176 398.151 0.0336914 -0.0151409 0.999318 -2.42375\n-0.64503 -0.764111 0.0083984 58.1196 0.763533 -0.644908 -0.033333 396.898 0.0308863 -0.0150883 0.999409 -2.43214\n-0.630222 -0.776362 0.00908238 59.1226 0.775757 -0.630127 -0.0337904 395.643 0.0319566 -0.0142498 0.999388 -2.44209\n-0.607698 -0.794133 0.00752896 60.5825 0.793541 -0.607567 -0.03399 393.728 0.0315669 -0.0146811 0.999394 -2.45482\n-0.584559 -0.811345 0.00322743 61.9805 0.810868 -0.584344 -0.0321659 391.791 0.0279835 -0.0161858 0.999477 -2.46756\n-0.561269 -0.827631 0.0020537 63.306 0.827127 -0.561011 -0.0335775 389.804 0.028942 -0.0171474 0.999434 -2.47289\n-0.545523 -0.838095 0.00110853 64.1729 0.837573 -0.545229 -0.0345931 388.457 0.0295967 -0.0179428 0.999401 -2.48434\n-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\n-0.514181 -0.857682 0.000384407 65.826 0.857276 -0.513951 -0.030531 385.729 0.0263834 -0.0153689 0.999534 -2.49006\n-0.491017 -0.871123 0.0068023 67.0048 0.870498 -0.490937 -0.0348443 383.607 0.0336932 -0.0111877 0.99937 -2.5071\n-0.477285 -0.878719 0.00720538 67.7838 0.878145 -0.477246 -0.0331344 382.185 0.0325546 -0.0094872 0.999425 -2.52576\n-0.465847 -0.884834 0.00741484 68.5254 0.884278 -0.465826 -0.032519 380.747 0.032228 -0.0085921 0.999444 -2.53698\n-0.446639 -0.894678 0.00801276 69.6022 0.894263 -0.44668 -0.0277574 378.555 0.0284131 -0.00523201 0.999583 -2.54661\n-0.417054 -0.908881 -0.000867462 70.9816 0.908617 -0.416909 -0.0245334 375.568 0.0219363 -0.01102 0.999699 -2.56512\n-0.401751 -0.915739 -0.0042378 71.6345 0.915456 -0.401502 -0.0271447 374.044 0.023156 -0.014785 0.999623 -2.56996\n-0.385886 -0.922528 -0.00586854 72.2775 0.922232 -0.385581 -0.028543 372.492 0.0240689 -0.0164265 0.999575 -2.55421\n-0.359312 -0.933179 -0.00845561 73.1733 0.932925 -0.358958 -0.0282777 370.119 0.023353 -0.018049 0.999564 -2.54396\n-0.340994 -0.939967 -0.0136413 73.7337 0.939863 -0.340583 -0.0257233 368.525 0.0195331 -0.0215924 0.999576 -2.557\n-0.312523 -0.949692 -0.0203495 74.5048 0.949697 -0.311927 -0.0278747 366.091 0.0201248 -0.0280373 0.999404 -2.53582\n-0.278986 -0.960093 -0.019704 75.2216 0.960089 -0.278442 -0.0264381 363.639 0.0198966 -0.0262934 0.999456 -2.53669\n-0.255995 -0.966502 -0.01847 75.6554 0.966368 -0.255382 -0.0302136 361.968 0.0244846 -0.0255833 0.999373 -2.53\n-0.220662 -0.975228 -0.0154354 76.224 0.975063 -0.220186 -0.0277627 359.437 0.0236763 -0.0211767 0.999495 -2.52827\n-0.185104 -0.982633 -0.0130133 76.6998 0.982409 -0.184697 -0.027563 356.88 0.0246808 -0.0178865 0.999535 -2.52715\n-0.148615 -0.988831 -0.011232 77.0882 0.988611 -0.148291 -0.0256697 354.295 0.0237174 -0.014919 0.999607 -2.52515\n-0.1262 -0.991937 -0.0115679 77.2911 0.991748 -0.125894 -0.0242349 352.547 0.0225831 -0.0145309 0.999639 -2.52773\n-0.107361 -0.994136 -0.0129256 77.4876 0.993958 -0.107025 -0.0243461 350.78 0.02282 -0.0154613 0.99962 -2.51892\n-0.0735913 -0.997244 -0.00940391 77.7473 0.996951 -0.0733176 -0.0267226 347.248 0.0259595 -0.0113418 0.999599 -2.52846\n-0.0555126 -0.998389 -0.0117259 77.8369 0.998125 -0.0551868 -0.0264827 345.465 0.0257929 -0.013174 0.99958 -2.53429\n-0.0389668 -0.999108 -0.0162455 77.8918 0.998929 -0.0385435 -0.0256046 343.665 0.0249556 -0.0172258 0.99954 -2.5381\n-0.0189792 -0.99957 -0.0223719 77.9217 0.99943 -0.0183419 -0.0283552 340.949 0.0279326 -0.0228973 0.999348 -2.54179\n-0.00933132 -0.999605 -0.0265034 77.9289 0.999595 -0.00861206 -0.0271241 339.117 0.0268851 -0.0267458 0.999281 -2.54622\n0.00147211 -0.99959 -0.0286099 77.9017 0.999729 0.00213617 -0.0231942 336.352 0.0232457 -0.028568 0.999322 -2.54962\n0.00685758 -0.999547 -0.0293206 77.8796 0.999654 0.00759704 -0.0251832 334.513 0.0253945 -0.0291377 0.999253 -2.54544\n0.00945599 -0.999589 -0.0270542 77.844 0.999751 0.00999758 -0.019954 331.741 0.0202162 -0.0268588 0.999435 -2.54861\n0.010872 -0.99967 -0.0232751 77.8193 0.999668 0.0114095 -0.0230867 329.876 0.0233447 -0.0230164 0.999462 -2.54042\n0.0131083 -0.999711 -0.0201319 77.788 0.99962 0.0135897 -0.0239671 327.068 0.0242337 -0.0198101 0.99951 -2.54235\n0.0143645 -0.999756 -0.0167782 77.7592 0.999679 0.0147096 -0.0206253 325.188 0.020867 -0.0164766 0.999646 -2.53777\n0.0163683 -0.99978 -0.013134 77.7012 0.999591 0.0166704 -0.0232317 322.37 0.0234455 -0.0127483 0.999644 -2.53243\n0.0179526 -0.999743 -0.0138573 77.6319 0.999576 0.0182641 -0.022685 319.53 0.0229323 -0.0134442 0.999647 -2.52957\n0.0184396 -0.999761 -0.0117841 77.5942 0.999508 0.0187313 -0.0251382 317.623 0.0253529 -0.0113147 0.999615 -2.53539\n0.0197989 -0.99973 -0.0121957 77.5408 0.999268 0.0201861 -0.0324905 314.742 0.0327279 -0.0115435 0.999398 -2.56558\n0.0212896 -0.999662 -0.0149068 77.4612 0.999205 0.0217776 -0.0333827 311.835 0.0336961 -0.0141843 0.999331 -2.58656\n0.0213422 -0.999666 -0.0145829 77.3971 0.999514 0.021666 -0.0224189 308.944 0.0227273 -0.0140974 0.999642 -2.59001\n0.0209854 -0.999694 -0.0131343 77.3259 0.999513 0.0212815 -0.0228255 305.995 0.023098 -0.0126489 0.999653 -2.59496\n0.0185575 -0.999705 -0.0157007 77.2766 0.999542 0.0189253 -0.0236094 304.028 0.0238996 -0.0152554 0.999598 -2.58856\n0.0112816 -0.999843 -0.0136961 77.2415 0.999736 0.0115523 -0.0198504 301.076 0.0200055 -0.0134685 0.999709 -2.57428\n0.00400704 -0.999914 -0.0125128 77.224 0.999796 0.00425385 -0.0197607 298.106 0.0198123 -0.012431 0.999726 -2.5723\n-0.00269864 -0.999849 -0.0171475 77.2215 0.999775 -0.00233672 -0.0210913 295.122 0.021048 -0.0172005 0.99963 -2.55334\n-0.00935674 -0.999781 -0.0187401 77.2452 0.999726 -0.00895086 -0.021626 292.12 0.0214536 -0.0189373 0.99959 -2.55942\n-0.0124775 -0.999743 -0.0189471 77.253 0.999707 -0.0120793 -0.02099 290.122 0.0207557 -0.0192034 0.9996 -2.54727\n-0.0134386 -0.999722 -0.0193588 77.2665 0.999709 -0.0130457 -0.0202821 289.11 0.020024 -0.0196257 0.999607 -2.54946\n-0.0155985 -0.999618 -0.0228129 77.2725 0.999655 -0.0151089 -0.0214787 287.091 0.0211258 -0.0231401 0.999509 -2.54355\n-0.0168183 -0.999553 -0.0247364 77.2953 0.999612 -0.0162598 -0.0226068 286.084 0.0221945 -0.025107 0.999438 -2.55031\n-0.0183776 -0.999517 -0.0250736 77.3421 0.999588 -0.0178141 -0.0225167 283.045 0.0220591 -0.0254771 0.999432 -2.52046\n-0.0182942 -0.999568 -0.0229883 77.3802 0.999649 -0.0178457 -0.0195653 281.039 0.0191467 -0.0233382 0.999544 -2.51767\n-0.0180324 -0.999588 -0.0223127 77.4038 0.999658 -0.0176018 -0.01935 280.023 0.0189493 -0.022654 0.999564 -2.5068\n-0.0174024 -0.999622 -0.0212939 77.4376 0.999671 -0.0169936 -0.0192303 277.987 0.0188612 -0.0216215 0.999588 -2.49953\n-0.0167626 -0.999634 -0.021253 77.4418 0.999664 -0.0163348 -0.0201458 276.98 0.0197912 -0.0215836 0.999571 -2.49028\n-0.0144084 -0.999664 -0.0215479 77.4672 0.99969 -0.0139645 -0.0206108 274.953 0.020303 -0.0218382 0.999555 -2.49286\n-0.0133516 -0.999635 -0.0234978 77.4676 0.9997 -0.0128624 -0.0208467 273.942 0.0205368 -0.0237691 0.999507 -2.49103\n-0.0123073 -0.999612 -0.0250052 77.4625 0.9997 -0.0117712 -0.0214757 272.931 0.021173 -0.025262 0.999457 -2.48776\n-0.00966494 -0.999584 -0.0271621 77.4767 0.999733 -0.00908967 -0.0212235 270.897 0.0209678 -0.0273599 0.999406 -2.47881\n-0.00812124 -0.999566 -0.028313 77.4933 0.999746 -0.00752147 -0.0212264 269.88 0.0210042 -0.0284782 0.999374 -2.47133\n-0.00674044 -0.999584 -0.0280568 77.4986 0.999766 -0.00615952 -0.0207402 268.859 0.0205587 -0.02819 0.999391 -2.47654\n-0.00437794 -0.999667 -0.0254437 77.5042 0.999767 -0.00383726 -0.0212603 266.837 0.0211556 -0.0255308 0.99945 -2.45749\n-0.00292487 -0.999727 -0.0231959 77.5145 0.99979 -0.00245308 -0.0203417 265.813 0.0202792 -0.0232505 0.999524 -2.4483\n-0.00119692 -0.999848 -0.0174028 77.5125 0.999837 -0.000882766 -0.0180485 263.778 0.0180304 -0.0174216 0.999686 -2.43752\n-0.000620105 -0.999879 -0.0155621 77.5108 0.999853 -0.000352936 -0.0171648 262.766 0.0171572 -0.0155705 0.999732 -2.43471\n-0.000453925 -0.999896 -0.0143865 77.5193 0.999856 -0.000209353 -0.0169971 261.749 0.0169923 -0.0143921 0.999752 -2.42901\n-0.000992149 -0.999889 -0.0148843 77.5092 0.999762 -0.000667378 -0.0218088 259.713 0.0217964 -0.0149024 0.999651 -2.4258\n-0.00106088 -0.999854 -0.0170516 77.5109 0.999733 -0.000666575 -0.0231132 258.695 0.0230985 -0.0170716 0.999587 -2.42864\n-0.00111935 -0.999795 -0.0202123 77.5061 0.99977 -0.000685884 -0.0214398 256.663 0.0214215 -0.0202317 0.999566 -2.42413\n-0.00145616 -0.999775 -0.0211685 77.5033 0.999784 -0.00101695 -0.0207442 255.653 0.020718 -0.0211941 0.999561 -2.41281\n-0.00178528 -0.999706 -0.0241908 77.4971 0.999739 -0.0012333 -0.0228134 252.598 0.0227769 -0.0242252 0.999447 -2.39803\n-0.00271861 -0.999663 -0.0258107 77.4925 0.999807 -0.00221561 -0.0194967 250.595 0.0194329 -0.0258587 0.999477 -2.39077\n-0.00384521 -0.999682 -0.0249384 77.4996 0.999803 -0.00335722 -0.0195804 247.616 0.0194904 -0.0250088 0.999497 -2.3624\n-0.00476169 -0.999765 -0.0211268 77.5002 0.999809 -0.00435907 -0.0190624 244.663 0.0189659 -0.0212135 0.999595 -2.33144\n-0.00355777 -0.999842 -0.0174054 77.5202 0.999784 -0.00320015 -0.020531 241.758 0.0204721 -0.0174746 0.999638 -2.31479\n-0.00183528 -0.999891 -0.0146793 77.5157 0.999754 -0.00151003 -0.0221373 238.929 0.0221127 -0.0147164 0.999647 -2.29086\n-0.00110368 -0.999922 -0.0124639 77.5073 0.999719 -0.000808087 -0.023696 237.112 0.0236841 -0.0124865 0.999642 -2.27951\n-0.000144488 -0.999909 -0.013459 77.5079 0.999782 0.000136375 -0.0208649 234.458 0.0208648 -0.013459 0.999692 -2.25694\n0.000126896 -0.999892 -0.0146945 77.4893 0.999727 0.000470456 -0.0233791 232.742 0.0233835 -0.0146875 0.999619 -2.24873\n0.000589695 -0.999883 -0.0152919 77.481 0.999702 0.000962899 -0.0244094 231.065 0.0244213 -0.015273 0.999585 -2.23172\n0.00155605 -0.999848 -0.0173802 77.4705 0.999725 0.00196239 -0.0233873 228.651 0.0234178 -0.017339 0.999575 -2.2203\n0.00262366 -0.999838 -0.0178103 77.4658 0.999891 0.00288181 -0.0144842 226.315 0.0145332 -0.0177703 0.999736 -2.19039\n0.00382597 -0.999799 -0.0196651 77.4559 0.999858 0.00414759 -0.0163402 224.002 0.0164185 -0.0195998 0.999673 -2.16558\n0.00511454 -0.999774 -0.0206588 77.4358 0.999832 0.00547643 -0.0174992 221.736 0.0176084 -0.0205658 0.999633 -2.13854\n0.00722288 -0.999764 -0.0204917 77.4152 0.99987 0.00751586 -0.0142567 220.235 0.0144074 -0.0203861 0.999688 -2.11716\n0.00869742 -0.999723 -0.0218492 77.4002 0.999871 0.00840005 0.0136649 218.78 -0.0134776 -0.0219652 0.999668 -2.07306\n0.00928338 -0.999751 -0.020275 77.3619 0.999742 0.00970012 -0.0205535 216.517 0.0207451 -0.020079 0.999583 -2.07708\n0.0106186 -0.999825 -0.0154187 77.3476 0.997351 0.0116995 -0.0717938 214.978 0.0719616 -0.0146155 0.9973 -2.07589\n0.0134476 -0.999807 -0.014356 77.33 0.999843 0.0136113 -0.011366 213.548 0.0115592 -0.0142009 0.999832 -2.03572\n0.0164455 -0.999771 -0.0136783 77.2934 0.999766 0.0166343 -0.0137986 211.295 0.014023 -0.0134482 0.999811 -2.04794\n0.0186189 -0.999675 -0.0173903 77.2305 0.999631 0.0189565 -0.0194532 209.015 0.0197765 -0.0170217 0.99966 -2.03199\n0.0170927 -0.999677 -0.0188168 77.1907 0.999792 0.0172983 -0.0108181 206.705 0.0111401 -0.018628 0.999764 -2.00735\n0.0165876 -0.999711 -0.0174171 77.1607 0.999749 0.0168456 -0.0147695 205.152 0.0150587 -0.0171678 0.999739 -1.98239\n0.0153728 -0.999767 -0.0151748 77.1319 0.999815 0.0155451 -0.0113021 202.813 0.0115353 -0.0149983 0.999821 -1.95961\n0.0145739 -0.999711 -0.0191404 77.0729 0.999771 0.0148689 -0.0153589 200.443 0.0156391 -0.0189122 0.999699 -1.93995\n0.014455 -0.999688 -0.0203506 77.0451 0.999719 0.0148323 -0.0185146 198.872 0.0188107 -0.0200773 0.999621 -1.91712\n0.0147767 -0.999442 -0.0299686 76.9824 0.999774 0.0152257 -0.0148094 196.531 0.0152575 -0.029743 0.999441 -1.90551\n0.0144785 -0.999329 -0.0336482 76.9479 0.999808 0.0149137 -0.0127197 194.986 0.013213 -0.0334576 0.999353 -1.8963\n0.00976972 -0.999023 -0.0431013 76.9072 0.999862 0.0103399 -0.0130264 192.689 0.0134594 -0.042968 0.998986 -1.84757\n0.00545272 -0.999146 -0.0409656 76.9083 0.999917 0.00592426 -0.011398 191.185 0.011631 -0.0409 0.999096 -1.82577\n4.01071e-05 -0.999128 -0.0417536 76.9005 0.999908 0.000607768 -0.0135829 188.941 0.0135964 -0.0417492 0.999036 -1.79372\n-0.00228628 -0.999254 -0.0385403 76.9052 0.999873 -0.00167625 -0.0158531 187.455 0.0157767 -0.0385716 0.999131 -1.76068\n-0.00633171 -0.999021 -0.043784 76.9051 0.999746 -0.00537753 -0.0218766 185.271 0.0216197 -0.0439114 0.998801 -1.74589\n-0.00607143 -0.999536 -0.0298431 76.9289 0.999892 -0.00566804 -0.0135833 183.858 0.0134078 -0.0299224 0.999462 -1.66696\n-0.00860115 -0.999662 -0.024552 76.948 0.999572 -0.0092815 0.0277327 182.459 -0.0279512 -0.024303 0.999314 -1.65745\n-0.0105208 -0.999727 -0.0208668 76.9675 0.998781 -0.00949987 -0.0484351 180.257 0.0482236 -0.0213509 0.998608 -1.69632\n-0.0108165 -0.999781 -0.0179035 76.9972 0.999693 -0.0104127 -0.0224982 178.121 0.0223068 -0.0181414 0.999587 -1.62486\n-0.0125297 -0.999771 -0.0173669 77.0024 0.999921 -0.0125189 -0.000726729 175.971 0.000509147 -0.0173747 0.999849 -1.61872\n-0.0123475 -0.999847 -0.01238 77.0261 0.999729 -0.0120995 -0.0199129 174.472 0.01976 -0.0126225 0.999725 -1.60681\n-0.0135726 -0.999809 -0.0140457 77.0545 0.999885 -0.0134769 -0.00688987 172.23 0.00669927 -0.0141376 0.999878 -1.59129\n-0.0126881 -0.999853 -0.0114913 77.0899 0.999828 -0.0125304 -0.0136871 169.958 0.0135411 -0.011663 0.99984 -1.55401\n-0.0113845 -0.999888 -0.00974684 77.1067 0.999879 -0.0112797 -0.010737 167.664 0.0106259 -0.0098679 0.999895 -1.52705\n-0.00759188 -0.999952 -0.0061414 77.1221 0.999879 -0.00750784 -0.0135939 165.362 0.0135471 -0.00624387 0.999889 -1.49765\n-0.00436736 -0.999981 -0.0044084 77.1265 0.999857 -0.00429473 -0.0163525 163.061 0.0163332 -0.00447919 0.999857 -1.47648\n-0.00207976 -0.999991 -0.00381281 77.128 0.999893 -0.00202441 -0.0144638 161.536 0.014456 -0.00384249 0.999888 -1.45572\n0.0021867 -0.999995 -0.00213157 77.1135 0.999925 0.00221221 -0.0120398 159.305 0.0120444 -0.00210509 0.999925 -1.43717\n0.00547133 -0.999966 -0.00617879 77.0931 0.999929 0.00553613 -0.0105198 157.098 0.0105537 -0.0061208 0.999926 -1.40472\n0.00496497 -0.999911 -0.0123806 77.0627 0.999918 0.00510996 -0.0117075 154.911 0.0117697 -0.0123214 0.999855 -1.37557\n0.00276538 -0.99985 -0.0170817 77.0263 0.999923 0.00297078 -0.012011 152.059 0.0120599 -0.0170472 0.999782 -1.32269\n0.00168749 -0.999815 -0.0191515 77.02 0.999919 0.00192935 -0.0126173 150.65 0.0126519 -0.0191287 0.999737 -1.29787\n0.000222516 -0.999787 -0.0206471 77.023 0.999932 0.000463119 -0.0116491 148.568 0.0116562 -0.0206431 0.999719 -1.26899\n-0.000679331 -0.999807 -0.0196368 77.0183 0.999941 -0.000465848 -0.0108741 146.546 0.0108629 -0.019643 0.999748 -1.22285\n-0.00109885 -0.999808 -0.0195644 77.0215 0.999947 -0.000897858 -0.0102794 145.219 0.0102599 -0.0195746 0.999756 -1.21083\n0.000147276 -0.999783 -0.0208292 77.0176 0.999943 0.000369376 -0.0106595 143.925 0.0106649 -0.0208264 0.999726 -1.18617\n0.000854991 -0.99979 -0.0204622 77.017 0.999943 0.00107194 -0.0105937 142.6 0.0106134 -0.020452 0.999735 -1.16715\n0.00227117 -0.999838 -0.0178754 77.0208 0.999997 0.00227443 -0.000162273 141.329 0.000202903 -0.0178749 0.99984 -1.11501\n0.00374345 -0.999859 -0.0163847 77.0046 0.999244 0.00310594 0.0387627 139.433 -0.0387063 -0.0165174 0.999114 -1.06447\n0.00419809 -0.999817 -0.0186847 76.9851 0.997852 0.0054098 -0.0652796 137.393 0.0653687 -0.0183706 0.997692 -1.07231\n0.00852608 -0.999767 -0.019854 76.9662 0.999932 0.00836525 0.00816955 135.518 -0.00800156 -0.0199223 0.99977 -1.00072\n0.00969564 -0.999732 -0.0209985 76.9413 0.999883 0.00994179 -0.0116497 133.459 0.0118553 -0.0208831 0.999712 -1.03759\n0.0119556 -0.999789 -0.0166815 76.9361 0.999572 0.012395 -0.0264918 132.097 0.026693 -0.0163576 0.99951 -0.968264\n0.0144639 -0.999829 -0.0115591 76.9055 0.99988 0.0143977 0.005788 130.014 -0.00562058 -0.0116414 0.999916 -0.937787\n0.0163872 -0.999861 -0.00292287 76.883 0.999839 0.0164081 -0.00727502 127.893 0.00732197 -0.00280318 0.999969 -0.911593\n0.0179732 -0.999832 -0.00361247 76.8546 0.999817 0.0179965 -0.00651968 126.45 0.0065836 -0.00349463 0.999972 -0.890512\n0.0209873 -0.999761 -0.00607758 76.7998 0.99977 0.0210135 -0.00427734 124.258 0.00440403 -0.00598641 0.999972 -0.852499\n0.02289 -0.999683 -0.0104669 76.7536 0.999716 0.0229583 -0.00644727 122.764 0.00668553 -0.0103164 0.999924 -0.832324\n0.0264081 -0.999488 -0.0180642 76.6788 0.999604 0.026578 -0.00923104 120.505 0.00970642 -0.0178133 0.999794 -0.795151\n0.031755 -0.999264 -0.0215241 76.5904 0.999453 0.0319447 -0.00852684 118.218 0.00920814 -0.0212416 0.999732 -0.76196\n0.0417828 -0.998927 -0.0199757 76.4791 0.999088 0.0419484 -0.00794208 115.867 0.0087715 -0.0196256 0.999769 -0.711894\n0.0562473 -0.998256 -0.0179234 76.3347 0.99836 0.056427 -0.00968483 113.484 0.0106793 -0.0173492 0.999792 -0.684458\n0.0795053 -0.996718 -0.0152201 76.1326 0.99681 0.0796002 -0.00573145 111.068 0.00692416 -0.0147159 0.999868 -0.653137\n0.0959294 -0.99526 -0.0159684 75.9667 0.995379 0.0959832 -0.00264153 109.43 0.00416171 -0.0156412 0.999869 -0.627141\n0.113448 -0.993397 -0.0171084 75.7642 0.993528 0.113528 -0.00377058 107.789 0.00568797 -0.0165699 0.999847 -0.594752\n0.141731 -0.989708 -0.0197645 75.3853 0.989864 0.141878 -0.00624525 105.29 0.00898513 -0.018679 0.999785 -0.556379\n0.161657 -0.986621 -0.0211204 75.0884 0.986809 0.161801 -0.00532622 103.614 0.00867227 -0.0199808 0.999763 -0.52706\n0.193462 -0.980955 -0.0173236 74.6058 0.981071 0.193578 -0.00525025 101.089 0.00850373 -0.01598 0.999836 -0.480754\n0.216151 -0.976266 -0.0135764 74.2221 0.976341 0.216213 -0.00325375 99.3859 0.00611191 -0.0125519 0.999903 -0.46179\n0.25114 -0.967883 -0.011443 73.5523 0.967939 0.251178 -0.00198784 96.8273 0.00479823 -0.0105769 0.999933 -0.408302\n0.287836 -0.957611 -0.0114968 72.7899 0.957665 0.287877 -0.00211876 94.2819 0.00533862 -0.0104003 0.999932 -0.373046\n0.323378 -0.946126 -0.0165056 71.8952 0.946243 0.32345 -0.00186501 91.7112 0.00710328 -0.0150152 0.999862 -0.334353\n0.348852 -0.93701 -0.0177557 71.2497 0.937139 0.348947 -0.00247297 89.9993 0.00851299 -0.0157768 0.999839 -0.296693\n0.384961 -0.922754 -0.0181801 70.1872 0.922898 0.385043 -0.00109194 87.4751 0.0080077 -0.016358 0.999834 -0.250524\n0.419708 -0.907438 -0.020061 69.0029 0.907607 0.419818 -0.0014419 84.9567 0.00973039 -0.0176023 0.999798 -0.218511\n0.450643 -0.89242 -0.0225082 67.7283 0.892661 0.450726 0.00154638 82.4488 0.00876498 -0.020789 0.999745 -0.18306\n0.471464 -0.881614 -0.0218826 66.3805 0.881822 0.471582 -0.000305023 79.9377 0.0105884 -0.0191528 0.999761 -0.125094\n0.487519 -0.872926 -0.0180229 64.9744 0.873059 0.487613 -0.000978338 77.4198 0.0096422 -0.0152581 0.999837 -0.0973393\n0.499798 -0.86592 -0.0195817 63.5108 0.866087 0.499893 0.000100082 74.9114 0.0097021 -0.0170095 0.999808 -0.0694071\n0.50262 -0.864176 -0.0239162 62.5032 0.864422 0.502766 -9.57893e-05 73.2286 0.012107 -0.0206256 0.999714 -0.0412776\n0.501345 -0.864913 -0.0240671 61.5022 0.86517 0.501478 0.000563113 71.5275 0.0115821 -0.0211045 0.99971 -0.0104893\n0.500555 -0.865405 -0.0228008 60.5155 0.865644 0.500658 0.00135242 69.8233 0.010245 -0.0204143 0.999739 0.0132423\n0.498853 -0.866346 -0.0242888 59.0191 0.866644 0.498911 0.00406955 67.2467 0.00859233 -0.0230799 0.999697 0.0500917\n0.498325 -0.866609 -0.0257183 58.0137 0.866947 0.498378 0.00476714 65.5265 0.00868618 -0.0246719 0.999658 0.0844487\n0.497179 -0.867261 -0.0258989 57.0158 0.867598 0.497248 0.00416012 63.7867 0.00927027 -0.0245382 0.999656 0.127833\n0.494597 -0.868653 -0.0285534 55.5214 0.869032 0.494753 0.00183262 61.2018 0.012535 -0.0257202 0.999591 0.164662\n0.49125 -0.87049 -0.0303285 54.5588 0.870926 0.491407 0.00254429 59.4872 0.0126889 -0.0276638 0.999537 0.197106\n0.485819 -0.873504 -0.0311514 53.5953 0.873954 0.486006 0.00178351 57.7644 0.0135819 -0.0280914 0.999513 0.218206\n0.479517 -0.87695 -0.0319779 52.6513 0.877424 0.479713 0.001736 56.0525 0.0138178 -0.0288906 0.999487 0.239514\n0.465021 -0.884736 -0.0315925 51.2785 0.885254 0.465062 0.00648592 53.5075 0.00895413 -0.0309835 0.99948 0.286549\n0.445778 -0.894552 -0.0325316 49.979 0.895097 0.445829 0.00605173 50.9465 0.00908997 -0.0318167 0.999452 0.348604\n0.424941 -0.904603 -0.03346 48.758 0.905166 0.425032 0.00470053 48.3951 0.00996945 -0.0322843 0.999429 0.399679\n0.403006 -0.91442 -0.0377045 47.6186 0.915127 0.403143 0.00423404 45.8393 0.0113286 -0.0362107 0.99928 0.443713\n0.388167 -0.920941 -0.0345646 46.8989 0.921549 0.388227 0.00524809 44.1303 0.00858573 -0.0338901 0.999389 0.471289\n0.36156 -0.931574 -0.0379967 45.8915 0.932227 0.361871 -0.00141586 41.582 0.0150689 -0.0349097 0.999277 0.526646\n0.333324 -0.942309 -0.0308117 45.0122 0.942723 0.333563 -0.00284765 39.0775 0.012961 -0.0280977 0.999521 0.581763\n0.315541 -0.948518 -0.0273534 44.4776 0.948859 0.315697 -0.00148358 37.4663 0.0100426 -0.0254864 0.999625 0.611135\n0.291374 -0.956315 -0.0237325 43.7598 0.956518 0.2916 -0.006609 35.0928 0.0132407 -0.0207749 0.999696 0.66074\n0.275973 -0.960915 -0.0219579 43.3393 0.96109 0.276165 -0.00619311 33.5659 0.0120151 -0.0193944 0.99974 0.690262\n0.253621 -0.967087 -0.020463 42.7623 0.967236 0.253797 -0.0064604 31.3396 0.0114412 -0.018154 0.99977 0.732198\n0.232396 -0.972416 -0.0199858 42.2684 0.972569 0.232547 -0.00555347 29.2206 0.0100479 -0.018147 0.999785 0.774738\n0.211746 -0.97715 -0.0185014 41.833 0.977289 0.211863 -0.00459169 27.1743 0.00840654 -0.0171089 0.999818 0.815536\n0.198167 -0.97998 -0.0191982 41.5614 0.980134 0.198286 -0.0044715 25.834 0.00818871 -0.0179307 0.999806 0.841816\n0.184843 -0.982532 -0.0215426 41.3269 0.982739 0.184962 -0.00364203 24.5029 0.00756298 -0.0204976 0.999761 0.853168\n0.166703 -0.985638 -0.026964 40.9883 0.985844 0.166115 0.0227519 22.5817 -0.017946 -0.0303751 0.999377 0.973382\n0.152886 -0.987848 -0.027956 40.6995 0.988159 0.153182 -0.0087565 20.6256 0.0129325 -0.0262863 0.999571 0.957169\n0.147778 -0.988253 -0.0389498 40.5033 0.988522 0.148839 -0.0259048 19.3766 0.0313978 -0.0346746 0.998905 1.0166\n0.146229 -0.988935 -0.024995 40.26 0.989232 0.146335 -0.00244099 17.5905 0.00607162 -0.0243689 0.999685 1.04166\n0.146486 -0.988672 -0.0326971 39.9862 0.989211 0.146472 0.00284222 15.8536 0.00197919 -0.0327607 0.999461 1.08707\n0.149978 -0.988292 -0.0280349 39.7435 0.988675 0.150066 -0.00107369 14.1788 0.00526821 -0.0275564 0.999606 1.13081\n0.15305 -0.987751 -0.0303933 39.5694 0.988211 0.153098 0.000747347 13.118 0.00391497 -0.0301493 0.999538 1.15259\n0.163775 -0.986151 -0.0261635 39.3138 0.986497 0.163687 0.0054949 11.5934 -0.00113619 -0.0267101 0.999643 1.21461\n0.185305 -0.982413 -0.0229444 39.038 0.982681 0.185228 0.00543532 10.1265 -0.00108979 -0.0235542 0.999722 1.24629\n0.225553 -0.974072 -0.0176055 38.6722 0.974219 0.225602 -0.000778455 8.71997 0.0047301 -0.0169761 0.999845 1.29446\n0.288075 -0.957493 -0.0147954 38.225 0.957604 0.288082 0.00170094 7.36098 0.00263365 -0.0146581 0.999889 1.30997\n0.367004 -0.930123 -0.0133854 37.6704 0.930202 0.367048 -0.00083121 6.08029 0.0056862 -0.0121461 0.99991 1.35018\n0.496953 -0.86768 -0.0130063 36.7004 0.867763 0.496975 0.00170253 4.51853 0.00498656 -0.0121325 0.999914 1.3773\n0.596509 -0.802362 -0.0197782 35.7735 0.802477 0.596674 -0.00324467 3.45297 0.0144045 -0.0139361 0.999799 1.40995\n0.692369 -0.721241 -0.0208972 34.7357 0.721342 0.69257 -0.00357629 2.48929 0.0170521 -0.0125979 0.999775 1.40233\n0.806152 -0.590859 -0.0317075 33.1362 0.591185 0.806535 0.00115625 1.46104 0.0248901 -0.0196771 0.999497 1.40033\n0.877981 -0.477715 -0.0306143 31.8037 0.478038 0.878331 0.00381186 0.844133 0.0250685 -0.0179815 0.999524 1.38002\n0.916087 -0.399679 -0.0322817 30.8325 0.400249 0.916307 0.0134539 0.537005 0.0242027 -0.0252456 0.999388 1.36619\n0.945291 -0.324539 -0.0331658 29.8299 0.325233 0.94546 0.0181152 0.272588 0.0254779 -0.0279107 0.999286 1.37161\n0.972915 -0.229138 -0.0305283 28.2343 0.230065 0.972668 0.0314125 -0.0156809 0.0224961 -0.0375852 0.99904 1.34232\n0.983154 -0.180013 -0.0316635 27.1477 0.181055 0.98289 0.0338445 -0.164533 0.0250293 -0.0390072 0.998925 1.37186\n0.989912 -0.138033 -0.031946 26.0195 0.138977 0.989856 0.0295156 -0.285402 0.0275478 -0.0336576 0.999054 1.34702\n0.994271 -0.100822 -0.035491 24.8551 0.102165 0.994029 0.0383159 -0.3535 0.031416 -0.0417224 0.998635 1.29995\n0.997355 -0.0666191 -0.0290504 23.0609 0.0678643 0.996714 0.0442211 -0.424997 0.0260089 -0.0460756 0.998599 1.35327\n0.998433 -0.0497337 -0.0256352 21.8551 0.0507274 0.997923 0.0396948 -0.48215 0.0236078 -0.040933 0.998883 1.30682\n0.999056 -0.0274189 -0.0337082 19.9431 0.0288234 0.998705 0.0419122 -0.502262 0.0325154 -0.0428442 0.998553 1.30641\n0.999467 -0.0158045 -0.0285603 18.6467 0.0169138 0.999095 0.0390256 -0.521113 0.0279177 -0.0394879 0.99883 1.2972\n0.999653 -0.00498309 -0.025869 17.3181 0.00596983 0.999252 0.0382076 -0.51809 0.0256592 -0.0383488 0.998935 1.27625\n0.999633 0.00303555 -0.026918 15.9711 -0.00193103 0.999159 0.0409643 -0.506273 0.0270197 -0.0408973 0.998798 1.26888\n0.999586 0.00775321 -0.0276909 14.5801 -0.00657198 0.999075 0.0424971 -0.476379 0.0279948 -0.0422975 0.998713 1.25988\n0.999485 0.00741094 -0.0312334 12.4509 -0.00597909 0.998938 0.0456904 -0.437754 0.0315388 -0.0454801 0.998467 1.24303\n0.999571 0.00674064 -0.0284967 11.015 -0.00548525 0.999021 0.0439047 -0.432516 0.0287648 -0.0437295 0.998629 1.24234\n0.999634 0.00506553 -0.0265904 9.56164 -0.00391641 0.999063 0.0430909 -0.420921 0.0267838 -0.0429709 0.998717 1.22274\n0.999582 0.00369184 -0.0286738 8.06638 -0.00244738 0.999058 0.0433154 -0.413908 0.0288067 -0.0432271 0.99865 1.21763\n0.999668 0.00252652 -0.0256526 6.54236 -0.00147352 0.999159 0.0409846 -0.407103 0.0257346 -0.0409332 0.99883 1.21549\n0.999727 0.0013644 -0.0233457 5.01558 -0.00043131 0.999203 0.0399269 -0.412125 0.0233816 -0.0399059 0.99893 1.2065\n0.99968 -0.000285631 -0.0252987 2.64347 0.00129444 0.999204 0.0398684 -0.409762 0.0252672 -0.0398884 0.998885 1.19987\n0.999618 -0.00148777 -0.0276084 1.04832 0.00257752 0.999217 0.0394783 -0.413168 0.0275281 -0.0395344 0.998839 1.19027\n0.999473 -0.00278153 -0.0323382 -0.592145 0.00410163 0.999158 0.0408271 -0.394652 0.0321974 -0.0409382 0.998643 1.17467\n0.999559 -0.00247719 -0.0295783 -2.21906 0.00363225 0.99923 0.0390614 -0.39588 0.0294588 -0.0391516 0.998799 1.17726\n0.999591 6.50547e-05 -0.0285996 -5.5661 0.0010472 0.999244 0.0388737 -0.391944 0.0285805 -0.0388878 0.998835 1.14616\n0.999492 0.00263583 -0.0317459 -7.29465 -0.00138263 0.999221 0.0394335 -0.385614 0.0318251 -0.0393696 0.998718 1.12893\n0.999611 0.00441836 -0.0275245 -9.02795 -0.00334725 0.99924 0.0388403 -0.365611 0.0276751 -0.038733 0.998866 1.12434\n0.999654 0.00675289 -0.0254081 -11.683 -0.00579559 0.999277 0.0375639 -0.346954 0.0256434 -0.0374036 0.998971 1.11061\n0.999543 0.00817121 -0.0290979 -13.4922 -0.00708123 0.999277 0.037367 -0.320258 0.0293821 -0.0371438 0.998878 1.09678\n0.99962 0.00816102 -0.0263336 -15.3077 -0.00717308 0.999275 0.0373953 -0.305178 0.0266197 -0.0371922 0.998954 1.08427\n0.999561 0.00750767 -0.0286766 -17.1426 -0.00640876 0.999249 0.0382223 -0.286867 0.028942 -0.0380217 0.998858 1.08201\n0.999581 0.00679491 -0.0281493 -19.0185 -0.00573225 0.999274 0.0376609 -0.259177 0.0283848 -0.0374837 0.998894 1.06444\n0.99962 0.00647214 -0.0267836 -20.9026 -0.00545927 0.999273 0.0377186 -0.255331 0.0270083 -0.0375581 0.998929 1.05473\n0.999652 0.00491264 -0.0259289 -22.8006 -0.003936 0.999286 0.0375833 -0.225569 0.026095 -0.0374681 0.998957 1.05557\n0.999558 0.00326809 -0.029551 -25.681 -0.00211726 0.999241 0.0388917 -0.215404 0.0296557 -0.0388119 0.998806 1.03167\n0.999578 0.0025935 -0.0289468 -27.6049 -0.00139613 0.999145 0.0413083 -0.204192 0.0290292 -0.0412505 0.998727 1.02901\n0.999705 0.000714068 -0.0242766 -30.5383 0.000253285 0.999207 0.0398207 -0.187773 0.0242858 -0.0398151 0.998912 1.02927\n0.999524 -5.6765e-05 -0.0308421 -32.531 0.00120628 0.999305 0.0372538 -0.182298 0.0308185 -0.0372733 0.99883 1.02282\n0.999608 -0.00084505 -0.0280023 -34.4954 0.00185609 0.999346 0.0360994 -0.196039 0.0279535 -0.0361372 0.998956 1.00323\n0.999568 -0.000955418 -0.0293711 -36.4652 0.00204991 0.999304 0.0372567 -0.186404 0.029315 -0.0373008 0.998874 0.99765\n0.999408 -0.00154335 -0.0343755 -38.4459 0.00287668 0.999244 0.0387718 -0.177175 0.0342897 -0.0388477 0.998657 0.98586\n0.999529 -0.00220878 -0.0305995 -40.3995 0.0033894 0.99925 0.0385851 -0.184294 0.0304914 -0.0386706 0.998787 0.977837\n0.999554 -0.00373961 -0.0296121 -44.3395 0.00492114 0.99919 0.0399286 -0.186546 0.0294388 -0.0400565 0.998764 0.958624\n0.999319 -0.00489861 -0.0365815 -47.3125 0.00641048 0.999125 0.0413265 -0.201718 0.0363471 -0.0415328 0.998476 0.943173\n0.999687 -0.00525649 -0.0244751 -49.2757 0.00623015 0.999185 0.0398766 -0.216098 0.0242455 -0.0400166 0.998905 0.938583\n0.99967 -0.0058291 -0.0250108 -51.247 0.00682502 0.99918 0.0399208 -0.208081 0.0247576 -0.0400784 0.99889 0.92497\n0.999364 -0.00692587 -0.0349739 -53.2253 0.00834562 0.99914 0.0406132 -0.214602 0.0346626 -0.0408792 0.998563 0.918959\n0.999461 -0.00705464 -0.0320749 -55.1701 0.00837923 0.99911 0.0413516 -0.231965 0.0317546 -0.041598 0.99863 0.91383\n0.99967 -0.00765056 -0.024518 -57.1171 0.00862097 0.999174 0.0397211 -0.244309 0.0241938 -0.0399194 0.99891 0.908594\n0.999538 -0.00786681 -0.0293605 -59.0548 0.00903272 0.999167 0.039791 -0.248256 0.029023 -0.0400378 0.998777 0.899101\n0.999356 -0.0081415 -0.0349512 -60.9571 0.00957107 0.999116 0.0409312 -0.259349 0.0345871 -0.0412393 0.99855 0.894465\n0.999583 -0.00841171 -0.0276299 -62.8403 0.0094913 0.999187 0.0391773 -0.274637 0.0272779 -0.0394232 0.99885 0.903703\n0.999695 -0.00874889 -0.0231114 -64.7327 0.0095624 0.99933 0.0353267 -0.298156 0.0227869 -0.0355369 0.999109 0.903149\n0.999579 -0.00641963 -0.028281 -66.6007 0.00736162 0.999417 0.0333309 -0.308595 0.0280506 -0.0335251 0.999044 0.890601\n0.999563 -0.00432531 -0.0292306 -68.4527 0.00532242 0.999404 0.0341206 -0.307817 0.0290656 -0.0342613 0.99899 0.897254\n0.999534 -0.00233965 -0.0304485 -70.289 0.00328952 0.999508 0.0311835 -0.319397 0.0303605 -0.0312691 0.99905 0.892204\n0.999492 -8.1362e-05 -0.0318739 -72.1208 0.00103873 0.999549 0.0300207 -0.319941 0.031857 -0.0300385 0.999041 0.88531\n0.999559 0.00211041 -0.029617 -73.9321 -0.00127591 0.999602 0.028167 -0.309811 0.0296646 -0.0281168 0.999164 0.878541\n0.999379 0.0039473 -0.0350036 -75.7439 -0.0029437 0.999584 0.0286767 -0.30359 0.0351022 -0.0285558 0.998976 0.858904\n0.999209 0.00787407 -0.038982 -78.4269 -0.00663291 0.99947 0.031867 -0.270811 0.0392123 -0.0315832 0.998732 0.841982\n0.999754 0.0112238 -0.0191409 -81.0539 -0.0106897 0.999557 0.027781 -0.244338 0.0194442 -0.0275696 0.999431 0.839112\n0.999502 0.0147055 -0.0279091 -82.8308 -0.0140559 0.999629 0.0233282 -0.225881 0.0282418 -0.0229243 0.999338 0.838365\n0.999796 0.0189653 -0.00694458 -84.5568 -0.0187352 0.999318 0.0318179 -0.17554 0.00754328 -0.0316813 0.99947 0.848818\n0.999727 0.0233842 0.000232451 -86.3114 -0.0233757 0.998978 0.0386949 -0.0995213 0.000672637 -0.0386897 0.999251 0.876035\n0.996981 0.0291069 -0.0719801 -88.9866 -0.0271768 0.999248 0.0276501 -0.0429263 0.0727308 -0.0256105 0.997023 0.855817\n0.999085 0.0347119 -0.0249682 -90.6903 -0.0340526 0.999072 0.0263618 0.0149962 0.0258601 -0.0254875 0.999341 0.831662\n0.999142 0.0391176 -0.0135738 -92.4436 -0.0387364 0.998877 0.0272981 0.100347 0.0146264 -0.0267489 0.999535 0.803641\n0.998563 0.0447277 -0.0295092 -95.1562 -0.0438431 0.998589 0.0299726 0.238548 0.0308082 -0.0286358 0.999115 0.797441\n0.998448 0.0489703 -0.0265136 -96.9622 -0.0480767 0.998287 0.033352 0.329483 0.0281014 -0.0320255 0.999092 0.796359\n0.998223 0.0530074 -0.0272122 -98.7776 -0.0521717 0.998171 0.0305557 0.428654 0.0287821 -0.0290817 0.999163 0.784074\n0.998002 0.0570888 -0.0270537 -100.622 -0.0561685 0.997855 0.0336358 0.561977 0.0289159 -0.032049 0.999068 0.768444\n0.997842 0.0612147 -0.0237692 -102.473 -0.060421 0.997634 0.0327883 0.676768 0.02572 -0.0312813 0.99918 0.778347\n0.997497 0.0671393 -0.0221731 -105.278 -0.0665014 0.997383 0.0283534 0.857713 0.0240187 -0.0268079 0.999352 0.771844\n0.996886 0.0716564 -0.0329131 -107.17 -0.0707358 0.997093 0.0283341 1.00739 0.0348477 -0.0259177 0.999057 0.759954\n0.996776 0.0754865 -0.0272001 -109.059 -0.0747901 0.996866 0.0257686 1.13629 0.02906 -0.0236512 0.999298 0.755919\n0.99665 0.0802802 -0.0156416 -110.956 -0.0798928 0.996515 0.0239965 1.2999 0.0175136 -0.0226665 0.99959 0.751647\n0.996115 0.0846861 -0.0241518 -112.906 -0.0840932 0.996154 0.0245908 1.47545 0.0261414 -0.0224642 0.999406 0.743733\n0.995441 0.0935304 -0.0186825 -116.765 -0.0930611 0.995357 0.0245864 1.84704 0.0208953 -0.0227357 0.999523 0.721086\n0.995179 0.0972449 -0.0127076 -118.695 -0.0968896 0.994953 0.0260939 2.05958 0.0151809 -0.0247368 0.999579 0.737582\n0.99481 0.10047 -0.0160744 -119.7 -0.100016 0.994621 0.0269317 2.16029 0.0186938 -0.0251842 0.999508 0.745022\n0.99441 0.104594 -0.0144159 -121.666 -0.104221 0.994251 0.0245721 2.36371 0.0169031 -0.0229323 0.999594 0.765102\n0.993832 0.109184 -0.0194136 -123.658 -0.108631 0.993699 0.0275855 2.59657 0.0223032 -0.0253065 0.999431 0.772322\n0.993333 0.111585 -0.0289472 -124.651 -0.110748 0.993424 0.0290434 2.7206 0.0319976 -0.0256439 0.999159 0.771168\n0.992639 0.116578 -0.0328343 -126.675 -0.115616 0.992846 0.0298157 2.94707 0.0360753 -0.0258001 0.999016 0.735268\n0.992358 0.121074 -0.0237827 -128.664 -0.120369 0.992301 0.0291417 3.20026 0.0271279 -0.0260563 0.999292 0.730661\n0.991958 0.124 -0.0253531 -130.71 -0.123256 0.991949 0.0290634 3.46139 0.0287529 -0.0257047 0.999256 0.706397\n0.991759 0.12472 -0.029311 -131.724 -0.123791 0.991805 0.0316033 3.5969 0.0330123 -0.0277144 0.999071 0.699149\n0.99174 0.124912 -0.0291355 -133.733 -0.123897 0.991699 0.0343927 3.84957 0.0331897 -0.0304988 0.998984 0.685672\n0.991994 0.12342 -0.0267529 -135.776 -0.12243 0.991827 0.0359273 4.13525 0.0309684 -0.0323643 0.998996 0.680107\n0.992029 0.122884 -0.0279075 -136.801 -0.12187 0.99191 0.0355312 4.24837 0.032048 -0.0318469 0.998979 0.671625\n0.992133 0.121352 -0.0307428 -138.842 -0.120343 0.99219 0.0328109 4.50695 0.0344844 -0.0288531 0.998989 0.652396\n0.992353 0.120127 -0.0283559 -140.903 -0.119247 0.992383 0.0309155 4.77271 0.0318537 -0.0272978 0.99912 0.63814\n0.992517 0.119231 -0.0263245 -142.962 -0.118387 0.992465 0.0315834 5.01667 0.0298919 -0.0282306 0.999154 0.610135\n0.992498 0.119067 -0.0277781 -143.991 -0.118174 0.992482 0.0318621 5.13373 0.031363 -0.0283405 0.999106 0.620109\n0.992556 0.118499 -0.0281254 -146.017 -0.117645 0.992593 0.0303022 5.38362 0.0315079 -0.0267678 0.999145 0.599137\n0.992614 0.117511 -0.0301313 -148.043 -0.116636 0.992741 0.0293453 5.6189 0.033361 -0.0256142 0.999115 0.587705\n0.992655 0.117515 -0.0287434 -149.066 -0.116699 0.992756 0.0285956 5.74865 0.0318955 -0.0250312 0.999178 0.581228\n0.992681 0.11812 -0.0251344 -153.031 -0.117332 0.992615 0.0307968 6.22265 0.0285865 -0.0276223 0.99921 0.561124\n0.992635 0.11846 -0.0253647 -154.049 -0.117631 0.992541 0.0320075 6.35795 0.0289671 -0.0287881 0.999166 0.556988\n0.992446 0.119287 -0.0286447 -156.024 -0.118296 0.992392 0.0341167 6.59302 0.0324964 -0.0304704 0.999007 0.549406\n0.992453 0.119576 -0.02718 -157.98 -0.118617 0.99234 0.0345022 6.84371 0.0310974 -0.0310177 0.999035 0.543907\n0.992499 0.119602 -0.0253171 -159.913 -0.118717 0.992347 0.0339781 7.07775 0.0291872 -0.0307176 0.999102 0.541288\n0.992383 0.119573 -0.0296378 -162.814 -0.118534 0.992346 0.0346321 7.44812 0.033552 -0.0308552 0.998961 0.51408\n0.992374 0.119221 -0.0313111 -164.717 -0.118113 0.992378 0.0351382 7.6721 0.0352616 -0.031172 0.998892 0.504084\n0.992531 0.118562 -0.028741 -166.644 -0.117555 0.992466 0.0345273 7.90896 0.0326181 -0.0308908 0.99899 0.4874\n0.992672 0.1178 -0.0269497 -169.49 -0.116829 0.992529 0.0351732 8.25659 0.0308918 -0.0317669 0.999018 0.471384\n0.992777 0.11756 -0.0239375 -171.354 -0.116669 0.992527 0.0357552 8.48758 0.027962 -0.0327042 0.999074 0.471036\n0.992733 0.117453 -0.0261783 -173.218 -0.11656 0.992623 0.0333608 8.70154 0.0299035 -0.030067 0.9991 0.475257\n0.992732 0.116777 -0.0290978 -176 -0.115876 0.992782 0.0309383 9.02912 0.0325007 -0.0273417 0.999098 0.454622\n0.992817 0.116612 -0.0267412 -177.832 -0.11584 0.99285 0.0288112 9.24663 0.0299097 -0.0255066 0.999227 0.452003\n0.992924 0.116093 -0.0250006 -179.64 -0.115406 0.992941 0.0273686 9.45568 0.0280014 -0.0242897 0.999313 0.456486\n0.992976 0.115385 -0.0261886 -181.447 -0.114789 0.993119 0.023206 9.67584 0.028686 -0.0200368 0.999388 0.466837\n0.993396 0.114734 0.000679983 -184.094 -0.114714 0.993076 0.0253226 9.98609 0.00223009 -0.0252334 0.999679 0.467957\n0.990946 0.117189 -0.0655144 -187.689 -0.11507 0.992732 0.0352385 10.4377 0.0691678 -0.0273807 0.997229 0.431533\n0.992462 0.120471 -0.0225076 -189.425 -0.119867 0.992436 0.0265114 10.6372 0.0255312 -0.0236136 0.999395 0.429398\n0.992554 0.121794 -0.00156563 -191.177 -0.121705 0.992183 0.0275743 10.8682 0.00491178 -0.0271784 0.999619 0.423461\n0.991724 0.124199 -0.0325335 -192.999 -0.123046 0.991772 0.0353277 11.1132 0.0366535 -0.0310322 0.998846 0.425472\n0.991528 0.125392 -0.0339071 -194.809 -0.12438 0.991767 0.0304572 11.3292 0.037447 -0.0259818 0.998961 0.408289\n0.991756 0.126381 -0.0211626 -196.578 -0.125601 0.991468 0.0348585 11.5678 0.0253875 -0.0319131 0.999168 0.371526\n0.991643 0.126703 -0.0243151 -198.379 -0.125745 0.991352 0.0375488 11.8067 0.0288623 -0.0341775 0.998999 0.379542\n0.991869 0.126226 -0.0162075 -200.171 -0.125539 0.991358 0.0380628 12.0382 0.0208719 -0.0357187 0.999144 0.382157\n0.991859 0.125788 -0.0198525 -201.978 -0.124953 0.991403 0.0388161 12.2736 0.0245644 -0.0360195 0.999049 0.382395\n0.991646 0.126542 -0.0250266 -203.787 -0.125593 0.991413 0.036419 12.5069 0.0294202 -0.0329716 0.999023 0.387705\n0.99164 0.126832 -0.0237317 -205.616 -0.125965 0.991416 0.0350295 12.7242 0.0279709 -0.0317473 0.999104 0.369825\n0.991452 0.12822 -0.0241254 -208.344 -0.127358 0.99126 0.0344056 13.0871 0.0283261 -0.031039 0.999117 0.356087\n0.991413 0.128925 -0.0218856 -210.176 -0.128136 0.991164 0.0342863 13.3276 0.0261126 -0.0311876 0.999172 0.347635\n0.991352 0.129156 -0.0232508 -212.013 -0.128291 0.991101 0.0355104 13.5827 0.0276303 -0.0322205 0.999099 0.342392\n0.991226 0.129523 -0.0263519 -213.862 -0.128536 0.991043 0.0362193 13.8374 0.0308071 -0.0325144 0.998996 0.340036\n0.991237 0.130401 -0.021087 -215.682 -0.129628 0.990959 0.0346089 14.0694 0.0254094 -0.0315721 0.999178 0.329882\n0.991251 0.129853 -0.0236584 -217.513 -0.128953 0.990987 0.0362674 14.3202 0.0281546 -0.0328992 0.999062 0.313428\n0.991117 0.130296 -0.0266482 -219.338 -0.129277 0.990921 0.036918 14.5569 0.0312165 -0.0331451 0.998963 0.324293\n0.991566 0.129584 0.00227558 -221.124 -0.129577 0.990853 0.037688 14.7986 0.00262899 -0.037665 0.999287 0.32625\n0.991779 0.12783 -0.00586979 -222.945 -0.127488 0.991 0.0408258 15.0671 0.0110357 -0.0397418 0.999149 0.35454\n0.989913 0.126569 -0.0636538 -225.746 -0.124119 0.991416 0.0410901 15.4089 0.0683082 -0.032775 0.997126 0.303119\n0.991963 0.126387 -0.00605806 -227.539 -0.1261 0.991395 0.0351304 15.6269 0.0104459 -0.0340841 0.999364 0.291783\n0.991975 0.125457 -0.0156897 -229.402 -0.124804 0.991479 0.0373322 15.8848 0.0202396 -0.0350744 0.99918 0.274464\n0.991519 0.125042 -0.0354156 -231.284 -0.123559 0.991475 0.0413709 16.122 0.0402867 -0.0366441 0.998516 0.282103\n0.992041 0.124245 -0.0204469 -233.152 -0.123411 0.991629 0.037966 16.3591 0.0249929 -0.0351404 0.99907 0.275772\n0.992347 0.122821 -0.0127312 -235.044 -0.122348 0.99194 0.0329426 16.5944 0.0166746 -0.0311329 0.999376 0.276667\n0.992221 0.122147 -0.0240514 -236.966 -0.121367 0.992106 0.0315627 16.8209 0.0277168 -0.0283981 0.999212 0.274971\n0.992172 0.121144 -0.0303279 -238.881 -0.12024 0.99229 0.0300576 17.0647 0.0337354 -0.0261757 0.999088 0.266419\n0.992502 0.119836 -0.0240726 -241.743 -0.119064 0.992394 0.03128 17.4173 0.027638 -0.0281793 0.999221 0.243022\n0.992512 0.119158 -0.026854 -243.696 -0.118264 0.992442 0.0327388 17.6705 0.0305521 -0.0293178 0.999103 0.229304\n0.992494 0.119171 -0.0274554 -245.613 -0.118163 0.992348 0.0358098 17.9015 0.0315128 -0.0322968 0.998981 0.237896\n0.992889 0.117221 -0.0207689 -247.537 -0.116472 0.992604 0.034221 18.1457 0.0246267 -0.0315586 0.999198 0.232074\n0.992966 0.116507 -0.0210924 -249.443 -0.115787 0.992737 0.0326684 18.3671 0.0247453 -0.0299964 0.999244 0.233875\n0.992909 0.116081 -0.0256385 -251.334 -0.115218 0.992795 0.0329144 18.5959 0.0292746 -0.029727 0.999129 0.230245\n0.992962 0.115585 -0.0257983 -253.204 -0.114683 0.992817 0.0340877 18.8106 0.029553 -0.0308892 0.999086 0.228896\n0.993013 0.1151 -0.0260203 -255.072 -0.114143 0.992823 0.0356944 19.0373 0.029942 -0.032475 0.999024 0.22008\n0.993069 0.114636 -0.0259235 -256.914 -0.113656 0.992846 0.0365912 19.2518 0.0299327 -0.0333913 0.998994 0.218238\n0.993258 0.113845 -0.0218786 -258.758 -0.112995 0.992914 0.0367845 19.4713 0.0259113 -0.0340643 0.999084 0.209126\n0.993251 0.113238 -0.0251098 -260.584 -0.112252 0.992964 0.0377186 19.6901 0.0292043 -0.0346454 0.998973 0.207327\n0.993595 0.112666 -0.00864465 -263.292 -0.1123 0.993057 0.0350184 19.9772 0.01253 -0.0338233 0.999349 0.240967\n0.99318 0.11149 0.0341002 -265.015 -0.112856 0.992758 0.0411661 20.2064 -0.0292636 -0.0447338 0.99857 0.285576\n0.991766 0.111196 -0.0635282 -267.727 -0.108271 0.992973 0.0477777 20.5335 0.0683945 -0.040506 0.996836 0.29634\n0.99125 0.110921 -0.0715598 -269.506 -0.107898 0.993151 0.0448145 20.7122 0.0760406 -0.0367012 0.996429 0.241684\n0.993785 0.111175 0.00558038 -271.201 -0.111302 0.993203 0.0340435 20.885 -0.00175765 -0.034453 0.999405 0.246812\n0.993893 0.110294 -0.00338986 -272.95 -0.110101 0.993264 0.0361237 21.1198 0.00735126 -0.0355299 0.999342 0.243176\n0.993488 0.110074 -0.029422 -274.742 -0.108935 0.993328 0.0378531 21.3177 0.0333923 -0.0344015 0.99885 0.255291\n0.993952 0.108377 -0.0176989 -276.482 -0.107624 0.993423 0.0390758 21.5016 0.0218174 -0.0369347 0.999079 0.249848\n0.994387 0.105193 -0.0113305 -278.225 -0.104675 0.993728 0.0393306 21.6983 0.0153968 -0.0379239 0.999162 0.266677\n0.994734 0.101927 -0.0107601 -280.011 -0.101483 0.994191 0.0358408 21.8729 0.0143507 -0.0345601 0.9993 0.294747\n0.995049 0.0985446 -0.0128783 -281.778 -0.0980548 0.994589 0.0343207 22.0495 0.0161907 -0.032888 0.999328 0.300153\n0.995449 0.0936564 -0.0176128 -284.44 -0.0930338 0.995104 0.0333544 22.3008 0.0206505 -0.031564 0.999288 0.313042\n0.99573 0.0905096 -0.0181365 -286.22 -0.0898717 0.995395 0.0333471 22.4614 0.0210712 -0.0315747 0.999279 0.316459\n0.996074 0.0870719 -0.0159459 -288.011 -0.0865034 0.995691 0.0334206 22.62 0.0187872 -0.03191 0.999314 0.323831\n0.99654 0.0816581 -0.0155127 -290.692 -0.0811297 0.996187 0.0320916 22.8405 0.0180741 -0.030722 0.999365 0.345667\n0.99682 0.0780395 -0.016102 -292.497 -0.0774997 0.996485 0.0317947 22.9923 0.0185267 -0.0304457 0.999365 0.349559\n0.997056 0.0748284 -0.0167591 -294.311 -0.0742548 0.996707 0.0325717 23.1268 0.0191412 -0.0312314 0.999329 0.356989\n0.997312 0.0711143 -0.0176388 -296.116 -0.0705434 0.997023 0.031113 23.2444 0.0197989 -0.029785 0.99936 0.382667\n0.997819 0.0655061 -0.00810351 -298.819 -0.0652346 0.997414 0.0301602 23.4391 0.0100582 -0.0295658 0.999512 0.403625\n0.997936 0.0621488 -0.0161883 -300.658 -0.0616435 0.997646 0.0300388 23.5499 0.0180171 -0.0289789 0.999418 0.424654\n0.998462 0.0542135 -0.0116152 -304.28 -0.0538615 0.998135 0.0287275 23.7578 0.0131509 -0.0280577 0.99952 0.46954\n0.998585 0.0512019 -0.0143802 -306.076 -0.050785 0.998317 0.0279986 23.8529 0.0157896 -0.0272287 0.999505 0.48831\n0.998724 0.0478259 -0.0162299 -307.847 -0.0473456 0.998464 0.028789 23.9403 0.0175818 -0.0279838 0.999454 0.504051\n0.998912 0.0436166 -0.0165332 -309.604 -0.0430699 0.998556 0.0320937 24.0292 0.0179091 -0.0313467 0.999348 0.513865\n0.999278 0.0369815 -0.0087318 -312.218 -0.0367218 0.998927 0.028234 24.1267 0.00976656 -0.027893 0.999563 0.563586\n0.999449 0.0331779 0.000630236 -313.928 -0.0331828 0.999081 0.0271298 24.1786 0.000270452 -0.0271357 0.999632 0.60247\n0.999162 0.0298699 0.0279955 -315.612 -0.0306339 0.999159 0.027271 24.2296 -0.0271574 -0.0281057 0.999236 0.674165\n0.999577 0.0263093 0.0123687 -317.345 -0.0266326 0.999288 0.0267364 24.2868 -0.0116565 -0.0270545 0.999566 0.700934\n0.997614 0.0265068 -0.0637517 -319.177 -0.0246176 0.999239 0.0302393 24.3488 0.0645048 -0.0285977 0.997508 0.693601\n0.998474 0.0275873 -0.0478408 -320.915 -0.0263422 0.999303 0.0264632 24.3836 0.0485375 -0.0251626 0.998504 0.708452\n0.99938 0.025935 0.023801 -322.616 -0.0265118 0.999354 0.0242479 24.4264 -0.0231568 -0.0248639 0.999423 0.710866\n0.999692 0.0246799 -0.00261819 -324.418 -0.024622 0.999492 0.0202469 24.4638 0.00311655 -0.0201762 0.999792 0.749009\n0.999715 0.0229743 -0.00653917 -326.185 -0.0228615 0.999597 0.0168299 24.5023 0.00692319 -0.0166756 0.999837 0.783254\n0.999767 0.0215183 -0.00151113 -327.947 -0.0214879 0.999609 0.0178714 24.548 0.0018951 -0.0178348 0.999839 0.788973\n0.999722 0.0200248 -0.0124231 -329.738 -0.0197694 0.999597 0.0203533 24.5816 0.0128257 -0.0201021 0.999716 0.82264\n0.999723 0.0189196 -0.0139977 -331.544 -0.0186394 0.999628 0.019889 24.6289 0.0143688 -0.0196225 0.999704 0.840689\n0.999818 0.0170493 -0.00854777 -333.365 -0.0168474 0.99959 0.0231689 24.6759 0.00893928 -0.0230207 0.999695 0.845639\n0.999822 0.0165211 -0.00913397 -335.176 -0.01627 0.999505 0.0269174 24.7133 0.00957416 -0.026764 0.999596 0.879671\n0.999864 0.0158714 -0.00445148 -337.003 -0.0157427 0.999495 0.027591 24.7464 0.00488714 -0.0275172 0.999609 0.911757\n0.999892 0.0146832 0.000297578 -338.814 -0.014686 0.999586 0.0247464 24.7766 6.59003e-05 -0.0247481 0.999694 0.956057\n0.999897 0.0117154 -0.0082756 -342.539 -0.0115585 0.999757 0.0187671 24.8095 0.00849346 -0.0186695 0.99979 1.02235\n0.999919 0.0109452 -0.00645814 -344.377 -0.0108241 0.99977 0.0184964 24.8396 0.00665911 -0.018425 0.999808 1.05302\n0.99995 0.00965101 -0.00257732 -346.249 -0.00960283 0.99979 0.0180966 24.8554 0.00275143 -0.0180709 0.999833 1.08458\n0.999963 0.00763828 -0.00390379 -348.136 -0.00757074 0.999826 0.0170337 24.8746 0.00403322 -0.0170035 0.999847 1.12694\n0.999964 0.00616785 -0.00578036 -350.033 -0.00607056 0.999842 0.0166999 24.8762 0.00588245 -0.0166642 0.999844 1.15898\n0.999953 0.00436233 -0.00869778 -351.92 -0.00420267 0.999824 0.0182913 24.9004 0.00877604 -0.0182538 0.999795 1.19017\n0.999949 0.00262102 -0.00972264 -353.826 -0.00244691 0.999837 0.0178763 24.9076 0.00976791 -0.0178517 0.999793 1.21988\n0.999987 0.000991427 -0.00500679 -355.677 -0.000901018 0.999837 0.0180274 24.9067 0.00502385 -0.0180226 0.999825 1.25502\n0.999972 -0.00031403 -0.00749447 -357.539 0.000444537 0.999848 0.0174185 24.9122 0.00748786 -0.0174213 0.99982 1.29561\n0.999949 -0.000168383 -0.0100951 -359.394 0.000345522 0.999846 0.0175479 24.9172 0.0100906 -0.0175505 0.999795 1.33048\n0.999974 0.00138524 -0.00705701 -362.109 -0.00125356 0.999826 0.0186301 24.9387 0.00708159 -0.0186207 0.999802 1.37969\n0.999991 0.00257379 -0.00329249 -363.895 -0.00251568 0.999843 0.0175314 24.9278 0.0033371 -0.017523 0.999841 1.41619\n0.999972 0.0034184 -0.00662777 -365.671 -0.00330716 0.999855 0.0167235 24.9442 0.00668397 -0.0167011 0.999838 1.45134\n0.99994 0.00407972 -0.0101699 -367.422 -0.0039078 0.99985 0.016868 24.9516 0.0102372 -0.0168273 0.999806 1.47911\n0.999964 0.00493401 -0.00688836 -369.153 -0.00479871 0.999798 0.0195229 24.9803 0.00698329 -0.0194891 0.999786 1.50499\n0.99999 0.00435723 -0.000247481 -370.899 -0.00435146 0.999794 0.0198455 24.9872 0.000333902 -0.0198442 0.999803 1.54794\n0.999269 0.00287886 0.0381329 -373.454 -0.00370449 0.99976 0.0215983 25.0141 -0.0380615 -0.0217238 0.999039 1.66353\n0.999624 0.000901203 0.0274104 -375.18 -0.00153813 0.999729 0.0232245 25.0131 -0.027382 -0.0232579 0.999354 1.72496\n0.998598 -0.00104488 -0.0529302 -377.021 0.0022755 0.999728 0.0231951 25.0233 0.0528916 -0.023283 0.998329 1.73301\n0.99993 -0.00239379 -0.0115656 -379.619 0.00264694 0.999756 0.0219224 25.039 0.0115103 -0.0219515 0.999693 1.74464\n0.999882 -0.00760162 -0.0133633 -383.178 0.00791758 0.999687 0.0237522 25.008 0.0131786 -0.0238552 0.999629 1.79241\n0.999906 -0.00982211 -0.00954702 -384.998 0.0100565 0.999641 0.0248188 24.9913 0.00929982 -0.0249125 0.999646 1.82378\n0.999904 -0.0125921 0.00578289 -386.772 0.0124437 0.999609 0.0250255 24.9793 -0.00609576 -0.0249512 0.99967 1.85742\n0.999897 -0.014322 -0.000700131 -388.634 0.0143347 0.999612 0.0238729 24.9453 0.000357951 -0.0238805 0.999715 1.89803\n0.999846 -0.0160157 -0.007176 -390.481 0.0161871 0.999569 0.0244893 24.9117 0.0067807 -0.0246016 0.999674 1.93193\n0.999826 -0.018638 -0.000110715 -392.312 0.0186352 0.999529 0.0243735 24.8845 -0.000343612 -0.0243714 0.999703 1.97519\n0.999771 -0.0209725 0.00416636 -394.16 0.0208675 0.999498 0.0238256 24.8509 -0.00466395 -0.0237332 0.999707 2.01651\n0.999741 -0.0227235 -0.00136239 -396.04 0.0227498 0.99945 0.0241436 24.8108 0.00081301 -0.0241683 0.999708 2.05948\n0.999678 -0.0247053 -0.00577164 -397.942 0.0248555 0.999311 0.0275811 24.7676 0.00508626 -0.0277157 0.999603 2.09336\n0.999633 -0.0268777 -0.00338709 -399.807 0.0269632 0.999232 0.0284398 24.7169 0.00262009 -0.0285207 0.99959 2.15917\n0.999557 -0.0297048 0.00176621 -401.633 0.0296448 0.99918 0.0275869 24.6667 -0.00258423 -0.0275224 0.999618 2.199\n0.99949 -0.0317267 -0.00349576 -403.457 0.0318102 0.999124 0.0272049 24.6228 0.00262957 -0.0273022 0.999624 2.24369\n0.999417 -0.033733 -0.00525629 -405.271 0.0338669 0.999037 0.0279021 24.5687 0.00431001 -0.0280638 0.999597 2.2869\n0.999325 -0.0366513 -0.00248101 -407.06 0.0367077 0.998908 0.0289057 24.5089 0.00141887 -0.0289773 0.999579 2.32782\n0.999192 -0.0401577 -0.00139878 -408.825 0.0401813 0.998779 0.0287272 24.436 0.000243454 -0.0287602 0.999586 2.38465\n0.998818 -0.0471379 0.0118262 -411.42 0.0467224 0.998354 0.0332473 24.3427 -0.0133739 -0.0326554 0.999377 2.44152\n0.998478 -0.050858 0.0213173 -413.152 0.0500198 0.998019 0.0381655 24.2705 -0.023216 -0.0370411 0.999044 2.54786\n0.99842 -0.0556741 -0.00766233 -414.938 0.0558775 0.998001 0.029541 24.1724 0.00600234 -0.0299224 0.999534 2.58557\n0.997704 -0.0581041 -0.0347964 -416.718 0.0590385 0.997905 0.0264577 24.0604 0.0331862 -0.0284512 0.999044 2.58517\n0.998174 -0.060269 -0.00391635 -418.444 0.0603532 0.99781 0.0270702 23.9331 0.00227628 -0.0272571 0.999626 2.62202\n0.997721 -0.067369 -0.00364465 -422.033 0.0674433 0.997354 0.027125 23.7145 0.00180762 -0.027309 0.999625 2.70207\n0.99746 -0.0701746 -0.0122222 -423.84 0.0704831 0.997149 0.0269593 23.5952 0.0102955 -0.0277523 0.999562 2.73303\n0.997279 -0.0735478 -0.00506441 -425.658 0.0736683 0.996819 0.0304044 23.4616 0.00281212 -0.0306948 0.999525 2.75599\n0.996968 -0.0775357 -0.0065602 -427.478 0.0777089 0.996447 0.0324796 23.3252 0.00401856 -0.0328909 0.999451 2.79524\n0.996652 -0.0813262 -0.00843084 -429.318 0.0815548 0.996163 0.0317392 23.1787 0.00581726 -0.0323205 0.999461 2.82271\n0.996325 -0.0853882 0.00677807 -431.135 0.0851125 0.995796 0.033865 23.0169 -0.00964124 -0.0331637 0.999403 2.88436\n0.995904 -0.0900102 0.00859691 -432.975 0.0897154 0.995514 0.0300612 22.8579 -0.0112642 -0.0291668 0.999511 2.94985\n0.995586 -0.0937155 -0.00510429 -434.831 0.0938162 0.99526 0.0256326 22.6787 0.00267791 -0.0259984 0.999658 2.99321\n0.995238 -0.097298 -0.00587887 -436.663 0.0974137 0.994943 0.0244636 22.5028 0.00346889 -0.0249198 0.999683 3.03404\n0.994956 -0.100309 -0.00108933 -438.5 0.100305 0.994637 0.0252374 22.3198 -0.00144806 -0.0252194 0.999681 3.06418\n0.994679 -0.102939 -0.00419568 -440.319 0.103012 0.994368 0.0248983 22.1428 0.00160903 -0.0251981 0.999681 3.11381\n0.994409 -0.105593 0.000157029 -442.124 0.105559 0.994128 0.023816 21.9544 -0.00267091 -0.0236662 0.999716 3.16225\n0.994093 -0.108535 -0.000338913 -443.916 0.108508 0.993766 0.0255819 21.7696 -0.00243973 -0.0254676 0.999673 3.20585\n0.993875 -0.110413 -0.00456317 -445.673 0.11049 0.993602 0.0233714 21.5765 0.00195348 -0.0237324 0.999716 3.27665\n0.992395 -0.11601 0.0411455 -448.257 0.114522 0.992737 0.0368553 21.2941 -0.0451222 -0.031863 0.998473 3.41713\n0.99261 -0.120573 -0.0136654 -450.049 0.120923 0.99225 0.0285832 21.0971 0.0101131 -0.0300244 0.999498 3.44634\n0.990112 -0.124875 -0.0639073 -451.799 0.126371 0.991783 0.0199079 20.887 0.0608962 -0.027787 0.997757 3.44504\n0.991645 -0.128461 -0.0117825 -453.471 0.12873 0.991337 0.0260669 20.6467 0.00833183 -0.0273658 0.999591 3.45645\n0.990899 -0.134453 0.00641929 -456.077 0.134232 0.990573 0.0273379 20.3043 -0.0100344 -0.0262275 0.999606 3.50791\n0.989811 -0.142319 -0.00437342 -459.602 0.142386 0.9894 0.0285278 19.8084 0.00026701 -0.0288598 0.999583 3.58607\n0.98927 -0.146085 -0.00222192 -461.375 0.146087 0.988845 0.0290593 19.5606 -0.00204799 -0.0290721 0.999575 3.62717\n0.988412 -0.151688 -0.00576896 -464.065 0.15179 0.988042 0.0270587 19.1455 0.00159549 -0.0276208 0.999617 3.69084\n0.987766 -0.155908 -0.00324432 -465.834 0.155938 0.987387 0.0273766 18.8666 -0.00106482 -0.0275476 0.99962 3.72669\n0.987135 -0.159657 -0.00858527 -467.621 0.159827 0.986819 0.0253795 18.5774 0.00442008 -0.0264251 0.999641 3.7713\n0.986589 -0.163094 -0.00645985 -469.397 0.163205 0.986279 0.0248473 18.2891 0.00231877 -0.0255684 0.99967 3.81138\n0.986039 -0.166318 -0.00805635 -471.158 0.166468 0.98574 0.0246027 17.9963 0.0038496 -0.0256004 0.999665 3.85072\n0.985553 -0.169221 -0.00707996 -472.926 0.169346 0.985248 0.0246758 17.6912 0.00279985 -0.0255183 0.99967 3.89094\n0.985079 -0.171885 -0.00869477 -474.682 0.172054 0.984758 0.0254885 17.3814 0.00418114 -0.0266042 0.999637 3.9257\n0.984563 -0.174795 -0.00906326 -476.422 0.174959 0.984315 0.0226432 17.0623 0.00496319 -0.0238794 0.999703 3.98893\n0.983898 -0.178307 0.0122708 -478.123 0.177974 0.983735 0.0243134 16.7506 -0.0164064 -0.021738 0.999629 4.03022\n0.982738 -0.182042 0.0329749 -479.791 0.180856 0.982851 0.035977 16.4594 -0.0389588 -0.0293922 0.998808 4.11984\n0.982464 -0.184854 -0.0243567 -481.541 0.185568 0.982132 0.0313189 16.1472 0.0181321 -0.0352895 0.999213 4.1324\n0.979535 -0.18939 -0.0681311 -483.277 0.190925 0.981462 0.0167107 15.7975 0.0637032 -0.0293766 0.997536 4.13024\n0.98096 -0.19373 -0.0136271 -484.894 0.194059 0.980543 0.0296113 15.4741 0.00762539 -0.0316919 0.999469 4.14377\n0.980379 -0.197063 0.00487689 -486.579 0.196835 0.979984 0.0297906 15.1548 -0.0106499 -0.0282461 0.999544 4.17472\n0.979498 -0.200983 -0.0137607 -488.31 0.201283 0.979191 0.0258933 14.7942 0.00827023 -0.0281322 0.99957 4.21816\n0.978755 -0.204871 -0.00820272 -490.003 0.205019 0.978393 0.0267112 14.4321 0.00255315 -0.0278254 0.99961 4.25791\n0.977824 -0.209427 0.00107086 -491.701 0.209327 0.977491 0.0263322 14.0708 -0.00656144 -0.0255241 0.999653 4.30544\n0.974879 -0.222655 -0.00604345 -495.103 0.222735 0.974621 0.0224401 13.2944 0.000893677 -0.0232224 0.99973 4.39427\n0.973328 -0.229401 -0.00272615 -496.806 0.229404 0.973075 0.0223156 12.8913 -0.00246647 -0.0223458 0.999747 4.44009\n0.969205 -0.246114 -0.00829329 -500.224 0.246237 0.968987 0.0207723 12.0447 0.00292373 -0.0221747 0.99975 4.52981\n0.96676 -0.255512 -0.0094187 -501.904 0.255662 0.966511 0.0222146 11.6032 0.00342717 -0.0238842 0.999709 4.57119\n0.963955 -0.265973 -0.00703175 -503.556 0.266065 0.963688 0.0226769 11.139 0.000744957 -0.0237305 0.999718 4.60961\n0.95921 -0.282508 -0.0102351 -506.048 0.282675 0.958925 0.0236013 10.4254 0.00314717 -0.0255318 0.999669 4.67333\n0.955939 -0.293414 -0.00946255 -507.68 0.293561 0.95563 0.024376 9.91622 0.00189042 -0.0260798 0.999658 4.71077\n0.952647 -0.303968 -0.00826463 -509.314 0.30408 0.952282 0.0263563 9.39772 -0.000141215 -0.0276214 0.999618 4.75286\n0.949507 -0.313608 -0.00930872 -510.931 0.313746 0.94916 0.0256537 8.85944 0.000790251 -0.0272789 0.999628 4.79134\n0.946617 -0.322203 -0.0100493 -512.544 0.322358 0.946237 0.0268439 8.31804 0.000859843 -0.0286503 0.999589 4.83589\n0.944356 -0.328858 -0.00663312 -514.142 0.328909 0.943916 0.029028 7.76537 -0.003285 -0.0295945 0.999557 4.8793\n0.942806 -0.333219 -0.00907464 -515.721 0.333341 0.942372 0.0286035 7.21047 -0.000979546 -0.0299925 0.99955 4.925\n0.941958 -0.335409 -0.0146666 -517.297 0.335693 0.941601 0.0263996 6.65442 0.00495541 -0.0297908 0.999544 4.97711\n0.941821 -0.335857 -0.0131372 -518.869 0.33609 0.941505 0.0247313 6.08673 0.00406259 -0.0277078 0.999608 5.00792\n0.942538 -0.333953 -0.00988957 -520.435 0.334099 0.942175 0.02617 5.54426 0.000578158 -0.0279703 0.999609 5.04468\n0.943988 -0.329816 -0.0104089 -521.986 0.329979 0.943613 0.0266352 5.01323 0.00103724 -0.028578 0.999591 5.08712\n0.94578 -0.324572 -0.0123757 -523.543 0.324795 0.945399 0.0269823 4.49764 0.00294225 -0.0295389 0.999559 5.11956\n0.94776 -0.318783 -0.0113668 -525.076 0.31898 0.947344 0.0281344 3.98836 0.00179952 -0.0302904 0.99954 5.15911\n0.951459 -0.307593 -0.0106103 -528.087 0.307758 0.951211 0.0219689 2.99901 0.00333515 -0.0241679 0.999702 5.25076\n0.952909 -0.302905 -0.014616 -529.572 0.30315 0.952746 0.0193628 2.53749 0.00806024 -0.0228818 0.999706 5.28633\n0.954125 -0.299065 -0.0143188 -531.011 0.299305 0.953958 0.0195004 2.08484 0.00782762 -0.0228915 0.999707 5.31396\n0.955806 -0.29383 -0.00993439 -533.079 0.293985 0.955537 0.022839 1.45688 0.00278191 -0.0247502 0.99969 5.36649\n0.95684 -0.290609 -0.00185059 -534.428 0.290567 0.956547 0.0242687 1.04985 -0.00528253 -0.023759 0.999704 5.39801\n0.95778 -0.287244 0.0122047 -535.738 0.286834 0.957584 0.0275507 0.648458 -0.0196008 -0.0228867 0.999546 5.46932\n0.958258 -0.284165 0.0315035 -537.041 0.283078 0.958462 0.0348913 0.270572 -0.0401098 -0.0245169 0.998894 5.5459\n0.959856 -0.279144 -0.0274663 -539.015 0.2796 0.960008 0.0144229 -0.28191 0.0223418 -0.0215235 0.999519 5.6187\n0.959429 -0.276229 -0.0565094 -540.326 0.276978 0.960859 0.00573591 -0.657822 0.0527131 -0.0211551 0.998386 5.63046\n0.961897 -0.27275 -0.0190264 -541.558 0.273067 0.961849 0.0167369 -0.998085 0.0137356 -0.0212947 0.999679 5.69299\n0.963067 -0.269188 0.00622149 -542.79 0.268929 0.962771 0.0273741 -1.35734 -0.0133587 -0.02469 0.999606 5.66706\n0.963952 -0.266006 -0.00616291 -544.065 0.266077 0.963659 0.0237596 -1.70393 -0.000381262 -0.0245429 0.999699 5.68347\n0.964701 -0.26301 -0.0133041 -545.282 0.263255 0.964466 0.0223985 -2.02769 0.00694034 -0.0251102 0.999661 5.74368\n0.966478 -0.256615 -0.00828763 -547.026 0.256739 0.966215 0.0226437 -2.47288 0.00219692 -0.0240124 0.999709 5.79401\n0.968732 -0.247916 -0.00977822 -548.117 0.248072 0.968517 0.0208405 -2.74104 0.00430369 -0.0226145 0.999735 5.81236\n0.971672 -0.236142 -0.009549 -549.167 0.236303 0.971413 0.0227618 -2.98013 0.003901 -0.0243734 0.999695 5.84116\n0.975316 -0.220687 -0.00747156 -550.16 0.220801 0.975054 0.0227252 -3.19517 0.00227003 -0.023814 0.999714 5.86647\n0.981164 -0.193076 -0.00628305 -551.578 0.19317 0.980904 0.0226277 -3.45164 0.00179419 -0.0234152 0.999724 5.89804\n0.988444 -0.151502 -0.00498421 -553.424 0.151578 0.988161 0.0237128 -3.69923 0.00133266 -0.0241943 0.999706 5.94249\n0.994529 -0.104187 -0.00754093 -555.186 0.104344 0.994233 0.0247616 -3.84964 0.00491761 -0.0254129 0.999665 5.9845\n0.998181 -0.0594394 -0.0100792 -556.787 0.0596976 0.997834 0.027615 -3.90724 0.00841597 -0.0281664 0.999568 6.01534\n0.999826 -0.0145732 -0.0116123 -558.218 0.0148582 0.999581 0.0248507 -3.90955 0.0112452 -0.0250189 0.999624 6.03979\n0.999355 0.0349066 -0.00842592 -559.508 -0.0346388 0.998947 0.0300726 -3.80246 0.00946678 -0.0297614 0.999512 6.07013\n0.996303 0.085879 -0.00244401 -560.717 -0.085764 0.995842 0.0307022 -3.66167 0.00507051 -0.030379 0.999526 6.07076\n0.988878 0.148718 0.00181273 -561.857 -0.148636 0.987757 0.0473644 -3.38372 0.00525342 -0.0471071 0.998876 6.09236\n0.969069 0.24676 -0.00392202 -563.258 -0.246305 0.968036 0.047329 -2.95759 0.0154755 -0.044899 0.998872 6.12006\n0.940174 0.340437 0.0132202 -564.321 -0.340595 0.938259 0.0605377 -2.42893 0.00820529 -0.0614188 0.998078 6.1302\n0.879667 0.475469 0.0107529 -565.639 -0.475258 0.877982 0.0572505 -1.61399 0.01778 -0.0554718 0.998302 6.102\n0.812929 0.582292 0.00906664 -566.638 -0.581728 0.811218 0.0593141 -0.740854 0.0271831 -0.0534925 0.998198 6.13356\n0.696513 0.717524 -0.00536298 -567.767 -0.716274 0.695705 0.05428 0.524582 0.0426782 -0.0339653 0.998511 6.08914\n0.571536 0.820576 -0.000786947 -568.458 -0.819283 0.570689 0.0555814 1.77328 0.0460579 -0.0311221 0.998454 6.05149\n0.424813 0.905281 5.01566e-05 -568.976 -0.904102 0.424256 0.0510532 3.16704 0.0461962 -0.0217334 0.998696 6.01611\n0.269647 0.962872 0.0129302 -569.286 -0.96215 0.268846 0.0446092 4.75404 0.0394768 -0.0244695 0.998921 5.95282\n0.143341 0.989466 0.0202432 -569.498 -0.989429 0.142821 0.0251418 6.48124 0.0219858 -0.023633 0.999479 5.95356\n0.0734165 0.996657 0.0358361 -569.562 -0.997295 0.0732445 0.00609223 8.3121 0.00344707 -0.0361865 0.999339 5.95614\n0.0383811 0.998633 0.0354818 -569.605 -0.998242 0.036713 0.0465265 10.2645 0.0451602 -0.0372052 0.998287 5.90551\n0.0275045 0.998947 0.0367125 -569.611 -0.997695 0.025154 0.0630191 11.3082 0.0620293 -0.0383612 0.997337 5.92063\n0.0131826 0.999493 0.0289836 -569.649 -0.998488 0.0116112 0.0537315 12.3201 0.0533677 -0.029648 0.998135 5.91874\n0.00442835 0.999352 0.0357257 -569.639 -0.999522 0.00332991 0.0307475 13.3478 0.0306087 -0.0358447 0.998889 5.86962\n-0.00507288 0.999561 0.0291893 -569.622 -0.999621 -0.00585876 0.0269015 15.0071 0.0270607 -0.0290417 0.999212 5.87069\n-0.0112672 0.999395 0.0329135 -569.575 -0.999377 -0.0123559 0.0330644 16.7647 0.0334511 -0.0325205 0.998911 5.829\n-0.0130685 0.999444 0.0306788 -569.546 -0.999462 -0.0139799 0.0296822 17.9389 0.0300946 -0.0302744 0.999088 5.847\n-0.0150097 0.999543 0.0262322 -569.543 -0.999436 -0.0157862 0.0296489 19.1395 0.0300494 -0.0257724 0.999216 5.816\n-0.0155036 0.99945 0.029319 -569.494 -0.999338 -0.016454 0.032457 20.3939 0.0329216 -0.0287964 0.999043 5.8019\n-0.0163924 0.999461 0.0284603 -569.474 -0.999337 -0.0173022 0.0320243 21.6589 0.0324994 -0.0279164 0.999082 5.79242\n-0.0165739 0.99945 0.0287098 -569.451 -0.9994 -0.0174324 0.0299161 22.9415 0.0304001 -0.0281968 0.99914 5.77126\n-0.0173677 0.999151 0.0373505 -569.376 -0.999352 -0.0185254 0.030877 24.9261 0.0315427 -0.03679 0.998825 5.7423\n-0.0172849 0.999081 0.0392146 -569.35 -0.999273 -0.0185951 0.0332962 26.2766 0.0339948 -0.0386105 0.998676 5.73659\n-0.0154524 0.998949 0.0431447 -569.296 -0.999332 -0.0168592 0.0324366 27.6692 0.0331299 -0.0426146 0.998542 5.71271\n-0.0137022 0.998867 0.0455762 -569.264 -0.999379 -0.0151604 0.0318049 29.0523 0.0324598 -0.0451121 0.998454 5.71134\n-0.0119 0.998944 0.0443825 -569.253 -0.999284 -0.0134748 0.0353534 30.4545 0.0359141 -0.04393 0.998389 5.69338\n-0.0104103 0.999004 0.0433824 -569.257 -0.999356 -0.0118839 0.0338505 31.9063 0.0343324 -0.0430021 0.998485 5.67284\n-0.00759076 0.99892 0.0458371 -569.194 -0.999613 -0.00880681 0.0263864 34.1239 0.0267616 -0.0456191 0.9986 5.65561\n-0.0054819 0.998866 0.0472889 -569.181 -0.999636 -0.00672303 0.0261268 35.6472 0.0264151 -0.0471284 0.99854 5.63976\n-0.00346554 0.998744 0.0499922 -569.168 -0.999672 -0.00472852 0.0251675 37.1722 0.0253722 -0.0498885 0.998432 5.64113\n-0.0019633 0.998609 0.0526854 -569.155 -0.999701 -0.00324384 0.0242309 38.7341 0.0243681 -0.0526221 0.998317 5.63058\n-0.000292589 0.998428 0.0560511 -569.148 -0.999697 -0.00167107 0.0245481 40.2928 0.0246032 -0.0560269 0.998126 5.63329\n0.00158768 0.998306 0.0581585 -569.14 -0.999713 0.000195528 0.0239351 41.8683 0.0238832 -0.0581798 0.99802 5.62159\n0.00260627 0.998092 0.0616958 -569.132 -0.999679 0.00104673 0.0252968 43.4888 0.025184 -0.0617419 0.997774 5.61547\n0.00326153 0.997164 0.0751908 -569.111 -0.999544 0.000992991 0.0301881 46.7383 0.0300278 -0.075255 0.996712 5.59683\n0.00348231 0.99696 0.0778389 -569.098 -0.999566 0.00119291 0.0294393 48.3911 0.0292569 -0.0779076 0.996531 5.58098\n0.00209783 0.997004 0.077327 -569.076 -0.999493 -0.000366498 0.031841 50.0497 0.0317739 -0.0773546 0.996497 5.58672\n-0.00089846 0.997337 0.0729316 -569.088 -0.999476 -0.00325552 0.0322064 51.6948 0.0323581 -0.0728644 0.996817 5.5893\n-0.00380682 0.997767 0.0666854 -569.071 -0.999622 -0.00561332 0.0269236 53.3529 0.0272378 -0.0665577 0.997411 5.57613\n-0.00745664 0.99823 0.0590021 -569.054 -0.999258 -0.00966744 0.0372736 55.8051 0.037778 -0.0586804 0.997562 5.55598\n-0.007826 0.998243 0.0587337 -569.026 -0.999276 -0.00999377 0.0367059 57.4266 0.0372284 -0.0584039 0.997599 5.53269\n-0.00681389 0.998344 0.0571294 -569.018 -0.999254 -0.00896961 0.0375629 59.0347 0.0380131 -0.0568308 0.99766 5.50842\n-0.00518442 0.998388 0.0565252 -568.994 -0.999275 -0.00730457 0.0373662 60.6246 0.0377188 -0.0562904 0.997702 5.501\n-0.00324003 0.998558 0.053577 -568.994 -0.999365 -0.00513501 0.0352694 62.2074 0.0354937 -0.0534287 0.997941 5.47743\n-0.00089828 0.998469 0.0553053 -568.984 -0.999252 -0.00303411 0.0385471 63.7641 0.0386559 -0.0552293 0.997725 5.44884\n0.00161926 0.998177 0.0603347 -568.969 -0.999351 -0.000555537 0.0360113 66.0904 0.0359792 -0.0603538 0.997528 5.43231\n0.00292971 0.998047 0.0624028 -568.964 -0.999494 0.000945507 0.0318026 67.6211 0.0316814 -0.0624644 0.997544 5.42242\n0.00356552 0.99805 0.0623226 -568.958 -0.999379 0.00137194 0.0352046 69.1482 0.0350504 -0.0624094 0.997435 5.40557\n0.00435266 0.998255 0.0588956 -568.947 -0.99896 0.00166762 0.0455623 70.6499 0.0453845 -0.0590326 0.997224 5.40939\n0.00185 0.998737 0.0502105 -568.966 -0.999492 0.000248832 0.0318765 72.0934 0.0318238 -0.050244 0.99823 5.43145\n0.000515486 0.998803 0.0489127 -568.96 -0.999787 -0.000493443 0.0206128 73.4934 0.0206122 -0.048913 0.99859 5.38817\n-0.000201833 0.999195 0.0401145 -568.968 -0.999359 -0.00163703 0.0357479 75.5555 0.0357848 -0.0400816 0.998555 5.40746\n1.77472e-05 0.999092 0.0426153 -568.967 -0.999611 -0.00117098 0.0278693 76.9078 0.0278939 -0.0425992 0.998703 5.37408\n0.000385034 0.999079 0.0429067 -568.959 -0.999664 -0.000727258 0.0259049 78.2267 0.0259122 -0.0429023 0.998743 5.38837\n0.000378826 0.998849 0.0479599 -568.959 -0.999999 0.000303688 0.00157396 80.8335 0.00155758 -0.0479604 0.998848 5.40796\n0.00117778 0.998679 0.0513735 -568.926 -0.999991 0.000973072 0.00400953 82.1418 0.00395424 -0.0513778 0.998671 5.40662\n0.00293682 0.998745 0.0500046 -568.932 -0.998056 -0.000185528 0.0623223 83.4952 0.0622534 -0.0500904 0.996803 5.34325\n0.00337646 0.998433 0.0558608 -568.926 -0.997686 -0.000429961 0.0679893 84.7875 0.0679068 -0.0559611 0.996121 5.37507\n0.000200244 0.998398 0.0565769 -568.932 -0.999836 -0.000824909 0.0180957 86.0632 0.0181134 -0.0565712 0.998234 5.37121\n0.000288669 0.998431 0.0559968 -568.912 -0.999536 -0.00141707 0.0304193 88.0684 0.0304509 -0.0559796 0.997967 5.30607\n0.000113675 0.998542 0.053972 -568.908 -0.999298 -0.00190896 0.0374225 89.3952 0.037471 -0.0539383 0.997841 5.33214\n-0.000979368 0.998492 0.0548877 -568.891 -0.999707 -0.00230455 0.0240855 91.3998 0.0241756 -0.0548481 0.998202 5.30763\n-0.00133965 0.998629 0.0523302 -568.894 -0.999555 -0.0028974 0.0297033 92.7686 0.0298142 -0.0522671 0.998188 5.30756\n-0.00161237 0.998751 0.0499452 -568.899 -0.999586 -0.00304371 0.0285955 94.1252 0.0287118 -0.0498785 0.998343 5.30541\n-0.00225049 0.998905 0.0467247 -568.895 -0.999668 -0.00344708 0.0255446 95.4861 0.0256777 -0.0466517 0.998581 5.30251\n-0.00206053 0.998866 0.0475575 -568.88 -0.99957 -0.00344858 0.0291232 97.564 0.0292542 -0.047477 0.998444 5.27806\n-0.0030191 0.998831 0.0482462 -568.879 -0.999457 -0.00459661 0.0326196 98.9479 0.0328033 -0.0481215 0.998303 5.27478\n-0.0028563 0.998656 0.0517403 -568.853 -0.999483 -0.00450798 0.031834 100.322 0.0320245 -0.0516226 0.998153 5.25316\n-0.00365854 0.998626 0.0522766 -568.838 -0.999459 -0.00536039 0.0324515 102.351 0.0326872 -0.0521296 0.998105 5.25437\n-0.00376766 0.998703 0.050773 -568.825 -0.999461 -0.00541693 0.0323848 104.375 0.0326178 -0.0506236 0.998185 5.2492\n-0.0034168 0.998927 0.0461787 -568.823 -0.999458 -0.00492262 0.0325343 105.707 0.0327267 -0.0460425 0.998403 5.23858\n-0.00308713 0.998987 0.044895 -568.818 -0.999478 -0.00452603 0.031984 107.025 0.0321548 -0.0447728 0.99848 5.2264\n-0.00408296 0.998824 0.0483008 -568.801 -0.999949 -0.00452632 0.0090733 108.997 0.00928126 -0.0482613 0.998792 5.22279\n-0.00511024 0.998487 0.054743 -568.783 -0.999945 -0.00460346 -0.00937952 110.29 -0.00911333 -0.0547879 0.998456 5.28095\n-0.00249643 0.999154 0.0410517 -568.774 -0.997063 -0.0056291 0.076373 113.028 0.0765395 -0.0407405 0.996234 5.20664\n-0.00647065 0.999421 0.0333996 -568.775 -0.999931 -0.00679302 0.00954738 114.863 0.00976874 -0.0333355 0.999396 5.26544\n-0.00559053 0.999444 0.0328778 -568.769 -0.999909 -0.00598964 0.0120531 116.184 0.0122433 -0.0328075 0.999387 5.19728\n-0.00359774 0.999199 0.0398517 -568.741 -0.999457 -0.00489795 0.0325767 118.074 0.0327458 -0.0397129 0.998674 5.22232\n-0.00386183 0.999007 0.0443848 -568.726 -0.999674 -0.00497727 0.0250482 119.274 0.0252442 -0.0442736 0.9987 5.21359\n-0.0017617 0.998437 0.0558683 -568.702 -0.999466 -0.00358166 0.0324926 120.989 0.0326419 -0.0557812 0.997909 5.2025\n-0.000684427 0.998013 0.063011 -568.696 -0.999416 -0.00283627 0.0340671 122.052 0.0341781 -0.0629508 0.997431 5.21105\n-0.000457589 0.997838 0.0657153 -568.695 -0.999566 -0.00239232 0.0293655 123.068 0.0294592 -0.0656733 0.997406 5.21016\n0.000495344 0.997486 0.0708679 -568.682 -0.999646 -0.00139083 0.0265635 124.598 0.0265953 -0.0708559 0.997132 5.21064\n0.00139428 0.997544 0.0700329 -568.673 -0.999574 -0.000652393 0.029193 125.603 0.029167 -0.0700438 0.997117 5.21697\n0.0019292 0.997781 0.0665596 -568.678 -0.999738 0.000405893 0.0228922 127.08 0.0228144 -0.0665863 0.99752 5.21194\n0.00233649 0.997776 0.0666226 -568.673 -0.999706 0.000723878 0.0242191 129.035 0.024117 -0.0666596 0.997484 5.22313\n0.00504865 0.997682 0.0678578 -568.677 -0.998436 0.00125042 0.0558999 130.547 0.0556854 -0.0680338 0.996128 5.16919\n0.00340865 0.997224 0.0743837 -568.659 -0.99961 0.00133737 0.0278778 132.505 0.027701 -0.0744497 0.99684 5.15495\n0.00403008 0.996602 0.0822707 -568.642 -0.999165 0.000667281 0.0408614 134.051 0.0406677 -0.0823666 0.995772 5.12668\n0.00282766 0.996057 0.0886677 -568.623 -0.999329 -0.000422644 0.036617 135.092 0.0365101 -0.0887117 0.995388 5.12527\n-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\n-0.00204031 0.995192 0.0979174 -568.59 -0.999525 -0.0050397 0.0303943 137.749 0.0307416 -0.0978089 0.99473 5.09217\n-0.00820571 0.99446 0.104793 -568.555 -0.9996 -0.0109925 0.0260431 139.892 0.0270508 -0.104537 0.994153 5.10057\n-0.0126105 0.994577 0.103232 -568.536 -0.999739 -0.0145099 0.0176692 140.961 0.0190713 -0.102982 0.9945 5.09987\n-0.0192799 0.993769 0.109775 -568.466 -0.99946 -0.0220771 0.0243235 142.571 0.0265954 -0.109247 0.993659 5.08831\n-0.0291426 0.993928 0.106104 -568.423 -0.999005 -0.0325474 0.0305003 143.646 0.0337685 -0.105109 0.993887 5.09699\n-0.05185 0.992863 0.107398 -568.317 -0.998289 -0.0544414 0.0213378 145.256 0.0270324 -0.106108 0.993987 5.07604\n-0.0736237 0.99152 0.107086 -568.212 -0.996916 -0.0760968 0.0191885 146.324 0.0271747 -0.105343 0.994065 5.07943\n-0.12613 0.987219 0.0974128 -568.012 -0.991659 -0.128103 0.0142476 147.929 0.0265444 -0.0948032 0.995142 5.07693\n-0.194662 0.976028 0.0973474 -567.664 -0.980379 -0.196746 0.0121959 149.547 0.0310562 -0.0930632 0.995176 5.04652\n-0.2489 0.964375 0.0896083 -567.378 -0.968076 -0.250545 0.00742039 150.61 0.029607 -0.0849008 0.995949 5.06917\n-0.331503 0.940386 0.076028 -566.774 -0.943111 -0.332477 0.00015911 152.176 0.0254272 -0.0716502 0.997106 5.02501\n-0.387388 0.918783 0.0759449 -566.288 -0.921383 -0.38865 0.00199476 153.204 0.0313487 -0.0692016 0.99711 5.00049\n-0.475744 0.876344 0.075428 -565.417 -0.878864 -0.477072 -0.000474071 154.663 0.0355691 -0.0665165 0.997151 5.00263\n-0.529922 0.844439 0.0781362 -564.746 -0.847137 -0.531368 -0.0026577 155.631 0.0392748 -0.0676004 0.996939 4.92828\n-0.602148 0.792975 0.0927811 -563.614 -0.796899 -0.604042 -0.00927486 157.005 0.0486889 -0.0795219 0.995643 4.89269\n-0.644371 0.760068 0.0841516 -562.805 -0.763445 -0.64573 -0.0135826 157.896 0.0440155 -0.0729973 0.99636 4.89543\n-0.695984 0.713428 0.0814037 -561.476 -0.716627 -0.697272 -0.0160652 159.2 0.0452992 -0.0695172 0.996552 4.80124\n-0.723118 0.685185 0.0873056 -560.523 -0.688575 -0.725051 -0.0129079 160.036 0.0544567 -0.0694503 0.996098 4.79595\n-0.749991 0.656619 0.0797801 -559.555 -0.659519 -0.75155 -0.0144274 160.862 0.0504854 -0.0634368 0.996708 4.74442\n-0.788095 0.610527 0.0784975 -557.994 -0.613544 -0.789404 -0.0201074 162.028 0.0496902 -0.0640082 0.996712 4.67054\n-0.811023 0.57956 0.0797026 -556.868 -0.582726 -0.812357 -0.0225059 162.762 0.0517034 -0.0646975 0.996565 4.6443\n-0.850051 0.521339 0.0749613 -554.549 -0.523834 -0.851647 -0.0171818 164.177 0.054883 -0.0538726 0.997038 4.55549\n-0.876568 0.47542 0.0748588 -552.723 -0.477595 -0.878479 -0.0133394 165.142 0.05942 -0.0474452 0.997105 4.48191\n-0.89234 0.444002 0.0811853 -551.469 -0.446294 -0.89481 -0.0116886 165.732 0.0674557 -0.0466627 0.99663 4.4167\n-0.914617 0.395481 0.084088 -549.56 -0.397947 -0.917298 -0.0142145 166.529 0.0715121 -0.0464635 0.996357 4.3258\n-0.927134 0.366323 0.0789271 -548.284 -0.368768 -0.929338 -0.0184925 166.999 0.0665757 -0.0462508 0.996709 4.26091\n-0.937044 0.340068 0.0793859 -546.971 -0.342664 -0.939216 -0.0213281 167.439 0.0673075 -0.047188 0.996616 4.1906\n-0.948359 0.307052 0.079595 -545.028 -0.309422 -0.950733 -0.0190853 168.047 0.0698134 -0.0427281 0.996645 4.1287\n-0.954098 0.289541 0.0765705 -543.728 -0.291565 -0.956409 -0.0164771 168.428 0.0684619 -0.0380461 0.996928 4.06283\n-0.958367 0.275183 0.0761989 -542.422 -0.276936 -0.960797 -0.0132738 168.788 0.0695589 -0.0338234 0.997004 4.0116\n-0.963573 0.259731 0.063773 -540.46 -0.260737 -0.965378 -0.00783934 169.323 0.059529 -0.0241817 0.997934 3.91189\n-0.965463 0.255228 0.0523479 -538.507 -0.256257 -0.966509 -0.0138914 169.788 0.0470492 -0.0268262 0.998532 3.90215\n-0.961357 0.255993 0.101293 -536.46 -0.256239 -0.966553 0.0107909 170.357 0.100667 -0.0155814 0.994798 3.73675\n-0.965128 0.257609 0.0465321 -534.529 -0.258128 -0.966095 -0.00541943 170.874 0.0435584 -0.0172417 0.998902 3.68315\n-0.963283 0.25966 0.0682742 -532.414 -0.259964 -0.965608 0.00456069 171.436 0.0671104 -0.0133556 0.997656 3.57448\n-0.963269 0.261064 0.062919 -531.014 -0.261703 -0.965146 -0.00198506 171.805 0.0602078 -0.0183782 0.998017 3.52249\n-0.962972 0.263251 0.0581769 -528.857 -0.263874 -0.964552 -0.00315031 172.383 0.0552853 -0.018385 0.998301 3.44836\n-0.962191 0.264906 0.0633527 -526.651 -0.265577 -0.964087 -0.00227039 172.99 0.060476 -0.0190096 0.997989 3.37015\n-0.961631 0.267214 0.0621521 -524.391 -0.26782 -0.963468 -0.00148513 173.617 0.0594847 -0.0180737 0.998066 3.27652\n-0.96139 0.268626 0.05975 -522.856 -0.26919 -0.963086 -0.00144959 174.043 0.057155 -0.0174777 0.998212 3.22228\n-0.96092 0.270718 0.0578364 -520.529 -0.271229 -0.962514 -0.00101745 174.69 0.055393 -0.0166646 0.998326 3.15142\n-0.959651 0.274392 0.0614772 -516.51 -0.274825 -0.961493 0.00146439 175.835 0.0595118 -0.0154902 0.998107 2.99991\n-0.959074 0.275715 0.0644822 -514.883 -0.276262 -0.961082 0.000457178 176.298 0.0620988 -0.0173755 0.997919 2.9323\n-0.958426 0.277462 0.0665895 -512.402 -0.278271 -0.960498 -0.00300074 177.003 0.0631265 -0.0214059 0.997776 2.83243\n-0.957861 0.279986 0.0641017 -509.898 -0.280711 -0.959789 -0.00240904 177.736 0.0608496 -0.0203016 0.99794 2.73515\n-0.957343 0.281938 0.0632829 -507.338 -0.282541 -0.959255 -0.000607 178.495 0.0605333 -0.0184611 0.997995 2.63483\n-0.956638 0.283777 0.0656855 -504.745 -0.284556 -0.958656 -0.0026217 179.253 0.0622258 -0.0211992 0.997837 2.53854\n-0.95628 0.285253 0.0644908 -502.994 -0.286035 -0.958214 -0.00304459 179.769 0.0609276 -0.0213581 0.997914 2.46154\n-0.95562 0.287647 0.0636403 -500.363 -0.288342 -0.957526 -0.00183041 180.551 0.0604107 -0.0200993 0.997971 2.36789\n-0.954981 0.289416 0.0651885 -497.7 -0.290313 -0.956921 -0.00453403 181.346 0.061068 -0.023255 0.997863 2.25772\n-0.954369 0.291393 0.0653494 -495.911 -0.292407 -0.956273 -0.00631282 181.875 0.0606524 -0.0251334 0.997842 2.18877\n-0.951178 0.302747 0.0600305 -492.356 -0.303731 -0.952726 -0.00779334 182.993 0.0548332 -0.025646 0.998166 2.05038\n-0.948949 0.309776 0.0594481 -490.607 -0.310884 -0.950393 -0.010168 183.563 0.0533493 -0.0281304 0.99818 1.99585\n-0.945215 0.320083 0.0641528 -487.985 -0.321719 -0.946687 -0.0167599 184.431 0.0553681 -0.0364809 0.997799 1.92807\n-0.943039 0.327561 0.0581513 -486.249 -0.329098 -0.944106 -0.0189216 185.029 0.048703 -0.0369813 0.998128 1.88063\n-0.94045 0.335275 0.0560771 -484.513 -0.336617 -0.941502 -0.0162123 185.647 0.0473612 -0.0341233 0.998295 1.83593\n-0.93741 0.343124 0.0593962 -482.783 -0.344352 -0.93877 -0.0115251 186.28 0.0518048 -0.031257 0.998168 1.78613\n-0.934726 0.350328 0.0596484 -481.069 -0.351393 -0.936193 -0.00806233 186.922 0.053018 -0.0284961 0.998187 1.74227\n-0.931062 0.360843 0.0539988 -478.524 -0.361686 -0.932278 -0.00640429 187.909 0.048031 -0.0254934 0.99852 1.66314\n-0.928257 0.367621 0.0565134 -476.847 -0.368479 -0.929621 -0.00521556 188.579 0.0506188 -0.0256654 0.998388 1.62623\n-0.925145 0.375503 0.0557113 -475.146 -0.376404 -0.926434 -0.0062688 189.275 0.0492589 -0.0267695 0.998427 1.58512\n-0.916992 0.395218 0.054111 -471.794 -0.396042 -0.918219 -0.00499801 190.706 0.0477104 -0.0260133 0.998522 1.49881\n-0.912008 0.40668 0.053406 -470.128 -0.407516 -0.913182 -0.00534296 191.453 0.0465965 -0.0266366 0.998559 1.46498\n-0.906822 0.417956 0.0546493 -468.461 -0.418702 -0.90812 -0.00245274 192.222 0.048603 -0.025106 0.998503 1.41446\n-0.901719 0.429391 0.0502622 -466.836 -0.430023 -0.902816 -0.00195448 193.008 0.0445383 -0.0233763 0.998734 1.37288\n-0.892799 0.447325 0.0530093 -464.402 -0.447921 -0.894073 0.00070921 194.222 0.0477114 -0.0231108 0.998594 1.32955\n-0.888079 0.45695 0.0501267 -462.794 -0.457394 -0.889259 0.00290321 195.045 0.0459022 -0.0203494 0.998739 1.28266\n-0.884261 0.464636 0.0468547 -461.215 -0.465216 -0.885195 -0.00167548 195.88 0.0406971 -0.0232791 0.9989 1.26428\n-0.880609 0.471608 0.0459733 -459.62 -0.472148 -0.881518 -0.00102655 196.735 0.0400422 -0.0226102 0.998942 1.23628\n-0.875198 0.481355 0.0482232 -457.251 -0.481868 -0.876243 0.00110981 198.028 0.0427894 -0.0222659 0.998836 1.18487\n-0.872446 0.486573 0.045648 -455.699 -0.487061 -0.873368 0.000510608 198.902 0.0401159 -0.0217878 0.998957 1.17249\n-0.870305 0.490619 0.0431533 -454.139 -0.491011 -0.871152 0.00173759 199.783 0.0384456 -0.0196765 0.999067 1.13966\n-0.868731 0.493141 0.046023 -452.59 -0.493447 -0.86976 0.00524831 200.665 0.0426172 -0.0181506 0.998927 1.11213\n-0.867309 0.495661 0.0457713 -450.266 -0.496164 -0.868228 0.000428065 201.975 0.0399521 -0.0223388 0.998952 1.05284\n-0.866761 0.49661 0.0458728 -448.726 -0.497283 -0.86758 -0.00385072 202.85 0.037886 -0.0261494 0.99894 1.03253\n-0.865772 0.498365 0.0455062 -447.194 -0.499201 -0.866444 -0.00855034 203.722 0.0351674 -0.0301194 0.998927 1.00773\n-0.864404 0.500516 0.047846 -444.897 -0.501502 -0.865091 -0.0106391 205.037 0.036066 -0.0331914 0.998798 0.966549\n-0.863159 0.502783 0.0465358 -443.393 -0.503761 -0.863765 -0.0116126 205.912 0.0343574 -0.0334665 0.998849 0.953271\n-0.86143 0.506027 0.0433064 -441.912 -0.506916 -0.861911 -0.0120699 206.775 0.0312186 -0.0323501 0.998989 0.942819\n-0.858821 0.510576 0.0416881 -439.711 -0.511368 -0.859298 -0.0104764 208.08 0.0304735 -0.0303154 0.999076 0.930636\n-0.856875 0.513952 0.0402324 -438.25 -0.514608 -0.857394 -0.0073406 208.962 0.0307223 -0.0269939 0.999163 0.919741\n-0.85271 0.521024 0.0376838 -435.377 -0.52128 -0.853379 0.00346136 210.729 0.033962 -0.0166923 0.999284 0.898164\n-0.85122 0.523365 0.038897 -433.939 -0.523564 -0.851967 0.00569846 211.612 0.0361214 -0.0155144 0.999227 0.87303\n-0.849552 0.526109 0.0383566 -432.522 -0.526297 -0.850281 0.00584306 212.489 0.035688 -0.015223 0.999247 0.860841\n-0.846305 0.531506 0.0356375 -430.413 -0.531619 -0.846955 0.00699932 213.809 0.0339035 -0.013022 0.99934 0.829743\n-0.843977 0.535222 0.0352106 -429.018 -0.535305 -0.844623 0.00782516 214.687 0.0339278 -0.0122441 0.999349 0.807904\n-0.841058 0.540011 0.03178 -427.661 -0.540117 -0.841569 0.00588676 215.563 0.029924 -0.0122138 0.999478 0.806611\n-0.837812 0.545074 0.0310703 -426.301 -0.545258 -0.838263 0.00293856 216.445 0.0276469 -0.0144794 0.999513 0.794111\n-0.834158 0.550325 0.0363595 -424.962 -0.550589 -0.83477 0.00320157 217.324 0.0321138 -0.0173486 0.999334 0.793553\n-0.83099 0.555124 0.0359536 -423.628 -0.55536 -0.8316 0.00394138 218.216 0.032087 -0.016692 0.999346 0.789943\n-0.827235 0.560693 0.0361415 -422.318 -0.560899 -0.827868 0.0051125 219.109 0.0327869 -0.0160425 0.999334 0.777629\n-0.821275 0.569129 0.039992 -420.362 -0.569383 -0.822052 0.00585555 220.472 0.0362081 -0.0179617 0.999183 0.756101\n-0.814368 0.579148 0.0373101 -418.454 -0.579382 -0.815039 0.0052914 221.824 0.0334737 -0.0173077 0.99929 0.740543\n-0.809449 0.585886 0.0391266 -417.201 -0.586096 -0.810211 0.0070573 222.733 0.0358356 -0.0172195 0.999209 0.73812\n-0.805016 0.592035 0.0379823 -415.976 -0.592229 -0.805738 0.00712863 223.659 0.0348242 -0.0167556 0.999253 0.730489\n-0.800062 0.598952 0.034013 -414.746 -0.598961 -0.800702 0.0110544 224.596 0.0338553 -0.0115282 0.99936 0.71728\n-0.79366 0.607541 0.0315918 -413.538 -0.607545 -0.794215 0.010581 225.541 0.0315191 -0.0107957 0.999445 0.7232\n-0.78606 0.617544 0.0273645 -412.361 -0.617226 -0.786535 0.0198455 226.507 0.0337786 -0.00129037 0.999429 0.712636\n-0.778436 0.627111 0.0277156 -411.19 -0.626652 -0.778929 0.0240296 227.462 0.0366577 0.00133745 0.999327 0.678583\n-0.770276 0.636749 0.0350175 -410.044 -0.636743 -0.77097 0.012756 228.404 0.0351198 -0.0124715 0.999305 0.685107\n-0.752661 0.657331 0.0376562 -407.842 -0.657716 -0.753261 0.00276608 230.304 0.0301832 -0.0226852 0.999287 0.664401\n-0.745058 0.665845 0.0392377 -406.816 -0.666037 -0.745853 0.0098428 231.241 0.0358194 -0.0188003 0.999181 0.695284\n-0.736276 0.675484 0.0402435 -405.828 -0.675563 -0.737176 0.0136646 232.166 0.0388968 -0.0171261 0.999096 0.670492\n-0.727988 0.684052 0.0459016 -404.88 -0.684142 -0.729169 0.0161728 233.063 0.0445331 -0.0196296 0.998815 0.669855\n-0.720201 0.69243 0.043038 -403.984 -0.692145 -0.721372 0.0236059 233.96 0.0473919 -0.0127875 0.998795 0.649484\n-0.712263 0.700504 0.0444495 -403.14 -0.700378 -0.713464 0.0209522 234.799 0.0463902 -0.016208 0.998792 0.637457\n-0.704864 0.708138 0.0413118 -402.325 -0.708096 -0.705882 0.0181834 235.641 0.0420376 -0.0164359 0.998981 0.625994\n-0.696592 0.716209 0.0424906 -401.505 -0.716276 -0.697626 0.0163406 236.477 0.0413458 -0.0190522 0.998963 0.586427\n-0.690437 0.722434 0.0372232 -400.729 -0.722366 -0.691284 0.0177001 237.305 0.038519 -0.014668 0.99915 0.63733\n-0.692174 0.721644 0.0112111 -399.976 -0.721708 -0.692192 -0.00276893 238.089 0.00576203 -0.0100077 0.999933 0.653977\n-0.704129 0.709932 0.0140943 -399.172 -0.710059 -0.704098 -0.00785924 238.876 0.00434422 -0.0155417 0.99987 0.625004\n-0.727019 0.685197 0.0441335 -398.328 -0.684284 -0.728345 0.0356266 239.669 0.0565556 -0.00429864 0.99839 0.575475\n-0.757745 0.647862 0.0780838 -397.471 -0.643852 -0.761745 0.0720992 240.337 0.10619 0.00435832 0.994336 0.523091\n-0.796781 0.599319 0.0771839 -396.609 -0.596212 -0.800506 0.0609994 240.885 0.0983443 0.00258513 0.995149 0.506499\n-0.840474 0.539543 0.0499768 -395.727 -0.53741 -0.841816 0.0503616 241.39 0.0692435 0.0154695 0.99748 0.476578\n-0.88247 0.468291 0.0441684 -394.787 -0.466473 -0.883354 0.045695 241.802 0.0604149 0.019721 0.997979 0.3715\n-0.938824 0.339526 0.0577298 -393.325 -0.338489 -0.940578 0.027175 242.198 0.063526 0.00597158 0.997962 0.356905\n-0.96827 0.244325 0.052513 -392.315 -0.243697 -0.969682 0.0181394 242.343 0.0553528 0.00476653 0.998455 0.319958\n-0.98806 0.146644 0.0472512 -391.313 -0.146423 -0.989189 0.00812469 242.387 0.0479318 0.00110902 0.99885 0.29062\n-0.997949 0.0485789 0.0416907 -390.307 -0.0486129 -0.998818 0.000198439 242.331 0.0416511 -0.00182867 0.999131 0.291093\n-0.989393 -0.143742 0.0209742 -388.345 0.14411 -0.989411 0.0172382 241.988 0.0182743 0.020078 0.999631 0.284538\n-0.974528 -0.224258 -0.00153265 -387.404 0.224257 -0.97453 0.000943962 241.647 -0.00170531 0.00057621 0.999998 0.250162\n-0.938595 -0.34188 0.0464585 -385.449 0.341264 -0.939736 -0.0208476 240.867 0.0507861 -0.00371289 0.998703 0.224488\n-0.919577 -0.3845 0.0808595 -384.458 0.382372 -0.9231 -0.0409529 240.398 0.0903878 -0.00674084 0.995884 0.193491\n-0.908708 -0.413945 0.0538437 -383.531 0.412368 -0.910216 -0.0382125 239.938 0.0648272 -0.0125206 0.997818 0.218665\n-0.901048 -0.433293 0.0192362 -382.628 0.432633 -0.901044 -0.030791 239.464 0.0306742 -0.0194219 0.999341 0.212232\n-0.893555 -0.448712 0.0147568 -381.668 0.448449 -0.893625 -0.0180937 238.971 0.021306 -0.00955001 0.999727 0.157048\n-0.886677 -0.461221 0.0328528 -380.658 0.460403 -0.887215 -0.0296273 238.426 0.0428123 -0.0111443 0.999021 0.118896\n-0.882146 -0.4703 0.0252409 -379.677 0.469305 -0.882263 -0.0369513 237.867 0.0396472 -0.0207508 0.998998 0.148104\n-0.878691 -0.477068 0.0175564 -378.671 0.476458 -0.878679 -0.0301727 237.317 0.0298209 -0.0181476 0.999391 0.13633\n-0.875192 -0.483301 0.0214229 -377.665 0.482609 -0.875299 -0.0306539 236.741 0.0335665 -0.0164891 0.9993 0.0972279\n-0.872044 -0.488846 0.0238624 -376.638 0.487867 -0.872119 -0.0373353 236.144 0.0390621 -0.0209164 0.999018 0.0904693\n-0.869449 -0.493455 0.0236817 -375.585 0.492465 -0.869514 -0.03773 235.543 0.0392096 -0.0211419 0.999007 0.0827713\n-0.868271 -0.495383 0.0264866 -374.514 0.494386 -0.868476 -0.0364971 234.929 0.041083 -0.0185948 0.998983 0.055886\n-0.868134 -0.495483 0.0289989 -373.434 0.494475 -0.868458 -0.0357172 234.306 0.0428816 -0.016668 0.998941 0.0346582\n-0.868752 -0.494431 0.0284116 -372.337 0.493475 -0.869067 -0.0347153 233.683 0.0418559 -0.0161386 0.998993 0.00077345\n-0.870193 -0.492145 0.0236124 -370.702 0.49113 -0.87024 -0.0383817 232.743 0.0394379 -0.0218027 0.998984 -0.0264114\n-0.87277 -0.487889 0.0154073 -369.614 0.487208 -0.872625 -0.0339837 232.138 0.0300251 -0.0221534 0.999304 -0.0207283\n-0.879339 -0.476069 0.0110125 -367.957 0.475581 -0.879143 -0.0305008 231.239 0.024202 -0.0215833 0.999474 -0.0263425\n-0.888092 -0.459189 0.0209327 -366.26 0.458651 -0.888237 -0.0259774 230.381 0.0305217 -0.0134696 0.999443 -0.025368\n-0.901359 -0.432544 0.0214111 -364.54 0.431908 -0.901458 -0.0287951 229.567 0.0317564 -0.0167071 0.999356 -0.0626854\n-0.912908 -0.407751 0.0183993 -363.386 0.407168 -0.9129 -0.0287813 229.085 0.0285324 -0.0187831 0.999416 -0.0272247\n-0.925117 -0.379386 0.0150009 -362.187 0.379015 -0.925112 -0.0226973 228.627 0.0224885 -0.0153121 0.99963 -0.0535163\n-0.936965 -0.349014 0.0168912 -360.966 0.348587 -0.936973 -0.0238617 228.193 0.0241547 -0.0164695 0.999573 -0.0454304\n" }, { "path": "SC-PGO/utils/sample_data/KAIST03/times.txt", "content": "1567410201.7452047\n1567410207.7531185\n1567410208.4532168\n1567410208.9530807\n1567410209.3450341\n1567410209.6453834\n1567410209.9452219\n1567410210.2447982\n1567410210.544965\n1567410210.8452191\n1567410211.0454617\n1567410211.2457383\n1567410211.4453835\n1567410211.6479533\n1567410211.8481796\n1567410212.0483134\n1567410212.2500279\n1567410212.4500558\n1567410212.6502178\n1567410212.8495522\n1567410213.0497336\n1567410213.2498364\n1567410213.4517281\n1567410213.6515045\n1567410213.8512917\n1567410214.053087\n1567410214.2529969\n1567410214.4529209\n1567410214.6528203\n1567410214.8520188\n1567410215.0450361\n1567410215.2451019\n1567410215.4450078\n1567410215.644906\n1567410215.8444886\n1567410216.0448954\n1567410216.2449172\n1567410216.4448299\n1567410216.6449676\n1567410216.844466\n1567410217.0450697\n1567410217.2468629\n1567410217.4467385\n1567410217.6467905\n1567410217.8470364\n1567410218.0471351\n1567410218.246779\n1567410218.4467428\n1567410218.6464281\n1567410218.8456533\n1567410219.0470054\n1567410219.2468638\n1567410219.4482846\n1567410219.6525531\n1567410219.8518422\n1567410220.0528071\n1567410220.2531123\n1567410220.4527521\n1567410220.6524866\n1567410220.8523083\n1567410221.0524056\n1567410221.2528963\n1567410221.4528005\n1567410221.6528673\n1567410221.8527119\n1567410222.0528967\n1567410222.2444682\n1567410222.4447186\n1567410222.6448774\n1567410222.8447428\n1567410223.0448372\n1567410223.2448287\n1567410223.445066\n1567410223.6447496\n1567410223.8449049\n1567410224.0450511\n1567410224.2455447\n1567410224.4451718\n1567410224.6450138\n1567410224.8444712\n1567410225.0453284\n1567410225.2464292\n1567410225.4464896\n1567410225.6464198\n1567410225.8482664\n1567410226.0515001\n1567410226.2511592\n1567410226.4524789\n1567410226.6523218\n1567410226.852448\n1567410227.0447011\n1567410227.2449179\n1567410227.4447949\n1567410227.6447864\n1567410227.84425\n1567410228.0447862\n1567410228.2448809\n1567410228.4449654\n1567410228.644845\n1567410228.8447249\n1567410229.0444915\n1567410229.2462554\n1567410229.4464157\n1567410229.6494269\n1567410229.8493502\n1567410230.0525463\n1567410230.2526176\n1567410230.452632\n1567410230.6525645\n1567410230.8526797\n1567410231.052994\n1567410231.2447214\n1567410231.4445608\n1567410231.6441135\n1567410231.845715\n1567410232.0442555\n1567410232.244323\n1567410232.4445336\n1567410232.6444561\n1567410232.8441877\n1567410233.0441844\n1567410233.2442737\n1567410233.4448144\n1567410233.6449964\n1567410233.8450687\n1567410234.0464108\n1567410234.2460315\n1567410234.446115\n1567410234.6462808\n1567410234.8467982\n1567410235.0468607\n1567410235.2463453\n1567410235.4464836\n1567410235.650646\n1567410235.8509562\n1567410236.0505643\n1567410236.2526717\n1567410236.4448004\n1567410236.6447854\n1567410236.8445621\n1567410237.0451427\n1567410237.2443817\n1567410237.4443457\n1567410237.6446819\n1567410237.9448888\n1567410238.1447361\n1567410238.3447051\n1567410238.5456662\n1567410238.7476647\n1567410238.9478137\n1567410239.149344\n1567410239.3495846\n1567410239.5501108\n1567410239.7523229\n1567410239.9522657\n1567410240.152586\n1567410240.3444958\n1567410240.5443921\n1567410240.7442033\n1567410240.9456682\n1567410241.1445596\n1567410241.3443632\n1567410241.6446719\n1567410241.8479199\n1567410242.0472202\n1567410242.2477927\n1567410242.4477267\n1567410242.647517\n1567410242.8477666\n1567410243.0472648\n1567410243.2479386\n1567410243.4477389\n1567410243.6491528\n1567410243.9522984\n1567410244.1444881\n1567410244.4442875\n1567410244.6442387\n1567410244.9442966\n1567410245.1444557\n1567410245.4460981\n1567410245.6459889\n1567410245.8462894\n1567410246.1459982\n1567410246.3474402\n1567410246.6491477\n1567410246.8490715\n1567410247.0488026\n1567410247.2491019\n1567410247.449121\n1567410247.6489952\n1567410247.8489127\n1567410248.0488837\n1567410248.251754\n1567410248.5515022\n1567410248.7520955\n1567410248.9519696\n1567410249.1518705\n1567410249.4441278\n1567410249.6441216\n1567410249.8440599\n1567410250.043725\n1567410250.2441108\n1567410250.4440937\n1567410250.7442124\n1567410250.9443879\n1567410251.1441865\n1567410251.3441107\n1567410251.5476499\n1567410251.7442834\n1567410251.9443839\n1567410252.1446218\n1567410252.3447344\n1567410252.5438731\n1567410252.7445574\n1567410252.9505448\n1567410253.1505055\n1567410253.351141\n1567410253.5505378\n1567410253.7511415\n1567410253.9508517\n1567410254.151011\n1567410254.3511043\n1567410254.5511172\n1567410254.7524383\n1567410254.9524498\n1567410255.152715\n1567410255.3523431\n1567410255.5520587\n1567410255.7521291\n1567410255.944541\n1567410256.1443701\n1567410256.4443743\n1567410256.646189\n1567410256.8509238\n1567410257.0509818\n1567410257.2527864\n1567410257.5439873\n1567410257.7437105\n1567410257.9443867\n1567410258.1442821\n1567410258.3444278\n1567410258.5493014\n1567410258.7494936\n1567410258.949369\n1567410259.1494846\n1567410259.3493142\n1567410259.5492857\n1567410259.7495325\n1567410259.9493563\n1567410260.1490409\n1567410260.34887\n1567410260.5485656\n1567410260.8492737\n1567410261.0496223\n1567410261.2523766\n1567410261.4443841\n1567410261.6441891\n1567410261.844177\n1567410262.0446141\n1567410262.2452085\n1567410262.4449069\n1567410262.6475999\n1567410262.8475163\n1567410263.0479436\n1567410263.2474761\n1567410263.4478259\n1567410263.6478345\n1567410263.8475497\n1567410264.0491617\n1567410264.2501869\n1567410264.4526927\n1567410264.6529202\n1567410264.8528953\n1567410265.0522687\n1567410265.2445018\n1567410265.4447777\n1567410265.6447027\n1567410265.8446312\n1567410266.0447655\n1567410266.2441697\n1567410266.4462101\n1567410266.6446285\n1567410266.8444536\n1567410267.0448856\n1567410267.2446373\n1567410267.445008\n1567410267.6447523\n1567410267.8462772\n1567410268.046546\n1567410268.2459023\n1567410268.4523396\n1567410268.6524906\n1567410268.8527055\n1567410269.0446415\n1567410269.2444525\n1567410269.4463761\n1567410269.6448543\n1567410269.8447227\n1567410270.044668\n1567410270.2445421\n1567410270.444977\n1567410270.6448677\n1567410270.8448317\n1567410271.0463133\n1567410271.2464786\n1567410271.4479482\n1567410271.6494319\n1567410271.8493488\n1567410272.0491111\n1567410272.2519262\n1567410272.4518559\n1567410272.6520915\n1567410272.8523567\n1567410273.0528908\n1567410273.25368\n1567410273.4455507\n1567410273.6441519\n1567410273.8445015\n1567410274.0442798\n1567410274.2441049\n1567410274.4443731\n1567410274.6446383\n1567410274.8447804\n1567410275.0444386\n1567410275.2457914\n1567410275.4463367\n1567410275.6463375\n1567410275.8475339\n1567410276.0460389\n1567410276.2472718\n1567410276.4475381\n1567410276.6477199\n1567410276.8473737\n1567410277.0440464\n1567410277.2437224\n1567410277.4436896\n1567410277.6441309\n1567410277.8441319\n1567410278.0445013\n1567410278.2437701\n1567410278.4443355\n1567410278.6446466\n1567410278.8445594\n1567410279.0443695\n1567410279.2436292\n1567410279.4472182\n1567410279.6441298\n1567410279.8444073\n1567410280.0443771\n1567410280.2443631\n1567410280.4441521\n1567410280.6459107\n1567410280.8488953\n1567410281.0502372\n1567410281.3519397\n1567410281.5438879\n1567410281.7438421\n1567410281.9442139\n1567410282.1443129\n1567410282.4466572\n1567410282.747366\n1567410283.0490723\n1567410283.3505931\n1567410283.6505954\n1567410283.950001\n1567410284.2518005\n1567410284.5520599\n1567410284.8516448\n1567410285.1437883\n1567410285.4430041\n1567410285.743808\n1567410286.0436404\n1567410286.3435266\n1567410286.6437721\n1567410286.843956\n1567410287.0441427\n1567410287.2455697\n1567410287.4457242\n1567410287.6457024\n1567410287.8457944\n1567410288.0456712\n1567410288.2470634\n1567410288.4473348\n1567410288.6477661\n1567410288.8472447\n1567410289.0472965\n1567410289.247077\n1567410289.4519136\n1567410289.6516876\n1567410289.8519208\n1567410290.0517545\n1567410290.2519848\n1567410290.4510291\n1567410290.6515088\n1567410290.85165\n1567410291.0515814\n1567410291.3440194\n1567410291.543813\n1567410291.7438586\n1567410291.9433451\n1567410292.1440833\n1567410292.3439219\n1567410292.6439986\n1567410292.9442139\n1567410293.1442177\n1567410293.4500093\n1567410293.6499281\n1567410293.8500121\n1567410294.0438249\n1567410294.2437944\n1567410294.4435737\n1567410294.6453245\n1567410294.8439207\n1567410295.1437914\n1567410295.3436935\n1567410295.6437223\n1567410295.8440139\n1567410296.0441308\n1567410296.2441339\n1567410296.4442928\n1567410296.6443572\n1567410296.8442965\n1567410297.0441227\n1567410297.2438879\n1567410297.4440227\n1567410297.6440165\n1567410297.8443787\n1567410298.0445864\n1567410298.2472246\n1567410298.4472644\n1567410298.6469665\n1567410298.9473794\n1567410299.1472862\n1567410299.4473715\n1567410299.6484258\n1567410299.8485994\n1567410300.1512413\n1567410300.3536341\n1567410300.544266\n1567410300.7443264\n1567410300.9469402\n1567410301.146765\n1567410301.3476253\n1567410301.6473334\n1567410301.8504765\n1567410302.1515248\n1567410302.3516631\n1567410302.5517364\n1567410302.7516174\n1567410302.9437904\n1567410303.1432853\n1567410303.3435969\n1567410303.5435505\n1567410303.7451563\n1567410303.9437265\n1567410304.1431615\n1567410304.343931\n1567410304.6434317\n1567410304.8435855\n1567410305.0437415\n1567410305.2438576\n1567410305.4435601\n1567410305.6433213\n1567410305.8435793\n1567410306.0439627\n1567410306.2439432\n1567410306.4435153\n1567410306.6437438\n1567410306.8440371\n1567410307.0442419\n1567410307.2455831\n1567410307.4453011\n1567410307.6469448\n1567410307.8470869\n1567410308.0469847\n1567410308.2470725\n1567410308.4483156\n1567410308.6477661\n1567410308.8483832\n1567410309.0498533\n1567410309.2496202\n1567410309.449779\n1567410309.7499375\n1567410309.9510431\n1567410310.1512198\n1567410310.3435941\n1567410310.5429881\n1567410310.7431571\n1567410310.9431992\n1567410311.1426611\n1567410311.3434441\n1567410311.5437143\n1567410311.7454619\n1567410311.9454749\n1567410312.2465646\n1567410312.4464986\n1567410312.64661\n1567410312.946908\n1567410313.1470048\n1567410313.3470998\n1567410313.5469246\n1567410313.7465718\n1567410313.9468861\n1567410314.1484623\n1567410314.3483121\n1567410314.5480759\n1567410314.7496269\n1567410314.9482849\n1567410315.2496192\n1567410315.4512198\n1567410315.6528115\n1567410315.8440332\n1567410316.0433464\n1567410316.2431216\n1567410316.4431038\n1567410316.6435995\n1567410316.8437085\n1567410317.043787\n1567410317.2434354\n1567410317.4435775\n1567410317.6481659\n1567410317.8506844\n1567410318.0510011\n1567410318.251178\n1567410318.4513674\n1567410318.6512711\n1567410318.8509917\n1567410319.0511892\n1567410319.2433083\n1567410319.4434991\n1567410319.6437125\n1567410319.8435354\n1567410320.0433514\n1567410320.2431757\n1567410320.4433608\n1567410320.6437871\n1567410320.8439615\n1567410321.0438511\n1567410321.2440598\n1567410321.443929\n1567410321.6442082\n1567410321.8438954\n1567410322.0439363\n1567410322.2439864\n1567410322.4442463\n1567410322.6436732\n1567410322.8435543\n1567410323.0437241\n1567410323.3466487\n1567410323.5463784\n1567410323.7465\n1567410323.948297\n1567410324.1482186\n1567410324.3479743\n1567410324.5484102\n1567410324.748553\n1567410324.9484015\n1567410325.1481299\n1567410325.3478744\n1567410325.5482888\n1567410325.7481797\n1567410325.948138\n1567410326.1478438\n1567410326.3507488\n1567410326.5513096\n1567410326.7532022\n1567410326.9435244\n1567410327.2431815\n1567410327.4434626\n1567410327.6435056\n1567410327.8426232\n1567410328.0437992\n1567410328.2436335\n1567410328.4452643\n1567410328.6450891\n1567410328.844877\n1567410329.0450649\n1567410329.2452624\n1567410329.4456213\n1567410329.6453848\n1567410329.844677\n1567410330.046643\n1567410330.2466731\n1567410330.4464989\n1567410330.6465862\n1567410330.8495548\n1567410331.0509691\n1567410331.3428357\n1567410331.5445726\n1567410331.7430668\n1567410331.9447389\n1567410332.1449225\n1567410332.3432028\n1567410332.5433753\n1567410332.7431757\n1567410332.9431918\n1567410333.1430621\n1567410333.342931\n1567410333.5443401\n1567410333.7433095\n1567410333.9437149\n1567410334.1432567\n1567410334.3433268\n1567410334.5452282\n1567410334.7450054\n1567410334.9450514\n1567410335.1449804\n1567410335.3451805\n1567410335.5466135\n1567410335.7466271\n1567410335.9483981\n1567410336.1431928\n1567410336.4452753\n1567410336.6452563\n1567410336.8467135\n1567410337.048034\n1567410337.2482498\n1567410337.4481089\n1567410337.6480582\n1567410337.8481321\n1567410338.0506048\n1567410338.2510293\n1567410338.4511526\n1567410338.6511776\n1567410338.8510411\n1567410339.0511887\n1567410339.2431521\n1567410339.4430408\n1567410339.6428211\n1567410339.8430002\n1567410340.1430488\n1567410340.3432455\n1567410340.5427685\n1567410340.7431979\n1567410340.9431272\n1567410341.1435177\n1567410341.3431995\n1567410341.5435977\n1567410341.7435575\n1567410341.9433463\n1567410342.1433911\n1567410342.3435667\n1567410342.5429966\n1567410342.7435067\n1567410343.042877\n1567410343.2438169\n1567410343.4436641\n1567410343.6450734\n1567410343.9467919\n1567410344.1478846\n1567410344.3478138\n1567410344.5489457\n1567410344.7496626\n1567410345.0492213\n1567410345.2515717\n1567410345.4508104\n1567410345.6508574\n1567410345.8514204\n1567410346.0432255\n1567410346.2437232\n1567410346.443089\n1567410346.6435697\n1567410346.8497965\n1567410347.0497034\n1567410347.2495308\n1567410347.4494176\n1567410347.6494744\n1567410347.8493688\n1567410348.1493287\n1567410348.3511345\n1567410348.5504715\n1567410348.8508782\n1567410349.051007\n1567410349.251199\n1567410349.451215\n1567410349.7433982\n1567410349.9430144\n1567410350.1426694\n1567410350.3431513\n1567410350.5424061\n1567410350.7448804\n1567410350.9465754\n1567410351.1463504\n1567410351.3464749\n1567410351.5462804\n1567410351.7464857\n1567410351.9459803\n1567410352.1475246\n1567410352.3432269\n1567410352.5436146\n1567410352.7437437\n1567410352.9431596\n1567410353.1428347\n1567410353.3476188\n1567410353.5492692\n1567410353.7505696\n1567410353.9506676\n1567410354.1507716\n1567410354.3509147\n1567410354.5507524\n1567410354.7429435\n1567410354.9433258\n1567410355.1450334\n1567410355.3464055\n1567410355.5511615\n1567410355.7503374\n1567410355.9507649\n1567410356.1508152\n1567410356.3432713\n1567410356.5432775\n1567410356.7429903\n1567410356.9431362\n1567410357.1432362\n1567410357.3432479\n1567410357.5430684\n1567410357.7427261\n1567410357.9429471\n1567410358.1433485\n1567410358.343236\n1567410358.5432317\n1567410358.7429798\n1567410358.9430676\n1567410359.1431284\n1567410359.3435237\n1567410359.5433295\n1567410359.7431009\n1567410359.9430649\n1567410360.2431765\n1567410360.4434311\n1567410360.6434069\n1567410360.9432433\n1567410361.2424874\n1567410361.4432337\n1567410361.6435363\n1567410361.8447671\n1567410362.0443966\n1567410362.3446109\n1567410362.5449364\n1567410362.7464948\n1567410362.9473169\n1567410363.1459525\n1567410363.3473814\n1567410363.5474811\n1567410363.7493522\n1567410363.9507365\n1567410364.1490164\n1567410364.3491695\n1567410364.549305\n1567410364.750813\n1567410364.9507535\n1567410365.1431534\n1567410365.3428497\n1567410365.54298\n1567410365.7431514\n1567410365.9432192\n1567410366.1431401\n1567410366.344713\n1567410366.5447779\n1567410366.7448969\n1567410366.9462965\n1567410367.1477721\n1567410367.3474305\n1567410367.5495956\n1567410367.7494957\n1567410367.9510167\n1567410368.1505389\n1567410368.3429494\n1567410368.5431387\n1567410368.743118\n1567410368.942982\n1567410369.1430626\n1567410369.3433213\n1567410369.5434637\n1567410369.743463\n1567410369.9434338\n1567410370.1433702\n1567410370.3431792\n1567410370.5433326\n1567410370.7424281\n1567410370.9430072\n1567410371.1430848\n1567410371.3430564\n1567410371.5434468\n1567410371.7436738\n1567410371.9440463\n1567410372.1436737\n1567410372.3432505\n1567410372.5433242\n1567410372.7435076\n1567410372.9435406\n1567410373.1449056\n1567410373.3461659\n1567410373.5448649\n1567410373.7447844\n1567410373.9464095\n1567410374.1478601\n1567410374.3480096\n1567410374.5481551\n1567410374.8476808\n1567410375.0490994\n1567410375.2493787\n1567410375.4491842\n1567410375.6491997\n1567410375.8492706\n1567410376.0430508\n1567410376.2431049\n1567410376.4426377\n1567410376.6428902\n1567410376.842639\n1567410377.0432377\n1567410377.2495689\n1567410377.4493372\n1567410377.6512203\n1567410377.8478844\n1567410378.0481112\n1567410378.2483006\n1567410378.4478123\n1567410378.6493142\n1567410378.8488283\n1567410379.042608\n1567410379.2431693\n1567410379.4426045\n1567410379.643064\n1567410379.8433766\n1567410380.0429747\n1567410380.2429264\n1567410380.4426591\n1567410380.6431952\n1567410380.8430879\n1567410381.0449202\n1567410381.2449682\n1567410381.4453096\n1567410381.6445024\n1567410381.8459287\n1567410382.1475887\n1567410382.4465654\n1567410382.7473874\n1567410383.0492859\n1567410383.3489983\n1567410383.6491287\n1567410383.9505274\n1567410384.2505481\n1567410384.5504646\n1567410384.8427606\n1567410385.1428924\n1567410385.4427042\n1567410385.7430067\n1567410386.0444622\n1567410386.3458128\n1567410386.6461675\n1567410386.9475522\n1567410387.2488098\n1567410387.5508738\n1567410387.8445377\n1567410388.1432941\n1567410388.4430287\n1567410388.7447875\n1567410388.944834\n1567410389.1451838\n1567410389.3444831\n1567410389.5493255\n1567410389.7493732\n1567410389.9493153\n1567410390.1507685\n1567410390.3506923\n1567410390.550796\n1567410390.7505713\n1567410390.9427717\n1567410391.2429259\n1567410391.4431484\n1567410391.6438065\n1567410391.8432026\n1567410392.0433173\n1567410392.2429535\n1567410392.4430885\n1567410392.6431644\n1567410392.9430952\n1567410393.1432216\n1567410393.3431394\n1567410393.5429988\n1567410393.7428591\n1567410393.9428778\n1567410394.1429362\n1567410394.4431064\n1567410394.6433673\n1567410394.8431697\n1567410395.0431459\n1567410395.2429056\n1567410395.4430985\n1567410395.7431993\n1567410395.9432261\n1567410396.2447484\n1567410396.4440367\n1567410396.6487384\n1567410396.9491599\n1567410397.1489294\n1567410397.3491056\n1567410397.5489223\n1567410397.7488627\n1567410397.9488463\n1567410398.1505098\n1567410398.4515874\n1567410398.6503339\n1567410398.8509729\n1567410399.042861\n1567410399.2427347\n1567410399.4441397\n1567410399.6425841\n1567410399.8426142\n1567410400.0427024\n1567410400.2428703\n1567410400.4427586\n1567410400.6425536\n1567410400.8425465\n1567410401.0429752\n1567410401.3442457\n1567410401.543952\n1567410401.8450787\n1567410402.0447905\n1567410402.3448141\n1567410402.6461039\n1567410402.8494492\n1567410403.050226\n1567410403.3426268\n1567410403.5425584\n1567410403.7426832\n1567410403.9432218\n1567410404.1430116\n1567410404.4426265\n1567410404.6425638\n1567410404.8428555\n1567410405.0429916\n1567410405.3448131\n1567410405.5445528\n1567410405.8457093\n1567410406.0461304\n1567410406.2478151\n1567410406.4475729\n1567410406.6472991\n1567410406.8489316\n1567410407.0504096\n1567410407.3426263\n1567410407.5426021\n1567410407.7426381\n1567410407.942697\n1567410408.1428642\n1567410408.3427374\n1567410408.5426269\n1567410408.7426052\n1567410408.9425383\n1567410409.1430528\n1567410409.3426983\n1567410409.5427868\n1567410409.74297\n1567410409.9428806\n1567410410.1444011\n1567410410.344878\n1567410410.5458868\n1567410410.7491331\n1567410410.9490056\n1567410411.1491549\n1567410411.3490119\n1567410411.5426629\n1567410411.7428\n1567410411.9428213\n1567410412.1427188\n1567410412.3426704\n1567410412.5428848\n1567410412.842886\n1567410413.0424676\n1567410413.2419894\n1567410413.4422174\n1567410413.6430662\n1567410413.8431571\n1567410414.044416\n1567410414.24544\n1567410414.5453959\n1567410414.7456372\n1567410414.9460194\n1567410415.1462951\n1567410415.3467002\n1567410415.5475078\n1567410415.7475638\n1567410415.9475734\n1567410416.1475682\n1567410416.3469095\n1567410416.5469718\n1567410416.7469172\n1567410416.9468002\n1567410417.1470397\n1567410417.3467691\n1567410417.5476158\n1567410417.7473588\n1567410417.9471078\n1567410418.1469443\n1567410418.3468313\n1567410418.5468266\n1567410418.7471211\n1567410418.9488897\n1567410419.1474781\n1567410419.3471997\n1567410419.5470612\n1567410419.749697\n1567410419.9499469\n1567410420.1421542\n1567410420.341994\n1567410420.5423031\n1567410420.7420824\n1567410420.9420705\n1567410421.142262\n1567410421.343924\n1567410421.5421238\n1567410421.7420449\n1567410422.0421877\n1567410422.2422776\n1567410422.4423478\n1567410422.6421826\n1567410422.8422356\n1567410423.0435438\n1567410423.2427137\n1567410423.4441347\n1567410423.7440217\n1567410423.9441226\n1567410424.2440193\n1567410424.4440887\n1567410424.6488893\n1567410424.8481297\n1567410425.0487916\n1567410425.2489722\n1567410425.4501247\n1567410425.6502049\n1567410425.8495998\n1567410426.0501931\n1567410426.3421173\n1567410426.5426133\n1567410426.8427696\n1567410427.1427581\n1567410427.3413868\n1567410427.5423799\n1567410427.8418863\n1567410428.0429089\n1567410428.2442005\n1567410428.5472424\n1567410428.7474284\n1567410428.9473991\n1567410429.147403\n1567410429.3474531\n1567410429.5472429\n1567410429.7470331\n1567410429.946939\n1567410430.1468666\n1567410430.3471465\n1567410430.5474076\n1567410430.7470338\n1567410430.9469039\n1567410431.1470244\n1567410431.3471634\n1567410431.5470119\n1567410431.7469745\n1567410431.9471433\n1567410432.1474438\n1567410432.3472581\n1567410432.5471778\n1567410432.7484891\n1567410432.9489236\n1567410433.1488101\n1567410433.3485587\n1567410433.5481932\n1567410433.7481513\n1567410433.9483948\n1567410434.1503191\n1567410434.3428304\n1567410434.5423338\n1567410434.7422802\n1567410434.9420788\n1567410435.1439211\n1567410435.3439863\n1567410435.5441597\n1567410435.74383\n1567410435.9437356\n1567410436.143959\n1567410436.3439927\n1567410436.5457473\n1567410436.7470071\n1567410436.94681\n1567410437.1485658\n1567410437.3487158\n1567410437.5486608\n1567410437.748421\n1567410437.9498708\n1567410438.1499078\n1567410438.3420026\n1567410438.5425997\n1567410438.7424457\n1567410438.9424841\n1567410439.1424789\n1567410439.3425903\n1567410439.5425014\n1567410439.7425091\n1567410439.9424183\n1567410440.1425934\n1567410440.3426435\n1567410440.5424907\n1567410440.742398\n1567410440.9426422\n1567410441.1470876\n1567410441.3487089\n1567410441.5484102\n1567410441.7484572\n1567410441.948662\n1567410442.1488175\n1567410442.3486052\n1567410442.5474367\n1567410442.7486157\n1567410442.9483726\n1567410443.1485868\n1567410443.3486595\n1567410443.5480242\n1567410443.748266\n1567410443.9498506\n1567410444.1420784\n1567410444.3424304\n1567410444.5421553\n1567410444.7425153\n1567410444.9421697\n1567410445.1420329\n1567410445.3421981\n1567410445.5424128\n1567410445.7426617\n1567410445.9422691\n1567410446.1426039\n1567410446.342767\n1567410446.5416417\n1567410446.7420936\n1567410447.0439415\n1567410447.2457612\n1567410447.4454908\n1567410447.6458776\n1567410447.8473163\n1567410448.0463238\n1567410448.2468488\n1567410448.4470797\n1567410448.6470962\n1567410448.8467619\n1567410449.0467634\n1567410449.2468045\n1567410449.4489498\n1567410449.6489041\n1567410449.8488162\n1567410450.0488257\n1567410450.2487674\n1567410450.448947\n1567410450.6502163\n1567410450.8423858\n1567410451.0423658\n1567410451.2426674\n1567410451.4423633\n1567410451.6426826\n1567410451.8427143\n1567410452.0444028\n1567410452.244128\n1567410452.4461322\n1567410452.6460502\n1567410452.8456774\n1567410453.0472944\n1567410453.2482848\n1567410453.4504321\n1567410453.642648\n1567410453.8426292\n1567410454.0428913\n1567410454.2439044\n1567410454.4441199\n1567410454.6439359\n1567410454.8441484\n1567410455.0439639\n1567410455.2433109\n1567410455.4440308\n1567410455.6470168\n1567410455.848454\n1567410456.0484545\n1567410456.2484684\n1567410456.4492948\n1567410456.6501114\n1567410456.8501716\n1567410457.0427251\n1567410457.2427468\n1567410457.4435766\n1567410457.6422045\n1567410457.842335\n1567410458.0426171\n1567410458.2440624\n1567410458.4441154\n1567410458.6442049\n1567410458.8444571\n1567410459.0457997\n1567410459.2455318\n1567410459.4468486\n1567410459.6483788\n1567410459.8485475\n1567410460.0502162\n1567410460.2423236\n1567410460.4421852\n1567410460.6421819\n1567410460.8427978\n1567410461.0425935\n1567410461.2422121\n1567410461.4437194\n1567410461.6442511\n1567410461.8443682\n1567410462.0452828\n1567410462.2448959\n1567410462.44573\n1567410462.6454511\n1567410462.8471098\n1567410463.0467107\n1567410463.2469192\n1567410463.4489479\n1567410463.6486127\n1567410463.8483307\n1567410464.0480313\n1567410464.2473216\n1567410464.4486396\n1567410464.6487443\n1567410464.8487406\n1567410465.0482335\n1567410465.2483475\n1567410465.4487398\n1567410465.648977\n1567410465.8486478\n1567410466.0485992\n1567410466.2486196\n1567410466.4485321\n1567410466.6487439\n1567410466.8486731\n1567410467.0484343\n1567410467.2481842\n1567410467.4482291\n1567410467.6484811\n1567410467.8485515\n1567410468.048445\n1567410468.2484424\n1567410468.447948\n1567410468.6485639\n1567410468.8483901\n1567410469.0482111\n1567410469.2482827\n1567410469.4479055\n1567410469.649987\n1567410469.8498027\n1567410470.1419191\n1567410470.3420801\n1567410470.6420717\n1567410470.8419058\n1567410471.1422663\n1567410471.3421545\n1567410471.5423324\n1567410471.7422175\n1567410471.9415653\n1567410472.2420413\n1567410472.4423974\n1567410472.6438615\n1567410472.9436269\n1567410473.1438289\n1567410473.3440623\n1567410473.6459\n1567410473.94647\n1567410474.1472573\n1567410474.3469348\n1567410474.546865\n1567410474.8468559\n1567410475.0469577\n1567410475.2468987\n1567410475.5480468\n1567410475.7481756\n1567410475.9484334\n1567410476.1499023\n1567410476.449455\n1567410476.6425979\n1567410476.842149\n1567410477.0420101\n1567410477.3417482\n1567410477.6422014\n1567410477.945034\n1567410478.2450373\n1567410478.5469198\n1567410478.8438721\n1567410479.1450069\n1567410479.4481966\n1567410479.7421315\n1567410480.0422046\n1567410480.3423169\n1567410480.7422738\n1567410481.2425258\n1567410481.8419845\n1567410482.3421071\n1567410482.7420888\n1567410483.2434833\n1567410483.6455631\n1567410484.0480301\n1567410484.4420457\n1567410484.7416525\n1567410485.1419988\n1567410485.5420022\n1567410485.842067\n1567410486.142338\n1567410486.4413719\n1567410486.8424711\n1567410487.042232\n1567410487.2424345\n1567410487.4417634\n1567410487.6428597\n1567410487.8428364\n1567410488.0437262\n1567410488.3467517\n1567410488.5467441\n1567410488.746855\n1567410488.9483156\n1567410489.1481943\n1567410489.351105\n1567410489.541573\n1567410489.7416937\n1567410489.9419594\n1567410490.1419342\n1567410490.3418207\n1567410490.5423903\n1567410490.7438178\n1567410490.9436803\n1567410491.1428895\n1567410491.3437445\n1567410491.5452719\n1567410491.7451518\n1567410491.94662\n1567410492.1476827\n1567410492.3482606\n1567410492.5481079\n1567410492.7482085\n1567410492.9496725\n1567410493.1493318\n1567410493.3495927\n1567410493.5492892\n1567410493.7494373\n1567410493.9494231\n1567410494.1413074\n1567410494.3421514\n1567410494.6418672\n1567410494.8416059\n1567410495.0415318\n1567410495.241601\n1567410495.5419955\n1567410495.742079\n1567410496.0420868\n1567410496.2425537\n1567410496.5437121\n1567410496.7436116\n1567410496.943152\n1567410497.1431036\n1567410497.3434951\n1567410497.5448942\n1567410497.8449526\n1567410498.0463238\n1567410498.3485754\n1567410498.549577\n1567410498.7493525\n1567410498.9413383\n1567410499.1413877\n1567410499.342155\n1567410499.5417321\n1567410499.7419801\n1567410499.9419127\n1567410500.141607\n1567410500.3418894\n1567410500.5416398\n1567410500.7414837\n1567410500.941381\n1567410501.1413963\n1567410501.3416934\n1567410501.5414264\n1567410501.7415118\n1567410501.9415698\n1567410502.1411562\n1567410502.3414581\n1567410502.4416006\n1567410502.5416622\n1567410502.8419638\n1567410502.9419577\n1567410503.041877\n1567410503.2418094\n1567410503.3417768\n1567410503.4416995\n1567410503.5416064\n1567410503.6411161\n1567410503.7415717\n1567410503.8415093\n1567410503.9417629\n1567410504.0417299\n1567410504.1417346\n1567410504.2420268\n1567410504.3415473\n1567410504.4421606\n1567410504.541867\n1567410504.6417687\n1567410504.7416515\n1567410504.8415997\n1567410504.9421318\n1567410505.0436246\n1567410505.1435421\n1567410505.2435281\n1567410505.348053\n1567410505.447875\n1567410505.5476232\n1567410505.6476526\n1567410505.7477422\n1567410505.8476658\n1567410505.9477565\n1567410506.0477452\n1567410506.1478713\n1567410506.2494507\n1567410506.3495035\n1567410506.4493754\n1567410506.5493453\n1567410506.6492245\n1567410506.749285\n1567410506.9495173\n1567410507.0510726\n1567410507.1415808\n1567410507.2416108\n1567410507.3413656\n1567410507.4413972\n1567410507.541286\n1567410507.641573\n1567410507.7411301\n1567410507.8413239\n1567410507.9417331\n1567410508.0433447\n1567410508.1432333\n1567410508.2432601\n1567410508.342905\n1567410508.4449797\n1567410508.5449207\n1567410508.6448901\n1567410508.7449071\n1567410508.8449271\n1567410508.9449563\n1567410509.0448542\n1567410509.1446903\n1567410509.2476909\n1567410509.3467479\n1567410509.4473286\n1567410509.5474803\n1567410509.6475301\n1567410509.7478449\n1567410509.8478706\n1567410509.9479454\n1567410510.0477521\n1567410510.1479223\n1567410510.2476566\n1567410510.3486264\n1567410510.4492819\n1567410510.5491648\n1567410510.6494007\n1567410510.7494643\n1567410510.8491781\n1567410511.1412792\n1567410511.341167\n1567410511.5411088\n1567410511.8417459\n1567410512.041687\n1567410512.2419512\n1567410512.5418584\n1567410512.7433975\n1567410512.9434614\n1567410513.1435888\n1567410513.3440077\n1567410513.5450244\n1567410513.7448878\n1567410513.946244\n1567410514.149123\n1567410514.3410215\n1567410514.5417969\n1567410514.7416205\n1567410514.9414756\n1567410515.1434631\n1567410515.4433727\n1567410515.7448471\n1567410515.9449329\n1567410516.1464474\n1567410516.3466268\n1567410516.5464885\n1567410516.8470626\n1567410517.0483055\n1567410517.347338\n1567410517.5477073\n1567410517.8481925\n1567410518.1476943\n1567410518.3475788\n1567410518.547776\n1567410518.8479977\n1567410519.0479357\n1567410519.2480063\n1567410519.4496958\n1567410519.7418633\n1567410519.9417388\n1567410520.1444705\n1567410520.3417988\n1567410520.541374\n1567410520.7412758\n1567410520.941747\n1567410521.1419029\n1567410521.3422129\n1567410521.6417723\n1567410521.8412335\n1567410522.0418599\n1567410522.2419505\n1567410522.4449761\n1567410522.6465542\n1567410522.8463733\n1567410523.0462008\n1567410523.2478998\n1567410523.548254\n1567410523.7484095\n1567410523.9481103\n1567410524.148165\n1567410524.348\n1567410524.5475605\n1567410524.8478274\n1567410525.0413032\n1567410525.3417356\n1567410525.5410435\n1567410525.7414646\n1567410525.9417362\n1567410526.1415484\n1567410526.3421347\n1567410526.5435607\n1567410526.743602\n1567410526.9433558\n1567410527.1494298\n1567410527.3493733\n1567410527.541429\n1567410527.7413349\n1567410527.9414618\n1567410528.141907\n1567410528.3419745\n1567410528.5424855\n1567410528.7448249\n1567410528.9461918\n1567410529.1462941\n1567410529.3461895\n1567410529.5462971\n1567410529.7463164\n1567410529.9480762\n1567410530.1497242\n1567410530.3417246\n1567410530.6413314\n1567410530.841634\n1567410531.0417645\n1567410531.2421396\n1567410531.4414809\n1567410531.6416602\n1567410531.8416893\n1567410532.0430288\n1567410532.2430356\n1567410532.4429367\n1567410532.6430628\n1567410532.8431976\n1567410533.0443485\n1567410533.2447262\n1567410533.4449978\n1567410533.7458522\n1567410533.947356\n1567410534.1475828\n1567410534.4478598\n1567410534.6479485\n1567410534.848074\n1567410535.1410863\n1567410535.3426719\n1567410535.541635\n1567410535.7431188\n1567410535.9445446\n1567410536.1446278\n1567410536.3449528\n1567410536.6450315\n1567410536.8446164\n1567410537.144429\n1567410537.344682\n1567410537.5463693\n1567410537.7465494\n1567410538.0455673\n1567410538.3458691\n1567410538.5461071\n1567410538.8463337\n1567410539.0461719\n1567410539.2464468\n1567410539.44612\n1567410539.6463737\n1567410539.8463113\n1567410540.0462952\n1567410540.2460115\n1567410540.4459906\n1567410540.6463413\n1567410540.846576\n1567410541.0464466\n1567410541.246136\n1567410541.4459255\n1567410541.6459851\n1567410541.849323\n1567410542.1492498\n1567410542.3412471\n1567410542.5407031\n1567410542.740725\n1567410542.9416053\n1567410543.2411981\n1567410543.4411232\n1567410543.6415656\n1567410543.8414376\n1567410544.0414803\n1567410544.3417411\n1567410544.6463056\n1567410544.8461177\n1567410545.0477798\n1567410545.3477519\n1567410545.5476205\n1567410545.748956\n1567410546.1414239\n1567410546.4413509\n1567410546.7411075\n1567410547.0409214\n1567410547.4433424\n1567410547.8486745\n1567410548.2486753\n1567410548.6491582\n1567410549.1491485\n1567410549.5412133\n1567410549.8413301\n1567410550.1411071\n1567410550.4413543\n1567410550.841388\n1567410551.2414591\n1567410551.6488876\n1567410551.8488448\n1567410552.0410392\n1567410552.3409452\n1567410552.5408683\n1567410552.7407405\n1567410553.0414233\n1567410553.3414438\n1567410553.6412971\n1567410553.9412928\n1567410554.1415784\n1567410554.3434391\n1567410554.5431144\n1567410554.7428384\n1567410555.0429089\n1567410555.3431585\n1567410555.542872\n1567410555.7428157\n1567410555.944212\n1567410556.2443349\n1567410556.5472798\n1567410556.8472378\n1567410557.0411172\n1567410557.342628\n1567410557.7420807\n1567410557.9441884\n1567410558.2435846\n1567410558.5460749\n1567410558.7475011\n1567410559.0469849\n1567410559.2470336\n1567410559.5416558\n1567410559.8431044\n1567410560.0446374\n1567410560.3455944\n1567410560.5407276\n1567410560.8410337\n1567410561.0410604\n1567410561.2413728\n1567410561.4413528\n1567410561.7403183\n1567410561.9409838\n1567410562.2407579\n1567410562.4404852\n1567410562.7422919\n1567410563.0428743\n1567410563.2429514\n1567410563.5449042\n1567410563.7442818\n1567410563.943382\n1567410564.1470509\n1567410564.5409157\n1567410564.7409384\n1567410564.9409261\n1567410565.1411788\n1567410565.441956\n1567410565.6428096\n1567410565.9438682\n1567410566.2439826\n1567410566.4434431\n1567410566.6442811\n1567410566.8472846\n1567410567.0469735\n1567410567.3474321\n1567410567.5473959\n1567410567.7486196\n1567410567.9407079\n1567410568.241009\n1567410568.4404578\n1567410568.6412034\n1567410568.841162\n1567410569.0408244\n1567410569.3410974\n1567410569.5409305\n1567410569.7409868\n1567410569.9410107\n1567410570.2407279\n1567410570.4407179\n1567410570.6420705\n1567410570.8413386\n1567410571.0411804\n1567410571.3424699\n1567410571.5424168\n1567410571.74264\n1567410572.0429139\n1567410572.2428548\n1567410572.4430892\n1567410572.644671\n1567410572.8456774\n1567410573.0455515\n1567410573.3469796\n1567410573.64726\n1567410573.8474071\n1567410574.0487227\n1567410574.3485835\n1567410574.5484536\n1567410574.7408621\n1567410575.0408638\n1567410575.3424387\n1567410575.5438452\n1567410575.7440097\n1567410575.9433689\n1567410576.1455073\n1567410576.4467816\n1567410576.7484901\n1567410577.0407956\n1567410577.2406688\n1567410577.5404484\n1567410577.8402975\n1567410578.1402245\n1567410578.3402629\n1567410578.6408663\n1567410578.9404607\n1567410579.2422628\n1567410579.442116\n1567410579.6424527\n1567410579.9423454\n1567410580.2422931\n1567410580.5419841\n1567410580.8420596\n1567410581.042192\n1567410581.3421524\n1567410581.6420906\n1567410581.8425093\n1567410582.1422982\n1567410582.3421998\n1567410582.5421925\n1567410582.8419628\n1567410583.141413\n1567410583.3421807\n1567410583.6421113\n1567410583.9422741\n1567410584.2422011\n1567410584.5424199\n1567410584.7439582\n1567410585.0437229\n1567410585.2436888\n1567410585.6435182\n1567410585.9469774\n1567410586.1471477\n1567410586.3472838\n1567410586.6476614\n1567410586.8402948\n1567410587.1403077\n1567410587.34022\n1567410587.5402844\n1567410587.8403521\n1567410588.0404642\n1567410588.2405474\n1567410588.5404599\n1567410588.7402689\n1567410589.1397679\n1567410589.3410678\n1567410589.6400924\n1567410589.8407116\n1567410590.0404162\n1567410590.3438363\n1567410590.5402346\n1567410590.8404751\n1567410591.0405571\n1567410591.2403843\n1567410591.5404799\n1567410591.740922\n1567410592.0423737\n1567410592.2422481\n1567410592.6448643\n1567410592.9453132\n1567410593.2404165\n1567410593.5402083\n1567410593.8405533\n1567410594.0422566\n1567410594.343673\n1567410594.6440101\n1567410594.8442175\n1567410595.1435978\n1567410595.4436996\n1567410595.643647\n1567410595.8468113\n1567410596.0481894\n1567410596.3480432\n1567410596.5402625\n1567410596.8402243\n1567410597.0401387\n1567410597.3401399\n1567410597.6397309\n1567410597.9404049\n1567410598.2407382\n1567410598.4409359\n1567410598.7422709\n1567410599.1434493\n1567410599.3451297\n1567410599.6484325\n1567410599.940419\n1567410600.1401408\n1567410600.4406059\n1567410600.7404096\n1567410600.9400418\n1567410601.2400823\n1567410601.4403365\n1567410601.7402012\n1567410601.9403145\n1567410602.240386\n1567410602.5405052\n1567410602.8406799\n1567410603.1402581\n1567410603.4404085\n1567410603.7406301\n1567410604.0420511\n1567410604.2435477\n1567410604.5486233\n1567410604.8396606\n1567410605.040076\n1567410605.3396533\n1567410605.5405009\n1567410605.7404332\n1567410605.9406755\n1567410606.2406926\n1567410606.4408579\n1567410606.7405679\n1567410607.0410733\n1567410607.3399944\n1567410607.6404169\n1567410607.9422088\n1567410608.2423222\n1567410608.5422757\n1567410608.7421308\n1567410608.9421444\n1567410609.143225\n1567410609.3435941\n1567410609.643831\n1567410609.9438431\n1567410610.144078\n1567410610.4436295\n1567410610.7437253\n1567410610.9436877\n1567410611.24332\n1567410611.4437006\n1567410611.7437053\n1567410611.9436257\n1567410612.1436961\n1567410612.4437733\n1567410612.7433143\n1567410612.9435072\n1567410613.2452183\n1567410613.4452379\n1567410613.7483685\n1567410613.9483833\n1567410614.2405851\n1567410614.4407012\n1567410614.7483644\n1567410615.0483015\n1567410615.3482342\n1567410615.7483077\n1567410615.9404905\n1567410616.1406815\n1567410616.4405818\n1567410616.7407026\n1567410616.9467373\n1567410617.1467884\n1567410617.4466519\n1567410617.746763\n1567410618.0479672\n1567410618.3403454\n1567410618.7407238\n1567410619.0405874\n1567410619.2422268\n1567410619.4421749\n1567410619.6433003\n1567410619.9450746\n1567410620.1452491\n1567410620.4451988\n1567410620.7453251\n1567410621.0463204\n1567410621.2465613\n1567410621.5462897\n1567410621.9470017\n1567410622.1467793\n1567410622.4469385\n1567410622.747041\n1567410623.046428\n1567410623.3483891\n1567410623.6482725\n1567410623.9483304\n1567410624.2483387\n1567410624.5475509\n1567410624.8482707\n1567410625.2482977\n1567410625.5481589\n1567410625.8480597\n1567410626.0479555\n1567410626.2482526\n1567410626.5483067\n1567410626.8481033\n1567410627.0475714\n1567410627.2483218\n1567410627.5485899\n1567410627.8481984\n1567410628.1512897\n1567410628.4482234\n1567410628.7402973\n1567410629.0396049\n1567410629.2402039\n1567410629.5402157\n1567410629.7402048\n1567410630.0393705\n1567410630.2400267\n1567410630.44017\n1567410630.7404456\n1567410630.9420896\n1567410631.1416197\n1567410631.3419294\n1567410631.6450465\n1567410631.8450179\n1567410632.0447788\n1567410632.2465048\n1567410632.5461147\n1567410632.7482278\n1567410633.0483377\n1567410633.2484243\n1567410633.4479587\n1567410633.6399872\n1567410633.8401201\n1567410634.1399629\n1567410634.3402641\n1567410634.5403268\n1567410634.7404835\n1567410634.9402285\n1567410635.1406131\n1567410635.4405746\n1567410635.7405984\n1567410635.9405966\n1567410636.1420939\n1567410636.4433589\n1567410636.6432314\n1567410636.8432505\n1567410637.0434742\n1567410637.2430468\n1567410637.5436652\n1567410637.7429411\n1567410637.9432325\n1567410638.1435134\n1567410638.3435514\n1567410638.5451307\n1567410638.7444086\n1567410638.9448156\n1567410639.2478397\n1567410639.4480102\n1567410639.6399965\n1567410639.8398919\n1567410640.039788\n1567410640.2428398\n1567410640.4438527\n1567410640.6450176\n1567410640.845253\n1567410641.0451758\n1567410641.2463222\n1567410641.4471476\n1567410641.6462049\n1567410641.8480279\n1567410642.2470534\n1567410642.4407525\n1567410642.7400408\n1567410642.9397676\n1567410643.2393391\n1567410643.4397724\n1567410643.7397373\n1567410643.9401836\n1567410644.1400573\n1567410644.4407637\n1567410644.6407061\n1567410644.8417928\n1567410645.0416331\n1567410645.3433144\n1567410645.5435286\n1567410645.7436764\n1567410645.9434049\n1567410646.1432207\n1567410646.3434119\n1567410646.6461911\n1567410647.0397658\n1567410647.2396224\n1567410647.4403234\n1567410647.6403711\n1567410647.8401887\n1567410648.0401936\n1567410648.2397642\n1567410648.4403481\n1567410648.6419678\n1567410648.9437227\n1567410649.146265\n1567410649.3463397\n1567410649.5479751\n1567410649.7401428\n1567410649.9398155\n1567410650.1399076\n1567410650.3401358\n1567410650.5403392\n1567410650.7402956\n1567410651.0400712\n1567410651.2397029\n1567410651.4403412\n1567410651.7402477\n1567410651.9467313\n1567410652.1468866\n1567410652.3467052\n1567410652.5467155\n1567410652.7463188\n1567410652.9457793\n1567410653.146234\n1567410653.4462273\n1567410653.6479454\n1567410653.8478844\n1567410654.0478964\n1567410654.24841\n1567410654.4473879\n1567410654.9400103\n1567410655.1405938\n1567410655.3403587\n1567410655.6397817\n1567410655.8400476\n1567410656.0403206\n1567410656.2406132\n1567410656.4418092\n1567410656.6419694\n1567410656.841918\n1567410657.0421209\n1567410657.242115\n1567410657.4418309\n1567410657.7433488\n1567410657.9448688\n1567410658.1460695\n1567410658.4395866\n1567410658.7401443\n1567410658.9391336\n1567410659.140388\n1567410659.4391615\n1567410659.6461987\n1567410659.941361\n1567410660.1418455\n1567410660.3433187\n1567410660.6448622\n1567410660.8450167\n1567410661.0449159\n1567410661.2448893\n1567410661.4457369\n1567410661.6465356\n1567410661.848062\n1567410662.0401795\n1567410662.2402062\n1567410662.5402305\n1567410662.7402871\n1567410663.0404663\n1567410663.2406611\n1567410663.5403445\n1567410663.8405795\n1567410664.041271\n1567410664.3417084\n1567410664.6420298\n1567410664.8421042\n1567410665.1422491\n1567410665.443639\n1567410665.8403685\n1567410666.1398575\n1567410666.4400752\n1567410666.74032\n1567410667.0408237\n1567410667.240593\n1567410667.4400761\n1567410667.7402489\n1567410668.039959\n1567410668.3402593\n1567410668.6402197\n1567410668.9405437\n1567410669.2401841\n1567410669.5402813\n1567410669.7403903\n1567410670.040823\n1567410670.2404597\n1567410670.5400016\n1567410670.8403263\n1567410671.0404444\n1567410671.2401555\n1567410671.5400813\n1567410671.740119\n1567410672.0400887\n1567410672.3405683\n1567410672.5404737\n1567410672.8402646\n1567410673.0478933\n1567410673.3491461\n1567410673.5480816\n1567410673.8400452\n1567410674.0398655\n1567410674.3406231\n1567410674.540081\n1567410674.840589\n1567410675.0398841\n1567410675.3412156\n1567410675.5403035\n1567410675.8403261\n1567410676.0405653\n1567410676.1397913\n1567410676.3397796\n1567410676.439966\n1567410676.6395493\n1567410676.8412025\n1567410676.9422629\n1567410677.1463711\n1567410677.3467882\n1567410677.446938\n1567410677.6480238\n1567410677.8477945\n1567410677.9480467\n1567410678.1483567\n1567410678.3480346\n1567410678.548193\n1567410678.6483004\n1567410678.8482213\n1567410679.0405407\n1567410679.3402526\n1567410679.5400438\n1567410679.6401317\n1567410679.8402483\n1567410680.0401988\n1567410680.1402025\n1567410680.3400757\n1567410680.5399327\n1567410680.7402811\n1567410680.8408544\n1567410681.0403843\n1567410681.2404695\n1567410681.4409325\n1567410681.6404345\n1567410681.8405554\n1567410682.040535\n1567410682.1406083\n1567410682.3420844\n1567410682.5452282\n1567410682.7471263\n1567410683.048559\n1567410683.148535\n1567410683.3477459\n1567410683.4479885\n1567410683.6403928\n1567410683.7406487\n1567410683.9405844\n1567410684.0403831\n1567410684.2402093\n1567410684.3400698\n1567410684.5404625\n1567410684.6403997\n1567410684.8395171\n1567410684.9401908\n1567410685.1407571\n1567410685.3416092\n1567410685.5433438\n1567410685.745234\n1567410686.0453224\n1567410686.3450377\n1567410686.6448984\n1567410686.94666\n1567410687.1480978\n1567410687.3482966\n1567410687.5405526\n1567410687.7402751\n1567410688.041151\n1567410688.2402518\n1567410688.4403355\n1567410688.6405046\n1567410688.9406588\n1567410689.2421856\n1567410689.5423369\n1567410689.7416503\n1567410689.9432473\n1567410690.1434391\n1567410690.3437507\n1567410690.5453987\n1567410690.8417804\n1567410691.1437922\n1567410691.4435694\n1567410691.745239\n1567410692.0449767\n1567410692.3444717\n1567410692.644969\n1567410692.9450266\n1567410693.1451778\n1567410693.3482299\n1567410693.5482433\n1567410693.8421421\n1567410694.1436856\n1567410694.4439225\n1567410694.7436349\n1567410695.1439278\n1567410695.4436224\n1567410695.7450826\n1567410696.0403647\n1567410696.3403292\n1567410696.6406236\n1567410696.9405727\n1567410697.1403573\n1567410697.3404033\n1567410697.5400608\n1567410697.8403261\n1567410698.0398231\n1567410698.340323\n1567410698.6407716\n1567410698.8403871\n1567410699.1403761\n1567410699.4407845\n1567410699.7403815\n1567410700.0398114\n1567410700.2436194\n1567410700.5436025\n1567410700.7440856\n1567410701.0435731\n1567410701.2440865\n1567410701.4437418\n1567410701.745599\n1567410701.9454718\n1567410702.2473505\n1567410702.5466886\n1567410702.8469212\n1567410703.1486695\n1567410703.4405596\n1567410703.7406046\n1567410704.0402465\n1567410704.2405956\n1567410704.5400953\n1567410704.9405878\n1567410705.1406562\n1567410705.4408653\n1567410705.6407945\n1567410705.9435554\n1567410706.1438339\n1567410706.4454391\n1567410706.6455007\n1567410706.9472682\n1567410707.2470694\n1567410707.4466085\n1567410707.7467849\n1567410707.9471164\n1567410708.3468392\n1567410708.5466292\n1567410708.7467697\n1567410709.0466695\n1567410709.2467506\n1567410709.4485843\n1567410709.6483433\n1567410709.9482477\n1567410710.2403579\n1567410710.5405378\n1567410710.8403378\n1567410711.1400034\n1567410711.5404637\n1567410711.7404149\n1567410712.0401907\n1567410712.3400688\n1567410712.5402062\n1567410712.8407657\n1567410713.0408127\n1567410713.3402545\n1567410713.6404781\n1567410713.9400306\n1567410714.1400254\n1567410714.4403846\n1567410714.8403952\n1567410715.0406852\n1567410715.2403479\n1567410715.54055\n1567410715.8402328\n1567410716.1402161\n1567410716.3405573\n1567410716.5419877\n1567410716.7413464\n1567410717.0437701\n1567410717.2414877\n1567410717.5437407\n1567410717.743329\n1567410718.0448537\n1567410718.24454\n1567410718.5450835\n1567410718.7446949\n1567410719.0449579\n1567410719.2462492\n1567410719.4480665\n1567410719.7480111\n1567410719.9485288\n1567410720.3482327\n1567410720.5499725\n1567410720.7400186\n1567410720.9402852\n1567410721.1404715\n1567410721.340163\n1567410721.5430391\n1567410721.7392712\n1567410721.9408381\n1567410722.1421788\n1567410722.34203\n1567410722.6434162\n1567410722.9438167\n1567410723.148062\n1567410723.347883\n1567410723.5477884\n1567410723.7479813\n1567410724.0482156\n1567410724.248239\n1567410724.5483828\n1567410724.7401421\n1567410724.9396777\n1567410725.1400943\n1567410725.542042\n1567410725.7421868\n1567410725.9423399\n1567410726.1419466\n1567410726.3449774\n1567410726.5449626\n1567410726.7464516\n1567410726.945699\n1567410727.1463702\n1567410727.3466635\n1567410727.5468261\n1567410727.7464476\n1567410728.0480433\n1567410728.2403531\n1567410728.4401908\n1567410728.6402085\n1567410728.9394403\n1567410729.1401002\n1567410729.3403432\n1567410729.5418336\n1567410729.841984\n1567410730.0434759\n1567410730.2433825\n1567410730.4430852\n1567410730.743427\n1567410731.0433934\n1567410731.3463602\n1567410731.6480207\n1567410731.9402554\n1567410732.2397971\n1567410732.4410517\n1567410732.6420584\n1567410732.8433778\n1567410733.1447985\n1567410733.3447659\n1567410733.5496213\n1567410733.7461231\n1567410733.9470072\n1567410734.2396567\n1567410734.4396393\n1567410734.639894\n1567410734.9399207\n1567410735.2399282\n1567410735.4397428\n1567410735.6403551\n1567410735.8404739\n1567410736.0404928\n1567410736.2420797\n1567410736.4418373\n1567410736.6423314\n1567410736.8415921\n1567410737.0427415\n1567410737.3460715\n1567410737.5462892\n1567410737.8463767\n1567410738.0479925\n1567410738.2489636\n1567410738.4397352\n1567410738.639765\n1567410738.9399183\n1567410739.2399933\n1567410739.440078\n1567410739.6401212\n1567410739.8401167\n1567410740.0401514\n1567410740.2397337\n1567410740.5399244\n1567410740.7399225\n1567410740.9400642\n1567410741.2400663\n1567410741.5399537\n1567410741.8396597\n1567410742.343353\n1567410742.6439745\n1567410742.9445446\n1567410743.2444432\n1567410743.5444736\n1567410743.7446988\n1567410743.9461448\n1567410744.1452081\n1567410744.345917\n1567410744.5461922\n1567410744.7463462\n1567410745.0477843\n1567410745.2472765\n1567410745.4393241\n1567410745.7395132\n1567410746.0395262\n1567410746.3429918\n1567410746.5429707\n1567410746.7443314\n1567410746.9445829\n1567410747.2443483\n1567410747.4456966\n1567410747.6451476\n1567410747.9462163\n1567410748.3480787\n1567410748.5441282\n1567410748.7443507\n1567410749.0445936\n1567410749.2463095\n1567410749.5461066\n1567410749.8460872\n1567410750.0472333\n1567410750.346607\n1567410750.6461537\n1567410750.9459178\n1567410751.14572\n1567410751.3457565\n1567410751.7399061\n1567410751.9400547\n1567410752.1391199\n1567410752.4397297\n1567410752.6394954\n1567410752.9399912\n1567410753.140022\n1567410753.4396939\n1567410753.6401472\n1567410753.9398711\n1567410754.138916\n1567410754.4397724\n1567410754.739939\n1567410754.9400191\n1567410755.2398183\n1567410755.5397465\n1567410755.8393807\n1567410756.1399515\n1567410756.3400147\n1567410756.6396041\n1567410756.939743\n1567410757.2395225\n1567410757.5397964\n1567410757.7397144\n1567410757.8398409\n1567410758.0398519\n1567410758.1388285\n1567410758.4396327\n1567410758.6395237\n1567410758.7409325\n1567410758.9408486\n1567410759.0422843\n1567410759.242393\n1567410759.342346\n1567410759.4425418\n1567410759.6426935\n1567410759.7425549\n1567410759.8426738\n1567410760.042582\n1567410760.1417639\n1567410760.342298\n1567410760.4422393\n1567410760.5424073\n1567410760.7428472\n1567410760.8425386\n1567410761.0443001\n1567410761.1444261\n1567410761.4446232\n1567410761.6441231\n1567410761.9453454\n1567410762.2457566\n1567410762.5471168\n1567410762.8467116\n1567410763.0473573\n1567410763.338943\n1567410763.539542\n1567410763.7397566\n1567410764.0398993\n1567410764.3394556\n1567410764.6407928\n1567410764.9442337\n1567410765.1441112\n1567410765.3465068\n1567410765.639286\n1567410765.8387427\n1567410766.0394418\n1567410766.3392172\n1567410766.6393218\n1567410766.9395308\n1567410767.1394424\n1567410767.4393926\n1567410767.7394705\n1567410767.939692\n1567410768.2427244\n1567410768.442631\n1567410768.7425878\n1567410768.9427986\n1567410769.2426798\n1567410769.4424868\n1567410769.7426343\n1567410769.9444513\n1567410770.1458709\n1567410770.4454019\n1567410770.7470226\n1567410771.047112\n1567410771.2468855\n1567410771.5467958\n1567410771.8469629\n1567410772.138943\n1567410772.4388354\n1567410772.7392478\n1567410772.9391599\n1567410773.2394373\n1567410773.5392969\n1567410773.8392091\n1567410774.2394965\n1567410774.4397907\n1567410774.7411213\n1567410775.0423553\n1567410775.2452221\n1567410775.4455836\n1567410775.6471291\n1567410775.8390567\n1567410776.1388037\n1567410776.4390223\n1567410776.7391834\n1567410777.0385547\n1567410777.2390397\n1567410777.5392072\n1567410777.8392782\n1567410778.0386002\n1567410778.3388963\n1567410778.5390105\n1567410778.840678\n1567410779.142282\n1567410779.4441361\n1567410779.7437568\n1567410780.0436296\n1567410780.2453597\n1567410780.445637\n1567410780.7467721\n1567410780.9389877\n1567410781.2388735\n1567410781.4388494\n1567410781.738641\n1567410782.0382369\n1567410782.3391211\n1567410782.5394914\n1567410782.840898\n1567410783.1409612\n1567410783.4408906\n1567410783.7410316\n1567410784.042192\n1567410784.3473737\n1567410784.5468545\n1567410784.7394102\n1567410784.9391336\n1567410785.239259\n1567410785.4389617\n1567410785.639303\n1567410785.9412575\n1567410786.1393826\n1567410786.340905\n1567410786.5407829\n1567410786.8406808\n1567410787.1410482\n1567410787.4406302\n1567410787.7407701\n1567410787.9408534\n1567410788.2402058\n1567410788.5407305\n1567410788.7399507\n1567410789.0409365\n1567410789.2403877\n1567410789.5410225\n1567410789.8405328\n1567410790.1421275\n1567410790.3437707\n1567410790.5437493\n1567410790.8437712\n1567410791.1438947\n1567410791.347018\n1567410791.6465349\n1567410791.9389188\n1567410792.2390058\n1567410792.4398234\n1567410792.7384593\n1567410793.0391262\n1567410793.3388543\n1567410793.6389279\n1567410793.9392817\n1567410794.3392682\n1567410794.6388943\n1567410794.9411967\n1567410795.3407369\n1567410795.6407959\n1567410795.8405752\n1567410796.0403366\n1567410796.3402736\n1567410796.5437181\n1567410796.7437074\n1567410796.9451034\n1567410797.2448463\n1567410797.4450247\n1567410797.7465558\n1567410797.9465568\n1567410798.1465518\n1567410798.3467848\n1567410798.5469768\n1567410798.8466489\n1567410799.0466697\n1567410799.2468319\n1567410799.4468207\n1567410799.6465182\n1567410799.846674\n1567410800.1468148\n1567410800.3387959\n1567410800.5391352\n1567410800.7392695\n1567410801.1392648\n1567410801.3389723\n1567410801.538852\n1567410801.838732\n1567410802.0389004\n1567410802.2437391\n1567410802.4430428\n1567410802.6433547\n1567410802.8436213\n1567410803.0435681\n1567410803.3431256\n1567410803.543215\n1567410803.8450782\n1567410804.0452552\n1567410804.2447271\n1567410804.4446983\n1567410804.6449959\n1567410804.8388119\n1567410805.2388432\n1567410805.5389857\n1567410805.738466\n1567410805.9381058\n1567410806.1403909\n1567410806.3407958\n1567410806.5408063\n1567410806.740711\n1567410806.9400835\n1567410807.1419158\n1567410807.3434739\n1567410807.5434272\n1567410807.7434442\n1567410807.943424\n1567410808.1438763\n1567410808.3438156\n1567410808.5436451\n1567410808.8450224\n1567410809.1452043\n1567410809.3451421\n1567410809.5453343\n1567410809.7449868\n1567410810.0452302\n1567410810.2450895\n1567410810.4444218\n1567410810.7386005\n1567410810.938556\n1567410811.1393373\n1567410811.3408246\n1567410811.5409083\n1567410811.8411539\n1567410812.0407929\n1567410812.2404978\n1567410812.4404306\n1567410812.6431584\n1567410813.043643\n1567410813.2436106\n1567410813.343785\n1567410813.5437663\n1567410813.7436609\n1567410813.8435404\n1567410814.043535\n1567410814.2435551\n1567410814.4430625\n1567410814.5438442\n1567410814.7437663\n1567410814.9436917\n1567410815.043638\n1567410815.24523\n1567410815.4461663\n1567410815.6466799\n1567410815.7467127\n1567410815.9384117\n1567410816.1391208\n1567410816.2390568\n1567410816.6390398\n1567410816.7389162\n1567410816.9390402\n1567410817.139415\n1567410817.3390522\n1567410817.639153\n1567410817.8407316\n1567410818.0404377\n1567410818.3407598\n1567410818.5452139\n1567410818.7452559\n1567410819.0466387\n1567410819.2466443\n1567410819.4466929\n1567410819.6464078\n1567410819.9467123\n1567410820.3464882\n1567410820.5387261\n1567410820.7389009\n1567410820.9390323\n1567410821.1390302\n1567410821.3387768\n1567410821.5389647\n1567410821.7390409\n1567410821.9405818\n1567410822.1407471\n1567410822.3434498\n1567410822.6435728\n1567410822.8434191\n1567410823.0435717\n1567410823.2435367\n1567410823.4436719\n1567410823.6430948\n1567410823.844871\n1567410824.0447726\n1567410824.2449718\n1567410824.5454443\n1567410824.745358\n1567410824.9451096\n1567410825.1448967\n1567410825.3451915\n1567410825.5389113\n1567410825.7391441\n1567410825.9389291\n1567410826.2408974\n1567410826.4405463\n1567410826.6424584\n1567410826.842119\n1567410827.0419452\n1567410827.2467051\n1567410827.4386597\n1567410827.6384444\n1567410827.8387563\n1567410828.0389071\n1567410828.2388411\n1567410828.5384018\n1567410828.7386327\n1567410829.0388162\n1567410829.2386642\n1567410829.4384\n1567410829.6384308\n1567410829.838691\n1567410830.040206\n1567410830.240371\n1567410830.4405427\n1567410830.6401973\n1567410830.9400167\n1567410831.1396091\n1567410831.3407366\n1567410831.6404161\n1567410831.8408368\n1567410832.0401323\n1567410832.2401173\n1567410832.540343\n1567410832.7404349\n1567410833.1444855\n1567410833.3460419\n1567410833.5460801\n1567410833.7465048\n1567410834.0463703\n1567410834.2460246\n1567410834.4462059\n1567410834.6462507\n1567410834.8462501\n1567410835.0460768\n1567410835.2461977\n1567410835.4463282\n1567410835.6458428\n1567410835.8456576\n1567410836.0457897\n1567410836.2380171\n1567410836.4380815\n1567410836.6383042\n1567410836.8384361\n1567410837.0382383\n1567410837.4381948\n1567410837.6381133\n1567410837.8382349\n1567410838.0382929\n1567410838.2385595\n1567410838.4462552\n1567410838.6459174\n1567410838.8454046\n1567410839.0460422\n1567410839.2460923\n1567410839.5384073\n1567410839.7382808\n1567410839.9383469\n1567410840.137362\n1567410840.3382328\n1567410840.5381598\n1567410840.8378406\n1567410841.0378342\n1567410841.2380135\n1567410841.5379758\n1567410841.9380112\n1567410842.1381249\n1567410842.3379061\n1567410842.5379696\n1567410842.7378898\n1567410842.9382029\n1567410843.1383462\n1567410843.3379474\n1567410843.5383346\n1567410843.738251\n1567410843.9381959\n1567410844.1383123\n1567410844.3395493\n1567410844.5400243\n1567410844.7397261\n1567410845.0392201\n1567410845.241194\n1567410845.441292\n1567410845.6413343\n1567410845.8406205\n1567410846.2424035\n1567410846.4442387\n1567410846.644608\n1567410846.8438637\n1567410847.0448012\n1567410847.2457318\n1567410847.4379289\n1567410847.6397429\n1567410847.8398247\n1567410848.0419555\n1567410848.2428701\n1567410848.444428\n1567410848.6443067\n1567410848.8435395\n1567410849.1460829\n1567410849.3376703\n1567410849.5387762\n1567410849.7379282\n1567410850.038501\n1567410850.4384727\n1567410850.6382029\n1567410850.9382536\n1567410851.1397762\n1567410851.3400283\n1567410851.5400987\n1567410851.7400126\n1567410851.9396684\n1567410852.1397176\n1567410852.3398578\n1567410852.5398898\n1567410852.7399671\n1567410852.9393542\n1567410853.1397967\n1567410853.3398926\n1567410853.5414894\n1567410853.7410564\n1567410853.9412162\n1567410854.1428661\n1567410854.5441804\n1567410854.7454844\n1567410855.1442549\n1567410855.3445661\n1567410855.5444393\n1567410855.8455503\n1567410856.0456536\n1567410856.2380266\n1567410856.4378512\n1567410856.6378129\n1567410856.8378978\n1567410857.0382874\n1567410857.2379966\n1567410857.4377999\n1567410857.6385024\n1567410857.8384125\n1567410858.0397043\n1567410858.2395172\n1567410858.6396232\n1567410858.8414085\n1567410859.041842\n1567410859.3411274\n1567410859.5400696\n1567410859.740922\n1567410859.9409983\n1567410860.2408972\n1567410860.4409442\n1567410860.6411397\n1567410860.8411078\n1567410861.0406821\n1567410861.2409706\n1567410861.5406644\n1567410861.7431831\n1567410861.9430084\n1567410862.1439519\n1567410862.4440024\n1567410862.8440154\n1567410863.2454655\n1567410863.6381407\n1567410864.0381548\n1567410864.4395819\n1567410864.8397083\n1567410865.2425747\n1567410865.7458012\n1567410866.1377106\n1567410866.6377721\n1567410867.039144\n1567410867.5391476\n1567410867.9394557\n1567410868.3394876\n1567410868.7425423\n1567410869.1438284\n1567410869.5431468\n1567410869.9442656\n1567410870.1441352\n1567410870.344239\n1567410870.5439963\n1567410870.8453817\n1567410871.1457572\n1567410871.3377199\n1567410871.5372121\n1567410871.7373691\n1567410871.9375529\n1567410872.1375675\n1567410872.4375453\n1567410872.6376123\n1567410872.8374944\n1567410873.0370824\n1567410873.2374942\n1567410873.4376388\n1567410873.7378809\n1567410873.9375567\n1567410874.1377921\n1567410874.3376315\n1567410874.5394263\n1567410874.7392268\n1567410874.9392095\n1567410875.3438747\n1567410875.5454273\n1567410875.7454176\n1567410875.9454646\n1567410876.1379199\n1567410876.4378014\n1567410876.6374032\n1567410876.8372431\n1567410877.0373256\n1567410877.2378409\n1567410877.4391751\n1567410877.7395113\n1567410877.9393792\n1567410878.1394374\n1567410878.3376305\n1567410878.5375886\n1567410878.7376513\n1567410879.0372849\n1567410879.2374787\n1567410879.4393284\n1567410879.8393805\n1567410880.0384274\n1567410880.239023\n1567410880.4392033\n1567410880.6396415\n1567410880.9421666\n1567410881.1419792\n1567410881.4423625\n1567410881.6424527\n1567410881.8423049\n1567410882.042239\n1567410882.3418899\n1567410882.5420845\n1567410882.7417812\n1567410883.0425053\n1567410883.3423264\n1567410883.5377502\n1567410883.7383635\n1567410884.0394363\n1567410884.2391317\n1567410884.6391542\n1567410884.9408407\n1567410885.1408203\n1567410885.4406905\n1567410885.6404784\n1567410885.9426365\n1567410886.1422698\n1567410886.3423295\n1567410886.6423948\n1567410886.842505\n1567410887.1425014\n1567410887.5420747\n1567410887.8422811\n1567410888.2415705\n1567410888.5426331\n1567410888.742151\n1567410889.041822\n1567410889.2415783\n1567410889.6424773\n1567410889.842041\n1567410890.1419933\n1567410890.3420796\n1567410890.6420681\n1567410890.8422031\n1567410891.1423423\n1567410891.4424508\n1567410891.6422894\n1567410891.942044\n1567410892.142072\n1567410892.4423051\n1567410892.6418531\n1567410892.9421124\n1567410893.1422179\n1567410893.4415576\n1567410893.6431184\n1567410893.8449216\n1567410894.1448503\n1567410894.3372188\n1567410894.7372057\n1567410895.0370886\n1567410895.2387817\n1567410895.5391471\n1567410895.7388155\n1567410895.9386261\n1567410896.2387106\n1567410896.4388103\n1567410896.6449807\n1567410896.9449847\n1567410897.2448704\n1567410897.5373375\n1567410897.836936\n1567410898.1370943\n1567410898.337342\n1567410898.6371\n1567410898.9371355\n1567410899.2371767\n1567410899.4370024\n1567410899.7368131\n1567410900.2369771\n1567410900.4367239\n1567410900.7368407\n1567410901.0372758\n1567410901.3372104\n1567410901.6375868\n1567410901.8372986\n1567410902.1374805\n1567410902.4382441\n1567410902.6403775\n1567410903.0406122\n1567410903.2405357\n1567410903.5406456\n1567410903.7420051\n1567410903.9421217\n1567410904.1424837\n1567410904.343786\n1567410904.6433914\n1567410904.8444774\n1567410905.0448849\n1567410905.4448879\n1567410905.6372833\n1567410905.8369262\n1567410906.0370233\n1567410906.3370929\n1567410906.5369391\n1567410906.7370312\n1567410906.9375634\n1567410907.2373495\n1567410907.4368341\n1567410907.6374471\n1567410907.8386509\n1567410908.1401546\n1567410908.3400779\n1567410908.5403433\n1567410908.8408422\n1567410909.0422382\n1567410909.2420626\n1567410909.5420325\n1567410909.7438366\n1567410910.1436825\n1567410910.343601\n1567410910.5434899\n1567410910.8435946\n1567410911.0432465\n1567410911.2431827\n1567410911.4432893\n1567410911.6449726\n1567410911.8451309\n1567410912.037322\n1567410912.3373966\n1567410912.6377149\n1567410912.8374028\n1567410913.0372229\n1567410913.2373238\n1567410913.4372189\n1567410913.637677\n1567410913.837182\n1567410914.0373304\n1567410914.4373868\n1567410914.6388717\n1567410914.8388422\n1567410915.0386896\n1567410915.2388504\n1567410915.4389515\n1567410915.6388798\n1567410915.8367562\n1567410916.0367484\n1567410916.2371624\n1567410916.4367225\n1567410916.6371312\n1567410916.8371313\n1567410917.0374472\n1567410917.237371\n1567410917.4389701\n1567410917.741982\n1567410917.9418962\n1567410918.1413937\n1567410918.3435106\n1567410918.743247\n1567410918.9445827\n1567410919.3366606\n1567410919.5369344\n1567410919.7372127\n1567410919.9374495\n1567410920.138339\n1567410920.3387694\n1567410920.5387018\n1567410920.7388396\n1567410920.9386055\n1567410921.1384838\n1567410921.3390167\n1567410921.5389075\n1567410921.7387321\n1567410921.9386337\n1567410922.2385588\n1567410922.4384642\n1567410922.7381907\n1567410923.040446\n1567410923.3402898\n1567410923.5396993\n1567410923.7415941\n1567410923.941746\n" } ]