[ { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(gril_calib)\n\nSET(CMAKE_BUILD_TYPE \"Release\")\n\nADD_COMPILE_OPTIONS(-std=c++14 )\n# ADD_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 architecture: ${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)\nfind_package(Ceres REQUIRED)\n\nmessage(Eigen: ${EIGEN3_INCLUDE_DIR})\n\ninclude_directories(\n ../../devel/include\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 States.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(gril_calib\n src/laserMapping.cpp\n include/ikd-Tree/ikd_Tree.cpp\n include/Gril_Calib/Gril_Calib.cpp\n src/preprocess.cpp\n include/Fusion/FusionAhrs.c\n include/Fusion/FusionCompass.c\n include/Fusion/FusionOffset.c)\n\ntarget_link_libraries(gril_calib ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${CERES_LIBRARIES})\ntarget_include_directories(gril_calib PRIVATE ${PYTHON_INCLUDE_DIRS})" }, { "path": "LI-Init-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": "README.md", "content": "# GRIL-Calib [ROS1 / ROS2]\nOfficial implementation of our paper **\"GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrinsic Calibration Method using Ground Plane Motion Constraints\"**. \n\n- ArXiv : [https://arxiv.org/abs/2312.14035](https://arxiv.org/abs/2312.14035) \n- IEEE : [https://ieeexplore.ieee.org/document/10506583](https://ieeexplore.ieee.org/document/10506583) \n\n## About GRIL-Calib\n

\n\n- **GRIL-Calib** is the LiDAR-IMU calibration method for ground robots.\n- Using only **planar motion**, the 6-DOF calibration parameter could be estimated.\n\n## 🚀 ROS2 Support \n\nIf you want to use ROS2 version, check out `humble` branch. \n\n## Prerequisites (ROS1 version)\n- Ubuntu 18.04\n- ROS Melodic\n- PCL >= 1.8\n- Eigen >= 3.3.4\n- [livox_ros_driver](https://github.com/Livox-SDK/livox_ros_driver)\n- [ceres-solver-2.0.0](http://ceres-solver.org/installation.html#linux)\n\n### Set up your environment easily with Docker! 🐳 \n\n**Requires [Docker](https://www.docker.com/) and the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html) installed.**\n\n**1. Enter the `/docker` folder and make a docker image.**\n```\ngit clone https://github.com/Taeyoung96/GRIL-Calib.git\n```\n```\ncd GRIL-Calib/docker\n```\n```\ndocker build -t gril-calib .\n```\n\nWhen you have finished it, use the command `docker images` and you can see the output below.\n```\nREPOSITORY TAG IMAGE ID CREATED SIZE\ngril-calib latest 9f90339349a0 5 months ago 3.78GB\n```\n\n**2. Make docker container (same path as above)**\n\nIn `/docker`, \n```\nsudo chmod -R 777 container_run.sh\n```\n```\n./container_run.sh \n```\n**:warning: You should change {container_name}, {docker image} to suit your environment.** \n\n```\n./container_run.sh gril-calib-container gril-calib:latest \n```\n\nIf you have successfully created the docker container, the terminal output will be similar to the below.\n```\n================Gril-Calib Docker Env Ready================\nroot@taeyoung-cilab:/root/catkin_ws#\n```\n\n**3. Build and run GRIL-Calib**\n\nInside the docker container, build and run the package. \n```\ncatkin_make\n```\n```\nsource devel/setup.bash\n```\n\n## Run with a public dataset \n\nThe launch files for [M2DGR](https://ieeexplore.ieee.org/abstract/document/9664374), [HILTI](https://arxiv.org/abs/2109.11316), and [S3E](https://arxiv.org/abs/2210.13723), as experimented with in the paper, are shown below.\n\n- For M2DGR,\n```\nroslaunch gril_calib m2dgr_xxxx.launch\n```\n\n- For HILTI,\n```\nroslaunch gril_calib hilti_xxxx.launch\n```\n- For S3E,\n```\nroslaunch gril_calib s3e_xxxx.launch\n```\n\nAfter running the launch file, you simply run the bag file for each sequence. \n\n## Run with your custom data\n\n**:warning: This version only supports Spinning LiDAR (Velodyne, Ouster), not Solid-state LiDAR.** \n\nThe reason for this is that the [LiDAR ground segmentation](https://github.com/url-kaist/patchwork-plusplus-ros) algorithm has only been tested on Spinning LiDAR. \nIf we could achieve ground segmentation, we could theoretically do it for a Solid-state LiDAR like Livox Avia. \n\n- Make sure to acquire your data on an area with flat ground.\n- It would be helpful to collect data as the ground robot draws an \"8\".\n- Please make sure the unit of your input angular velocity is rad/s.\n\n### Important parameters\n\nSimilar to [LI-Init](https://github.com/hku-mars/LiDAR_IMU_Init), edit `config/xxx.yaml` to set the below parameters: \n\n- `lid_topic`: Topic name of LiDAR point cloud.\n- `imu_topic`: Topic name of IMU measurements.\n- `imu_sensor_height`: Height from ground to IMU sensor (meter)\n- `data_accum_length`: A threshold to assess if the data is enough for calibration.\n- `x_accumulate`: Parameter that determines how much the x-axis rotates (Assuming the x-axis is front)\n- `y_accumulate`: Parameter that determines how much the y-axis rotates (Assuming the y-axis is left)\n- `z_accumulate`: Parameter that determines how much the z-axis rotates (Assuming the z-axis is up)\n- `gyro_factor`, `acc_factor`, `ground_factor`: Weight for each residual\n- `set_boundary`: When performing nonlinear optimization, set the bound based on the initial value. (only translation vector)\n- `bound_th`: Set the threshold for the bound. (meter) ⭐️ See the [ceres-solver documentation](http://ceres-solver.org/nnls_modeling.html#_CPPv4N5ceres7Problem22SetParameterUpperBoundEPdid) for more information.\n\n## Acknowledgments \n\nThanks to [hku-mars/LiDAR_IMU_Init](https://github.com/hku-mars/LiDAR_IMU_Init) for sharing their awesome work! \nWe also thanks to [url-kaist/patchwork-plusplus-ros](https://github.com/url-kaist/patchwork-plusplus-ros) for sharing LiDAR ground segmentation algorithm. \n\n## Citation\n\nIf you find our paper useful in your research, please cite us using the following entry: \n```BibTeX\n@ARTICLE{10506583,\n author={Kim, TaeYoung and Pak, Gyuhyeon and Kim, Euntai},\n journal={IEEE Robotics and Automation Letters}, \n title={GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrinsic Calibration Method Using Ground Plane Motion Constraints}, \n year={2024},\n volume={9},\n number={6},\n pages={5409-5416},\n keywords={Calibration;Laser radar;Robot sensing systems;Robots;Optimization;Odometry;Vectors;Calibration and identification;sensor fusion},\n doi={10.1109/LRA.2024.3392081}}\n\n```\n\n\n" }, { "path": "config/hilti/Basement_3.yaml", "content": "# rosbag play -s 85 -u 230 Basement_3.bag \n\ncommon:\n lid_topic: \"/os_cloud_node/points\"\n imu_topic: \"/alphasense/imu\"\n\npreprocess:\n lidar_type: 3 # Ouster LiDAR\n scan_line: 64\n blind: 2\n feature_extract_en: false\n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 1.69 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 350 # To determine how much data is used\n x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.90 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : 0.1 \n trans_IL_y : 0.2 \n trans_IL_z : 0.4 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n\nmapping:\n filter_size_surf: 0.5\n filter_size_map: 0.5\n gyr_cov: 0.5\n acc_cov: 0.5\n det_range: 100.0\n ground_cov : 100.0\n\npublish:\n path_en: true\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: false\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\"\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 1.60 # Height from ground to LiDAR sensor (meter)\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 5 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 5.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/hilti/Basement_4.yaml", "content": "# rosbag play -s 75 -u 150 Basement_4.bag \n\ncommon:\n lid_topic: \"/os_cloud_node/points\"\n imu_topic: \"/alphasense/imu\"\n\npreprocess:\n lidar_type: 3 # Ouster LiDAR\n scan_line: 64\n blind: 0.5\n feature_extract_en: false\n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 1.69 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 200 # To determine how much data is used 220 210\n x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.90 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : 0.1 \n trans_IL_y : 0.2 \n trans_IL_z : 0.4 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n\nmapping:\n filter_size_surf: 0.5\n filter_size_map: 0.5\n gyr_cov: 0.5\n acc_cov: 0.5\n det_range: 100.0\n ground_cov : 200.0\n\npublish:\n path_en: true\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: false\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\"\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 1.60 # Height from ground to LiDAR sensor (meter)\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 5 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 5.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/m2dgr/velodyne_m2dgr_gate01.yaml", "content": "# rosbag play -u 140 gate_01.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 32 # LiDAR scan line number\n blind: 1 # blind area in the beginning of each scan line \n feature_extract_en: false\n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 270 # To determine how much data is used\n x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : -0.6 \n trans_IL_y : -0.45 \n trans_IL_z : -0.6 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n\nmapping:\n filter_size_surf: 0.5 \n filter_size_map: 0.5 \n gyr_cov: 0.5 \n acc_cov: 0.5\n det_range: 200.0 # LiDAR range\n ground_cov : 500.0\n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: true\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/m2dgr/velodyne_m2dgr_hall04.yaml", "content": "# rosbag play -u 160 hall_04.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 32 # LiDAR scan line number\n blind: 0.5 # blind area in the beginning of each scan line \n feature_extract_en: false \n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 400 # To determine how much data is used\n x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : -0.6 \n trans_IL_y : -0.45 \n trans_IL_z : -0.6 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n \nmapping:\n filter_size_surf: 0.5 \n filter_size_map: 0.5 \n gyr_cov: 0.5 \n acc_cov: 0.5 \n det_range: 200.0 # LiDAR range\n ground_cov : 220.0\n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: false\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/m2dgr/velodyne_m2dgr_lift04.yaml", "content": "# rosbag play -u 80 lift_04.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 32 # LiDAR scan line number\n blind: 0.5 # blind area in the beginning of each scan line \n feature_extract_en: false \n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU \n\n imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 170 # To determine how much data is used \n x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.76 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : -0.6 \n trans_IL_y : -0.45 \n trans_IL_z : -0.6 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n \nmapping:\n filter_size_surf: 0.5 # 1.0 # 0.5 \n filter_size_map: 0.5 # 1.0 # 0.5\n gyr_cov: 0.5 # 0.5 # 50\n acc_cov: 0.5 # 1.0 # 2\n det_range: 200.0 # LiDAR range\n ground_cov : 10.0\n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: true\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/m2dgr/velodyne_m2dgr_rotation02.yaml", "content": "# rosbag play -u 120 rotation_02.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 32 # LiDAR scan line number\n blind: 1 # blind area in the beginning of each scan line \n feature_extract_en: false \n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 0.73 # absolute height from IMU sensor origin to ground (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 1300 # To determine how much data is used \n x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : -0.6 \n trans_IL_y : -0.45\n trans_IL_z : -0.6\n set_boundary : false\n bound_th : 0.1\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n \nmapping:\n filter_size_surf: 1.0 # 1.0 # 0.5 \n filter_size_map: 1.0 # 1.0 # 0.5\n gyr_cov: 0.5 # 0.5 # 50\n acc_cov: 0.5 # 1.0 # 2\n det_range: 200.0 # LiDAR range\n ground_cov : 2500.0 # 2500.0\n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: false\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/m2dgr/velodyne_m2dgr_street08.yaml", "content": "# rosbag play -s 20 -u 110 street_08.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 32 # LiDAR scan line number\n blind: 1 # blind area in the beginning of each scan line \n feature_extract_en: false \n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 400 # To determine how much data is used\n x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.58 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : -0.6 \n trans_IL_y : -0.45 \n trans_IL_z : -0.6 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n \nmapping:\n filter_size_surf: 1.0 \n filter_size_map: 1.0\n gyr_cov: 0.5 \n acc_cov: 0.5 \n det_range: 200.0 # LiDAR range\n ground_cov : 1000.0 \n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: true\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/ouster64.yaml", "content": "common:\n lid_topic: \"/os1_cloud_node/points\"\n imu_topic: \"/os1_cloud_node/imu\"\n\npreprocess:\n lidar_type: 3 # Ouster LiDAR\n scan_line: 64\n blind: 2\n feature_extract_en: false\n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 1.69 # TODO: Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 350 # To determine how much data is used\n x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : 0.0 \n trans_IL_y : 0.0 \n trans_IL_z : 0.0 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 13.0\n acc_factor : 1.0\n ground_factor : 5.0\n\nmapping:\n filter_size_surf: 0.5\n filter_size_map: 0.5\n gyr_cov: 0.5\n acc_cov: 0.5\n det_range: 100.0\n ground_cov : 100.0\n\npublish:\n path_en: true\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# Taeyoung 추가\ntrajectory_save:\n traj_save_en: false\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\"\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 1.60 # TODO: Height from ground to LiDAR sensor (meter)\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 5 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 5.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/s3e/velodyne_s3e_bob_lab01.yaml", "content": "# rosbag play -s 65 -u 145 S3E_Laboratory_1.bag\n\ncommon:\n lid_topic: \"/Bob/velodyne_points\"\n imu_topic: \"/Bob/imu/data\" # handsfree imu\n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 16 # LiDAR scan line number\n blind: 0.5 # blind area in the beginning of each scan line \n feature_extract_en: false \n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 0.7 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 200 # To determine how much data is used\n x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.1 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : 0.1 \n trans_IL_y : 0.2 \n trans_IL_z : 0.5 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 15.0\n acc_factor : 1.3\n ground_factor : 3.0\n \nmapping:\n filter_size_surf: 0.5 \n filter_size_map: 0.5 \n gyr_cov: 0.5 \n acc_cov: 0.5\n det_range: 200.0 # LiDAR range\n ground_cov : 10.0\n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: false\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.8 # For S3E dataset : https://github.com/PengYu-Team/S3E\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/s3e/velodyne_s3e_bob_lab02.yaml", "content": "# rosbag play -s 220 -u 120 S3E_Laboratory_2.bag\n\ncommon:\n lid_topic: \"/Bob/velodyne_points\"\n imu_topic: \"/Bob/imu/data\" # handsfree imu\n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 16 # LiDAR scan line number\n blind: 0.2 # blind area in the beginning of each scan line \n feature_extract_en: false \n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 2 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 0.7 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 200 # To determine how much data is used\n x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.17 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : 0.1 \n trans_IL_y : 0.2 \n trans_IL_z : 0.5\n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 20.0\n acc_factor : 0.5 \n ground_factor : 10.0\n \nmapping:\n filter_size_surf: 0.1 \n filter_size_map: 0.1 \n gyr_cov: 0.5\n acc_cov: 0.5 \n det_range: 200.0 # LiDAR range\n ground_cov : 100.0\n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: false\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.8 # For S3E dataset : https://github.com/PengYu-Team/S3E\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "config/velodyne16.yaml", "content": "common:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/imu/data\"\n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_line: 32 # LiDAR scan line number\n blind: 0.5 # blind area in the beginning of each scan line \n feature_extract_en: false \n\ncalibration:\n cut_frame: true # false: do not cut input pointcloud\n cut_frame_num: 3 # must be positive integer\n orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz)\n mean_acc_norm: 9.81 # 1: for livox built-in IMU\n\n imu_sensor_height : 0.1 # Height from ground to IMU sensor (meter)\n verbose : true # For debug, Rotation matrix related ground, lidar sensor height\n\n # For Data Accumulation\n data_accum_length: 300 # To determine how much data is used\n x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)\n y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)\n z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)\n\n # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m\n trans_IL_x : 0.0 \n trans_IL_y : 0.0\n trans_IL_z : 0.0 \n set_boundary : false\n bound_th : 0.3\n\n # Weight for residual\n gyro_factor : 10.0\n acc_factor : 1.0\n ground_factor : 5.0\n \nmapping:\n filter_size_surf: 0.5 \n filter_size_map: 0.5 \n gyr_cov: 0.5 \n acc_cov: 0.5 \n det_range: 200.0 # LiDAR range\n ground_cov : 100.0\n\npublish:\n path_en: true # true: output the path of the robot\n scan_publish_en: true # false: close all the point cloud output\n dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.\n scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame\n\n# For LiDAR Map Saving\npcd_save:\n pcd_save_en: false\n interval: -1 # how many LiDAR frames saved in each pcd file; \n # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.\n\n# For LiDAR odometry Saving\ntrajectory_save:\n traj_save_en: true\n traj_file_path: \"/root/catkin_ws/src/result/traj.txt\" # When you use docker, you should enter docker absolute path.\n\n# For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus \npatchworkpp:\n sensor_height: 0.85 # TODO: change this value to your sensor height\n\n mode: \"czm\" \n verbose: false # To check effect of uprightness/elevation/flatness\n visualize: true # Ground Likelihood Estimation is visualized\n \n # Ground Plane Fitting parameters\n num_iter: 3 # Number of iterations for ground plane estimation using PCA.\n num_lpr: 20 # Maximum number of points to be selected as lowest points representative.\n num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch.\n th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points.\n th_dist: 0.125 # threshold for thickenss of ground.\n th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points.\n th_dist_v: 0.1 # threshold for thickenss of vertical structure.\n max_r: 4.0 # max_range of ground estimation area\n min_r: 1.0 # min_range of ground estimation area\n uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.\n\n adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered\n czm:\n num_zones: 4\n num_sectors_each_zone: [16, 32, 54, 32]\n mum_rings_each_zone: [2, 4, 4, 4]\n elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively.\n flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively.\n\n enable_RNR : true\n enable_RVPF : true\n enable_TGR : true\n \n RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points.\n RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others." }, { "path": "docker/Dockerfile", "content": "# Author: Tae Young Kim\n# email: tyoung96@yonsei.ac.kr\n\nFROM ros:melodic\n\n# Install PCL & Eigen & essential libraries\nRUN apt-get update && apt-get install -y cmake libeigen3-dev libpcl-dev \\\n ros-melodic-rviz ros-melodic-pcl-ros ros-melodic-eigen-conversions \\\n libatlas-base-dev libgoogle-glog-dev libsuitesparse-dev libglew-dev wget ros-melodic-image-transport-plugins\n\n# Install jsk-visualization\nRUN apt-get update && apt-get install -y ros-melodic-jsk-recognition ros-melodic-jsk-common-msgs ros-melodic-jsk-rviz-plugins\n\n# Install matplotlib\nRUN apt-get update && apt-get install -y python-pip python-tk && pip install matplotlib\n\n# Install ceres-solver\nWORKDIR /root/thirdParty\nRUN wget https://github.com/ceres-solver/ceres-solver/archive/refs/tags/2.0.0.tar.gz\nRUN tar zxf 2.0.0.tar.gz\nRUN cd ceres-solver-2.0.0\nRUN mkdir build && cd build\nRUN ls\nRUN cmake -DCMAKE_BUILD_TYPE=Release ./ceres-solver-2.0.0 && make -j2 && make install\n\n# Install livox driver\nWORKDIR /root/livox_ws/src\nRUN wget https://github.com/Livox-SDK/livox_ros_driver/archive/refs/tags/v2.6.0.tar.gz\nRUN tar zxf v2.6.0.tar.gz && rm -rf v2.6.0.tar.gz\n\nRUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace'\n\nWORKDIR /root/livox_ws\nRUN /bin/bash -c 'source /opt/ros/melodic/setup.bash && catkin_make'\nRUN /bin/bash -c 'source /root/livox_ws/devel/setup.bash'\n\nWORKDIR /root/catkin_ws/\n\n# Load ROS environment at each run\nCOPY ./ros_entrypoint.sh /\nRUN chmod 755 /ros_entrypoint.sh\nENTRYPOINT [\"/ros_entrypoint.sh\"]\n\nCMD [\"bash\"]" }, { "path": "docker/container_run.sh", "content": "#!/bin/bash\n# Author : Taeyoung Kim (https://github.com/Taeyoung96)\n\n# Set the project directory (PROJECT_DIR) as the parent directory of the current working directory\nPROJECT_DIR=$(dirname \"$PWD\")\n\n# Move to the parent folder of the project directory\ncd \"$PROJECT_DIR\"\n\n# Check if arguments are provided for the image name and tag\nif [ \"$#\" -ne 2 ]; then\n echo \"[Error] Usage: $0 \"\n exit 1\nfi\n\n# Print the current working directory to verify the change\necho \"Current working directory: $PROJECT_DIR\"\n\nxhost +local:docker\n\n# Assign the arguments to variables for clarity\nCONTAINER_NAME=\"$1\"\nIMAGE_NAME=\"$2\"\n\n# Launch the nvidia-docker container with the provided image name and tag\ndocker run --privileged -it \\\n --gpus all \\\n -e NVIDIA_DRIVER_CAPABILITIES=all \\\n -e NVIDIA_VISIBLE_DEVICES=all \\\n --volume=\"$PROJECT_DIR:/root/catkin_ws/src\" \\\n --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw \\\n --net=host \\\n --ipc=host \\\n --shm-size=2gb \\\n --name=\"$CONTAINER_NAME\" \\\n --env=\"DISPLAY=$DISPLAY\" \\\n \"$IMAGE_NAME\" /bin/bash\n" }, { "path": "docker/ros_entrypoint.sh", "content": "#!/bin/bash\n \nset -e\n\n# Ros build\nsource \"/opt/ros/melodic/setup.bash\"\nsource \"/root/livox_ws/devel/setup.bash\"\n\n\n# Libray install if you want\n\necho \"================Gril-Calib Docker Env Ready================\"\n\ncd /root/catkin_ws\n\nexec \"$@\"\n" }, { "path": "include/Fusion/CMakeLists.txt", "content": "file(GLOB_RECURSE files \"*.c\")\n\nadd_library(Fusion ${files})\n\nif(UNIX AND NOT APPLE)\n target_link_libraries(Fusion m) # link math library for Linux\nendif()\n" }, { "path": "include/Fusion/Fusion.h", "content": "/**\n * @file Fusion.h\n * @author Seb Madgwick\n * @brief Main header file for the Fusion library. This is the only file that\n * needs to be included when using the library.\n */\n\n#ifndef FUSION_H\n#define FUSION_H\n\n//------------------------------------------------------------------------------\n// Includes\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n#include \"FusionAhrs.h\"\n#include \"FusionAxes.h\"\n#include \"FusionCalibration.h\"\n#include \"FusionCompass.h\"\n#include \"FusionMath.h\"\n#include \"FusionOffset.h\"\n\n#ifdef __cplusplus\n}\n#endif\n\n#endif\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionAhrs.c", "content": "/**\n * @file FusionAhrs.c\n * @author Seb Madgwick\n * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer\n * measurements into a single measurement of orientation relative to the Earth.\n */\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include // FLT_MAX\n#include \"FusionAhrs.h\"\n#include \"FusionCompass.h\"\n#include // atan2f, cosf, powf, sinf\n\n//------------------------------------------------------------------------------\n// Definitions\n\n/**\n * @brief Initial gain used during the initialisation.\n */\n#define INITIAL_GAIN (10.0f)\n\n/**\n * @brief Initialisation period in seconds.\n */\n#define INITIALISATION_PERIOD (3.0f)\n\n//------------------------------------------------------------------------------\n// Functions\n\n/**\n * @brief Initialises the AHRS algorithm structure.\n * @param ahrs AHRS algorithm structure.\n */\nvoid FusionAhrsInitialise(FusionAhrs *const ahrs) {\n const FusionAhrsSettings settings = {\n .gain = 0.5f,\n .accelerationRejection = 90.0f,\n .magneticRejection = 90.0f,\n .rejectionTimeout = 0,\n };\n FusionAhrsSetSettings(ahrs, &settings);\n FusionAhrsReset(ahrs);\n}\n\n/**\n * @brief Resets the AHRS algorithm. This is equivalent to reinitialising the\n * algorithm while maintaining the current settings.\n * @param ahrs AHRS algorithm structure.\n */\nvoid FusionAhrsReset(FusionAhrs *const ahrs) {\n ahrs->quaternion = FUSION_IDENTITY_QUATERNION;\n ahrs->accelerometer = FUSION_VECTOR_ZERO;\n ahrs->initialising = true;\n ahrs->rampedGain = INITIAL_GAIN;\n ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;\n ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;\n ahrs->accelerometerIgnored = false;\n ahrs->accelerationRejectionTimer = 0;\n ahrs->accelerationRejectionTimeout = false;\n ahrs->magnetometerIgnored = false;\n ahrs->magneticRejectionTimer = 0;\n ahrs->magneticRejectionTimeout = false;\n}\n\n/**\n * @brief Sets the AHRS algorithm settings.\n * @param ahrs AHRS algorithm structure.\n * @param settings Settings.\n */\nvoid FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) {\n ahrs->settings.gain = settings->gain;\n if ((settings->accelerationRejection == 0.0f) || (settings->rejectionTimeout == 0)) {\n ahrs->settings.accelerationRejection = FLT_MAX;\n } else {\n ahrs->settings.accelerationRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);\n }\n if ((settings->magneticRejection == 0.0f) || (settings->rejectionTimeout == 0)) {\n ahrs->settings.magneticRejection = FLT_MAX;\n } else {\n ahrs->settings.magneticRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);\n }\n ahrs->settings.rejectionTimeout = settings->rejectionTimeout;\n if (ahrs->initialising == false) {\n ahrs->rampedGain = ahrs->settings.gain;\n }\n ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;\n}\n\n/**\n * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and\n * magnetometer measurements.\n * @param ahrs AHRS algorithm structure.\n * @param gyroscope Gyroscope measurement in degrees per second.\n * @param accelerometer Accelerometer measurement in g.\n * @param magnetometer Magnetometer measurement in arbitrary units.\n * @param deltaTime Delta time in seconds.\n */\nvoid FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) {\n#define Q ahrs->quaternion.element\n\n // Store accelerometer\n ahrs->accelerometer = accelerometer;\n\n // Ramp down gain during initialisation\n if (ahrs->initialising == true) {\n ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;\n if (ahrs->rampedGain < ahrs->settings.gain) {\n ahrs->rampedGain = ahrs->settings.gain;\n ahrs->initialising = false;\n ahrs->accelerationRejectionTimeout = false;\n }\n }\n\n // Calculate direction of gravity indicated by algorithm\n const FusionVector halfGravity = {\n .axis.x = Q.x * Q.z - Q.w * Q.y,\n .axis.y = Q.y * Q.z + Q.w * Q.x,\n .axis.z = Q.w * Q.w - 0.5f + Q.z * Q.z,\n }; // third column of transposed rotation matrix scaled by 0.5\n\n // Calculate accelerometer feedback\n FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO;\n ahrs->accelerometerIgnored = true;\n if (FusionVectorIsZero(accelerometer) == false) {\n\n // Enter acceleration recovery state if acceleration rejection times out\n if (ahrs->accelerationRejectionTimer > ahrs->settings.rejectionTimeout) {\n const FusionQuaternion quaternion = ahrs->quaternion;\n FusionAhrsReset(ahrs);\n ahrs->quaternion = quaternion;\n ahrs->accelerationRejectionTimer = 0;\n ahrs->accelerationRejectionTimeout = true;\n }\n\n // Calculate accelerometer feedback scaled by 0.5\n ahrs->halfAccelerometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(accelerometer), halfGravity);\n\n // Ignore accelerometer if acceleration distortion detected\n if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection)) {\n halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;\n ahrs->accelerometerIgnored = false;\n ahrs->accelerationRejectionTimer -= ahrs->accelerationRejectionTimer >= 10 ? 10 : 0;\n } else {\n ahrs->accelerationRejectionTimer++;\n }\n }\n\n // Calculate magnetometer feedback\n FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO;\n ahrs->magnetometerIgnored = true;\n if (FusionVectorIsZero(magnetometer) == false) {\n\n // Set to compass heading if magnetic rejection times out\n ahrs->magneticRejectionTimeout = false;\n if (ahrs->magneticRejectionTimer > ahrs->settings.rejectionTimeout) {\n FusionAhrsSetHeading(ahrs, FusionCompassCalculateHeading(halfGravity, magnetometer));\n ahrs->magneticRejectionTimer = 0;\n ahrs->magneticRejectionTimeout = true;\n }\n\n // Compute direction of west indicated by algorithm\n const FusionVector halfWest = {\n .axis.x = Q.x * Q.y + Q.w * Q.z,\n .axis.y = Q.w * Q.w - 0.5f + Q.y * Q.y,\n .axis.z = Q.y * Q.z - Q.w * Q.x\n }; // second column of transposed rotation matrix scaled by 0.5\n\n // Calculate magnetometer feedback scaled by 0.5\n ahrs->halfMagnetometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfWest);\n\n // Ignore magnetometer if magnetic distortion detected\n if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection)) {\n halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;\n ahrs->magnetometerIgnored = false;\n ahrs->magneticRejectionTimer -= ahrs->magneticRejectionTimer >= 10 ? 10 : 0;\n } else {\n ahrs->magneticRejectionTimer++;\n }\n }\n\n // Convert gyroscope to radians per second scaled by 0.5\n const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));\n\n // Apply feedback to gyroscope\n const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));\n\n // Integrate rate of change of quaternion\n ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));\n\n // Normalise quaternion\n ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);\n#undef Q\n}\n\n/**\n * @brief Updates the AHRS algorithm using the gyroscope and accelerometer\n * measurements only.\n * @param ahrs AHRS algorithm structure.\n * @param gyroscope Gyroscope measurement in degrees per second.\n * @param accelerometer Accelerometer measurement in g.\n * @param deltaTime Delta time in seconds.\n */\nvoid FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) {\n\n // Update AHRS algorithm\n FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);\n\n // Zero heading during initialisation\n if ((ahrs->initialising == true) && (ahrs->accelerationRejectionTimeout == false)) {\n FusionAhrsSetHeading(ahrs, 0.0f);\n }\n}\n\n/**\n * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and\n * heading measurements.\n * @param ahrs AHRS algorithm structure.\n * @param gyroscope Gyroscope measurement in degrees per second.\n * @param accelerometer Accelerometer measurement in g.\n * @param heading Heading measurement in degrees.\n * @param deltaTime Delta time in seconds.\n */\nvoid FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime) {\n#define Q ahrs->quaternion.element\n\n // Calculate roll\n const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x);\n\n // Calculate magnetometer\n const float headingRadians = FusionDegreesToRadians(heading);\n const float sinHeadingRadians = sinf(headingRadians);\n const FusionVector magnetometer = {\n .axis.x = cosf(headingRadians),\n .axis.y = -1.0f * cosf(roll) * sinHeadingRadians,\n .axis.z = sinHeadingRadians * sinf(roll),\n };\n\n // Update AHRS algorithm\n FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);\n#undef Q\n}\n\n/**\n * @brief Returns the quaternion describing the sensor relative to the Earth.\n * @param ahrs AHRS algorithm structure.\n * @return Quaternion describing the sensor relative to the Earth.\n */\nFusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) {\n return ahrs->quaternion;\n}\n\n/**\n * @brief Returns the linear acceleration measurement equal to the accelerometer\n * measurement with the 1 g of gravity removed.\n * @param ahrs AHRS algorithm structure.\n * @return Linear acceleration measurement in g.\n */\nFusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) {\n#define Q ahrs->quaternion.element\n const FusionVector gravity = {\n .axis.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),\n .axis.y = 2.0f * (Q.y * Q.z + Q.w * Q.x),\n .axis.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),\n }; // third column of transposed rotation matrix\n const FusionVector linearAcceleration = FusionVectorSubtract(ahrs->accelerometer, gravity);\n return linearAcceleration;\n#undef Q\n}\n\n/**\n * @brief Returns the Earth acceleration measurement equal to accelerometer\n * measurement in the Earth coordinate frame with the 1 g of gravity removed.\n * @param ahrs AHRS algorithm structure.\n * @return Earth acceleration measurement in g.\n */\nFusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) {\n#define Q ahrs->quaternion.element\n#define A ahrs->accelerometer.axis\n const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations\n const float qwqx = Q.w * Q.x;\n const float qwqy = Q.w * Q.y;\n const float qwqz = Q.w * Q.z;\n const float qxqy = Q.x * Q.y;\n const float qxqz = Q.x * Q.z;\n const float qyqz = Q.y * Q.z;\n const FusionVector earthAcceleration = {\n .axis.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),\n .axis.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),\n .axis.z = (2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z)) - 1.0f,\n }; // rotation matrix multiplied with the accelerometer, with 1 g subtracted\n return earthAcceleration;\n#undef Q\n#undef A\n}\n\n/**\n * @brief Returns the AHRS algorithm internal states.\n * @param ahrs AHRS algorithm structure.\n * @return AHRS algorithm internal states.\n */\nFusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) {\n const FusionAhrsInternalStates internalStates = {\n .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),\n .accelerometerIgnored = ahrs->accelerometerIgnored,\n .accelerationRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->accelerationRejectionTimer / (float) ahrs->settings.rejectionTimeout,\n .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),\n .magnetometerIgnored = ahrs->magnetometerIgnored,\n .magneticRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->magneticRejectionTimer / (float) ahrs->settings.rejectionTimeout,\n };\n return internalStates;\n}\n\n/**\n * @brief Returns the AHRS algorithm flags.\n * @param ahrs AHRS algorithm structure.\n * @return AHRS algorithm flags.\n */\nFusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs) {\n const unsigned int warningTimeout = ahrs->settings.rejectionTimeout / 4;\n const FusionAhrsFlags flags = {\n .initialising = ahrs->initialising,\n .accelerationRejectionWarning = ahrs->accelerationRejectionTimer > warningTimeout,\n .accelerationRejectionTimeout = ahrs->accelerationRejectionTimeout,\n .magneticRejectionWarning = ahrs->magneticRejectionTimer > warningTimeout,\n .magneticRejectionTimeout = ahrs->magneticRejectionTimeout,\n };\n return flags;\n}\n\n/**\n * @brief Sets the heading of the orientation measurement provided by the AHRS\n * algorithm. This function can be used to reset drift in heading when the AHRS\n * algorithm is being used without a magnetometer.\n * @param ahrs AHRS algorithm structure.\n * @param heading Heading angle in degrees.\n */\nvoid FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) {\n#define Q ahrs->quaternion.element\n const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z);\n const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading));\n const FusionQuaternion rotation = {\n .element.w = cosf(halfYawMinusHeading),\n .element.x = 0.0f,\n .element.y = 0.0f,\n .element.z = -1.0f * sinf(halfYawMinusHeading),\n };\n ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);\n#undef Q\n}\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionAhrs.h", "content": "/**\n * @file FusionAhrs.h\n * @author Seb Madgwick\n * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer\n * measurements into a single measurement of orientation relative to the Earth.\n */\n\n#ifndef FUSION_AHRS_H\n#define FUSION_AHRS_H\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include \"FusionMath.h\"\n#include \n\n//------------------------------------------------------------------------------\n// Definitions\n\n/**\n * @brief AHRS algorithm settings.\n */\ntypedef struct {\n float gain;\n float accelerationRejection;\n float magneticRejection;\n unsigned int rejectionTimeout;\n} FusionAhrsSettings;\n\n/**\n * @brief AHRS algorithm structure. Structure members are used internally and\n * must not be accessed by the application.\n */\ntypedef struct {\n FusionAhrsSettings settings;\n FusionQuaternion quaternion;\n FusionVector accelerometer;\n bool initialising;\n float rampedGain;\n float rampedGainStep;\n FusionVector halfAccelerometerFeedback;\n FusionVector halfMagnetometerFeedback;\n bool accelerometerIgnored;\n unsigned int accelerationRejectionTimer;\n bool accelerationRejectionTimeout;\n bool magnetometerIgnored;\n unsigned int magneticRejectionTimer;\n bool magneticRejectionTimeout;\n} FusionAhrs;\n\n/**\n * @brief AHRS algorithm internal states.\n */\ntypedef struct {\n float accelerationError;\n bool accelerometerIgnored;\n float accelerationRejectionTimer;\n float magneticError;\n bool magnetometerIgnored;\n float magneticRejectionTimer;\n} FusionAhrsInternalStates;\n\n/**\n * @brief AHRS algorithm flags.\n */\ntypedef struct {\n bool initialising;\n bool accelerationRejectionWarning;\n bool accelerationRejectionTimeout;\n bool magneticRejectionWarning;\n bool magneticRejectionTimeout;\n} FusionAhrsFlags;\n\n//------------------------------------------------------------------------------\n// Function declarations\n\nvoid FusionAhrsInitialise(FusionAhrs *const ahrs);\n\nvoid FusionAhrsReset(FusionAhrs *const ahrs);\n\nvoid FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings);\n\nvoid FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime);\n\nvoid FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime);\n\nvoid FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime);\n\nFusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);\n\nFusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs);\n\nFusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs);\n\nFusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs);\n\nFusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs);\n\nvoid FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading);\n\n#endif\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionAxes.h", "content": "/**\n * @file FusionAxes.h\n * @author Seb Madgwick\n * @brief Swaps sensor axes for alignment with the body axes.\n */\n\n#ifndef FUSION_AXES_H\n#define FUSION_AXES_H\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include \"FusionMath.h\"\n\n//------------------------------------------------------------------------------\n// Definitions\n\n/**\n * @brief Axes alignment describing the sensor axes relative to the body axes.\n * For example, if the body X axis is aligned with the sensor Y axis and the\n * body Y axis is aligned with sensor X axis but pointing the opposite direction\n * then alignment is +Y-X+Z.\n */\ntypedef enum {\n FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */\n FusionAxesAlignmentPXNZPY, /* +X-Z+Y */\n FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */\n FusionAxesAlignmentPXPZNY, /* +X+Z-Y */\n FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */\n FusionAxesAlignmentNXPZPY, /* -X+Z+Y */\n FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */\n FusionAxesAlignmentNXNZNY, /* -X-Z-Y */\n FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */\n FusionAxesAlignmentPYNZNX, /* +Y-Z-X */\n FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */\n FusionAxesAlignmentPYPZPX, /* +Y+Z+X */\n FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */\n FusionAxesAlignmentNYNZPX, /* -Y-Z+X */\n FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */\n FusionAxesAlignmentNYPZNX, /* -Y+Z-X */\n FusionAxesAlignmentPZPYNX, /* +Z+Y-X */\n FusionAxesAlignmentPZPXPY, /* +Z+X+Y */\n FusionAxesAlignmentPZNYPX, /* +Z-Y+X */\n FusionAxesAlignmentPZNXNY, /* +Z-X-Y */\n FusionAxesAlignmentNZPYPX, /* -Z+Y+X */\n FusionAxesAlignmentNZNXPY, /* -Z-X+Y */\n FusionAxesAlignmentNZNYNX, /* -Z-Y-X */\n FusionAxesAlignmentNZPXNY, /* -Z+X-Y */\n} FusionAxesAlignment;\n\n//------------------------------------------------------------------------------\n// Inline functions\n\n/**\n * @brief Swaps sensor axes for alignment with the body axes.\n * @param sensor Sensor axes.\n * @param alignment Axes alignment.\n * @return Sensor axes aligned with the body axes.\n */\nstatic inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) {\n FusionVector result;\n switch (alignment) {\n case FusionAxesAlignmentPXPYPZ:\n break;\n case FusionAxesAlignmentPXNZPY:\n result.axis.x = +sensor.axis.x;\n result.axis.y = -sensor.axis.z;\n result.axis.z = +sensor.axis.y;\n return result;\n case FusionAxesAlignmentPXNYNZ:\n result.axis.x = +sensor.axis.x;\n result.axis.y = -sensor.axis.y;\n result.axis.z = -sensor.axis.z;\n return result;\n case FusionAxesAlignmentPXPZNY:\n result.axis.x = +sensor.axis.x;\n result.axis.y = +sensor.axis.z;\n result.axis.z = -sensor.axis.y;\n return result;\n case FusionAxesAlignmentNXPYNZ:\n result.axis.x = -sensor.axis.x;\n result.axis.y = +sensor.axis.y;\n result.axis.z = -sensor.axis.z;\n return result;\n case FusionAxesAlignmentNXPZPY:\n result.axis.x = -sensor.axis.x;\n result.axis.y = +sensor.axis.z;\n result.axis.z = +sensor.axis.y;\n return result;\n case FusionAxesAlignmentNXNYPZ:\n result.axis.x = -sensor.axis.x;\n result.axis.y = -sensor.axis.y;\n result.axis.z = +sensor.axis.z;\n return result;\n case FusionAxesAlignmentNXNZNY:\n result.axis.x = -sensor.axis.x;\n result.axis.y = -sensor.axis.z;\n result.axis.z = -sensor.axis.y;\n return result;\n case FusionAxesAlignmentPYNXPZ:\n result.axis.x = +sensor.axis.y;\n result.axis.y = -sensor.axis.x;\n result.axis.z = +sensor.axis.z;\n return result;\n case FusionAxesAlignmentPYNZNX:\n result.axis.x = +sensor.axis.y;\n result.axis.y = -sensor.axis.z;\n result.axis.z = -sensor.axis.x;\n return result;\n case FusionAxesAlignmentPYPXNZ:\n result.axis.x = +sensor.axis.y;\n result.axis.y = +sensor.axis.x;\n result.axis.z = -sensor.axis.z;\n return result;\n case FusionAxesAlignmentPYPZPX:\n result.axis.x = +sensor.axis.y;\n result.axis.y = +sensor.axis.z;\n result.axis.z = +sensor.axis.x;\n return result;\n case FusionAxesAlignmentNYPXPZ:\n result.axis.x = -sensor.axis.y;\n result.axis.y = +sensor.axis.x;\n result.axis.z = +sensor.axis.z;\n return result;\n case FusionAxesAlignmentNYNZPX:\n result.axis.x = -sensor.axis.y;\n result.axis.y = -sensor.axis.z;\n result.axis.z = +sensor.axis.x;\n return result;\n case FusionAxesAlignmentNYNXNZ:\n result.axis.x = -sensor.axis.y;\n result.axis.y = -sensor.axis.x;\n result.axis.z = -sensor.axis.z;\n return result;\n case FusionAxesAlignmentNYPZNX:\n result.axis.x = -sensor.axis.y;\n result.axis.y = +sensor.axis.z;\n result.axis.z = -sensor.axis.x;\n return result;\n case FusionAxesAlignmentPZPYNX:\n result.axis.x = +sensor.axis.z;\n result.axis.y = +sensor.axis.y;\n result.axis.z = -sensor.axis.x;\n return result;\n case FusionAxesAlignmentPZPXPY:\n result.axis.x = +sensor.axis.z;\n result.axis.y = +sensor.axis.x;\n result.axis.z = +sensor.axis.y;\n return result;\n case FusionAxesAlignmentPZNYPX:\n result.axis.x = +sensor.axis.z;\n result.axis.y = -sensor.axis.y;\n result.axis.z = +sensor.axis.x;\n return result;\n case FusionAxesAlignmentPZNXNY:\n result.axis.x = +sensor.axis.z;\n result.axis.y = -sensor.axis.x;\n result.axis.z = -sensor.axis.y;\n return result;\n case FusionAxesAlignmentNZPYPX:\n result.axis.x = -sensor.axis.z;\n result.axis.y = +sensor.axis.y;\n result.axis.z = +sensor.axis.x;\n return result;\n case FusionAxesAlignmentNZNXPY:\n result.axis.x = -sensor.axis.z;\n result.axis.y = -sensor.axis.x;\n result.axis.z = +sensor.axis.y;\n return result;\n case FusionAxesAlignmentNZNYNX:\n result.axis.x = -sensor.axis.z;\n result.axis.y = -sensor.axis.y;\n result.axis.z = -sensor.axis.x;\n return result;\n case FusionAxesAlignmentNZPXNY:\n result.axis.x = -sensor.axis.z;\n result.axis.y = +sensor.axis.x;\n result.axis.z = -sensor.axis.y;\n return result;\n }\n return sensor;\n}\n\n#endif\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionCalibration.h", "content": "/**\n * @file FusionCalibration.h\n * @author Seb Madgwick\n * @brief Gyroscope, accelerometer, and magnetometer calibration models.\n */\n\n#ifndef FUSION_CALIBRATION_H\n#define FUSION_CALIBRATION_H\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include \"FusionMath.h\"\n\n//------------------------------------------------------------------------------\n// Inline functions\n\n/**\n * @brief Gyroscope and accelerometer calibration model.\n * @param uncalibrated Uncalibrated measurement.\n * @param misalignment Misalignment matrix.\n * @param sensitivity Sensitivity.\n * @param offset Offset.\n * @return Calibrated measurement.\n */\nstatic inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) {\n return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));\n}\n\n/**\n * @brief Magnetometer calibration model.\n * @param uncalibrated Uncalibrated measurement.\n * @param softIronMatrix Soft-iron matrix.\n * @param hardIronOffset Hard-iron offset.\n * @return Calibrated measurement.\n */\nstatic inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) {\n return FusionVectorSubtract(FusionMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronOffset);\n}\n\n#endif\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionCompass.c", "content": "/**\n * @file FusionCompass.c\n * @author Seb Madgwick\n * @brief Tilt-compensated compass to calculate an heading relative to magnetic\n * north using accelerometer and magnetometer measurements.\n */\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include \"FusionCompass.h\"\n#include // atan2f\n\n//------------------------------------------------------------------------------\n// Functions\n\n/**\n * @brief Calculates the heading relative to magnetic north.\n * @param accelerometer Accelerometer measurement in any calibrated units.\n * @param magnetometer Magnetometer measurement in any calibrated units.\n * @return Heading angle in degrees.\n */\nfloat FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer) {\n\n // Compute direction of magnetic west (Earth's y axis)\n const FusionVector magneticWest = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));\n\n // Compute direction of magnetic north (Earth's x axis)\n const FusionVector magneticNorth = FusionVectorNormalise(FusionVectorCrossProduct(magneticWest, accelerometer));\n\n // Calculate angular heading relative to magnetic north\n return FusionRadiansToDegrees(atan2f(magneticWest.axis.x, magneticNorth.axis.x));\n}\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionCompass.h", "content": "/**\n * @file FusionCompass.h\n * @author Seb Madgwick\n * @brief Tilt-compensated compass to calculate an heading relative to magnetic\n * north using accelerometer and magnetometer measurements.\n */\n\n#ifndef FUSION_COMPASS_H\n#define FUSION_COMPASS_H\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include \"FusionMath.h\"\n\n//------------------------------------------------------------------------------\n// Function declarations\n\nfloat FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer);\n\n#endif\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionMath.h", "content": "/**\n * @file FusionMath.h\n * @author Seb Madgwick\n * @brief Math library.\n */\n\n#ifndef FUSION_MATH_H\n#define FUSION_MATH_H\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include // M_PI, sqrtf, atan2f, asinf\n#include \n#include \n\n//------------------------------------------------------------------------------\n// Definitions\n\n/**\n * @brief 3D vector.\n */\ntypedef union {\n float array[3];\n\n struct {\n float x;\n float y;\n float z;\n } axis;\n} FusionVector;\n\n/**\n * @brief Quaternion.\n */\ntypedef union {\n float array[4];\n\n struct {\n float w;\n float x;\n float y;\n float z;\n } element;\n} FusionQuaternion;\n\n/**\n * @brief 3x3 matrix in row-major order.\n * See http://en.wikipedia.org/wiki/Row-major_order\n */\ntypedef union {\n float array[3][3];\n\n struct {\n float xx;\n float xy;\n float xz;\n float yx;\n float yy;\n float yz;\n float zx;\n float zy;\n float zz;\n } element;\n} FusionMatrix;\n\n/**\n * @brief Euler angles. Roll, pitch, and yaw correspond to rotations around\n * X, Y, and Z respectively.\n */\ntypedef union {\n float array[3];\n\n struct {\n float roll;\n float pitch;\n float yaw;\n } angle;\n} FusionEuler;\n\n/**\n * @brief Vector of zeros.\n */\n#define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} })\n\n/**\n * @brief Vector of ones.\n */\n#define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} })\n\n/**\n * @brief Identity quaternion.\n */\n#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} })\n\n/**\n * @brief Identity matrix.\n */\n#define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} })\n\n/**\n * @brief Euler angles of zero.\n */\n#define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} })\n\n/**\n * @brief Pi. May not be defined in math.h.\n */\n#ifndef M_PI\n#define M_PI (3.14159265358979323846)\n#endif\n\n/**\n * @brief Include this definition or add as a preprocessor definition to use\n * normal square root operations.\n */\n//#define FUSION_USE_NORMAL_SQRT\n\n//------------------------------------------------------------------------------\n// Inline functions - Degrees and radians conversion\n\n/**\n * @brief Converts degrees to radians.\n * @param degrees Degrees.\n * @return Radians.\n */\nstatic inline float FusionDegreesToRadians(const float degrees) {\n return degrees * ((float) M_PI / 180.0f);\n}\n\n/**\n * @brief Converts radians to degrees.\n * @param radians Radians.\n * @return Degrees.\n */\nstatic inline float FusionRadiansToDegrees(const float radians) {\n return radians * (180.0f / (float) M_PI);\n}\n\n//------------------------------------------------------------------------------\n// Inline functions - Arc sine\n\n/**\n * @brief Returns the arc sine of the value.\n * @param value Value.\n * @return Arc sine of the value.\n */\nstatic inline float FusionAsin(const float value) {\n if (value <= -1.0f) {\n return (float) M_PI / -2.0f;\n }\n if (value >= 1.0f) {\n return (float) M_PI / 2.0f;\n }\n return asinf(value);\n}\n\n//------------------------------------------------------------------------------\n// Inline functions - Fast inverse square root\n\n#ifndef FUSION_USE_NORMAL_SQRT\n\n/**\n * @brief Calculates the reciprocal of the square root.\n * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/\n * @param x Operand.\n * @return Reciprocal of the square root of x.\n */\nstatic inline float FusionFastInverseSqrt(const float x) {\n\n typedef union {\n float f;\n int32_t i;\n } Union32;\n\n Union32 union32 = {.f = x};\n union32.i = 0x5F1F1412 - (union32.i >> 1);\n return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);\n}\n\n#endif\n\n//------------------------------------------------------------------------------\n// Inline functions - Vector operations\n\n/**\n * @brief Returns true if the vector is zero.\n * @param vector Vector.\n * @return True if the vector is zero.\n */\nstatic inline bool FusionVectorIsZero(const FusionVector vector) {\n return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);\n}\n\n/**\n * @brief Returns the sum of two vectors.\n * @param vectorA Vector A.\n * @param vectorB Vector B.\n * @return Sum of two vectors.\n */\nstatic inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) {\n FusionVector result;\n result.axis.x = vectorA.axis.x + vectorB.axis.x;\n result.axis.y = vectorA.axis.y + vectorB.axis.y;\n result.axis.z = vectorA.axis.z + vectorB.axis.z;\n return result;\n}\n\n/**\n * @brief Returns vector B subtracted from vector A.\n * @param vectorA Vector A.\n * @param vectorB Vector B.\n * @return Vector B subtracted from vector A.\n */\nstatic inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) {\n FusionVector result;\n result.axis.x = vectorA.axis.x - vectorB.axis.x;\n result.axis.y = vectorA.axis.y - vectorB.axis.y;\n result.axis.z = vectorA.axis.z - vectorB.axis.z;\n return result;\n}\n\n/**\n * @brief Returns the sum of the elements.\n * @param vector Vector.\n * @return Sum of the elements.\n */\nstatic inline float FusionVectorSum(const FusionVector vector) {\n return vector.axis.x + vector.axis.y + vector.axis.z;\n}\n\n/**\n * @brief Returns the multiplication of a vector by a scalar.\n * @param vector Vector.\n * @param scalar Scalar.\n * @return Multiplication of a vector by a scalar.\n */\nstatic inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) {\n FusionVector result;\n result.axis.x = vector.axis.x * scalar;\n result.axis.y = vector.axis.y * scalar;\n result.axis.z = vector.axis.z * scalar;\n return result;\n}\n\n/**\n * @brief Calculates the Hadamard product (element-wise multiplication).\n * @param vectorA Vector A.\n * @param vectorB Vector B.\n * @return Hadamard product.\n */\nstatic inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) {\n FusionVector result;\n result.axis.x = vectorA.axis.x * vectorB.axis.x;\n result.axis.y = vectorA.axis.y * vectorB.axis.y;\n result.axis.z = vectorA.axis.z * vectorB.axis.z;\n return result;\n}\n\n/**\n * @brief Returns the cross product.\n * @param vectorA Vector A.\n * @param vectorB Vector B.\n * @return Cross product.\n */\nstatic inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) {\n#define A vectorA.axis\n#define B vectorB.axis\n FusionVector result;\n result.axis.x = A.y * B.z - A.z * B.y;\n result.axis.y = A.z * B.x - A.x * B.z;\n result.axis.z = A.x * B.y - A.y * B.x;\n return result;\n#undef A\n#undef B\n}\n\n/**\n * @brief Returns the vector magnitude squared.\n * @param vector Vector.\n * @return Vector magnitude squared.\n */\nstatic inline float FusionVectorMagnitudeSquared(const FusionVector vector) {\n return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));\n}\n\n/**\n * @brief Returns the vector magnitude.\n * @param vector Vector.\n * @return Vector magnitude.\n */\nstatic inline float FusionVectorMagnitude(const FusionVector vector) {\n return sqrtf(FusionVectorMagnitudeSquared(vector));\n}\n\n/**\n * @brief Returns the normalised vector.\n * @param vector Vector.\n * @return Normalised vector.\n */\nstatic inline FusionVector FusionVectorNormalise(const FusionVector vector) {\n#ifdef FUSION_USE_NORMAL_SQRT\n const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));\n#else\n const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));\n#endif\n return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);\n}\n\n//------------------------------------------------------------------------------\n// Inline functions - Quaternion operations\n\n/**\n * @brief Returns the sum of two quaternions.\n * @param quaternionA Quaternion A.\n * @param quaternionB Quaternion B.\n * @return Sum of two quaternions.\n */\nstatic inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {\n FusionQuaternion result;\n result.element.w = quaternionA.element.w + quaternionB.element.w;\n result.element.x = quaternionA.element.x + quaternionB.element.x;\n result.element.y = quaternionA.element.y + quaternionB.element.y;\n result.element.z = quaternionA.element.z + quaternionB.element.z;\n return result;\n}\n\n/**\n * @brief Returns the multiplication of two quaternions.\n * @param quaternionA Quaternion A (to be post-multiplied).\n * @param quaternionB Quaternion B (to be pre-multiplied).\n * @return Multiplication of two quaternions.\n */\nstatic inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {\n#define A quaternionA.element\n#define B quaternionB.element\n FusionQuaternion result;\n result.element.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z;\n result.element.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y;\n result.element.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x;\n result.element.z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w;\n return result;\n#undef A\n#undef B\n}\n\n/**\n * @brief Returns the multiplication of a quaternion with a vector. This is a\n * normal quaternion multiplication where the vector is treated a\n * quaternion with a W element value of zero. The quaternion is post-\n * multiplied by the vector.\n * @param quaternion Quaternion.\n * @param vector Vector.\n * @return Multiplication of a quaternion with a vector.\n */\nstatic inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) {\n#define Q quaternion.element\n#define V vector.axis\n FusionQuaternion result;\n result.element.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z;\n result.element.x = Q.w * V.x + Q.y * V.z - Q.z * V.y;\n result.element.y = Q.w * V.y - Q.x * V.z + Q.z * V.x;\n result.element.z = Q.w * V.z + Q.x * V.y - Q.y * V.x;\n return result;\n#undef Q\n#undef V\n}\n\n/**\n * @brief Returns the normalised quaternion.\n * @param quaternion Quaternion.\n * @return Normalised quaternion.\n */\nstatic inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) {\n#define Q quaternion.element\n#ifdef FUSION_USE_NORMAL_SQRT\n const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);\n#else\n const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);\n#endif\n FusionQuaternion normalisedQuaternion;\n normalisedQuaternion.element.w = Q.w * magnitudeReciprocal;\n normalisedQuaternion.element.x = Q.x * magnitudeReciprocal;\n normalisedQuaternion.element.y = Q.y * magnitudeReciprocal;\n normalisedQuaternion.element.z = Q.z * magnitudeReciprocal;\n return normalisedQuaternion;\n#undef Q\n}\n\n//------------------------------------------------------------------------------\n// Inline functions - Matrix operations\n\n/**\n * @brief Returns the multiplication of a matrix with a vector.\n * @param matrix Matrix.\n * @param vector Vector.\n * @return Multiplication of a matrix with a vector.\n */\nstatic inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) {\n#define R matrix.element\n FusionVector result;\n result.axis.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z;\n result.axis.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z;\n result.axis.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z;\n return result;\n#undef R\n}\n\n//------------------------------------------------------------------------------\n// Inline functions - Conversion operations\n\n/**\n * @brief Converts a quaternion to a rotation matrix.\n * @param quaternion Quaternion.\n * @return Rotation matrix.\n */\nstatic inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) {\n#define Q quaternion.element\n const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations\n const float qwqx = Q.w * Q.x;\n const float qwqy = Q.w * Q.y;\n const float qwqz = Q.w * Q.z;\n const float qxqy = Q.x * Q.y;\n const float qxqz = Q.x * Q.z;\n const float qyqz = Q.y * Q.z;\n FusionMatrix matrix;\n matrix.element.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x);\n matrix.element.xy = 2.0f * (qxqy - qwqz);\n matrix.element.xz = 2.0f * (qxqz + qwqy);\n matrix.element.yx = 2.0f * (qxqy + qwqz);\n matrix.element.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y);\n matrix.element.yz = 2.0f * (qyqz - qwqx);\n matrix.element.zx = 2.0f * (qxqz - qwqy);\n matrix.element.zy = 2.0f * (qyqz + qwqx);\n matrix.element.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z);\n return matrix;\n#undef Q\n}\n\n/**\n * @brief Converts a quaternion to ZYX Euler angles in degrees.\n * @param quaternion Quaternion.\n * @return Euler angles in degrees.\n */\nstatic inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) {\n#define Q quaternion.element\n const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations\n FusionEuler euler;\n euler.angle.roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x));\n euler.angle.pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x)));\n euler.angle.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z));\n return euler;\n#undef Q\n}\n\n#endif\n\n//------------------------------------------------------------------------------\n// End of file" }, { "path": "include/Fusion/FusionOffset.c", "content": "/**\n * @file FusionOffset.c\n * @author Seb Madgwick\n * @brief Gyroscope offset correction algorithm for run-time calibration of the\n * gyroscope offset.\n */\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include \"FusionOffset.h\"\n#include // fabs\n\n//------------------------------------------------------------------------------\n// Definitions\n\n/**\n * @brief Cutoff frequency in Hz.\n */\n#define CUTOFF_FREQUENCY (0.02f)\n\n/**\n * @brief Timeout in seconds.\n */\n#define TIMEOUT (5)\n\n/**\n * @brief Threshold in degrees per second.\n */\n#define THRESHOLD (3.0f)\n\n//------------------------------------------------------------------------------\n// Functions\n\n/**\n * @brief Initialises the gyroscope offset algorithm.\n * @param offset Gyroscope offset algorithm structure.\n * @param sampleRate Sample rate in Hz.\n */\nvoid FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) {\n offset->filterCoefficient = 2.0f * (float) M_PI * CUTOFF_FREQUENCY * (1.0f / (float) sampleRate);\n offset->timeout = TIMEOUT * sampleRate;\n offset->timer = 0;\n offset->gyroscopeOffset = FUSION_VECTOR_ZERO;\n}\n\n/**\n * @brief Updates the gyroscope offset algorithm and returns the corrected\n * gyroscope measurement.\n * @param offset Gyroscope offset algorithm structure.\n * @param gyroscope Gyroscope measurement in degrees per second.\n * @return Corrected gyroscope measurement in degrees per second.\n */\nFusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) {\n\n // Subtract offset from gyroscope measurement\n gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);\n\n // Reset timer if gyroscope not stationary\n if ((fabs(gyroscope.axis.x) > THRESHOLD) || (fabs(gyroscope.axis.y) > THRESHOLD) || (fabs(gyroscope.axis.z) > THRESHOLD)) {\n offset->timer = 0;\n return gyroscope;\n }\n\n // Increment timer while gyroscope stationary\n if (offset->timer < offset->timeout) {\n offset->timer++;\n return gyroscope;\n }\n\n // Adjust offset if timer has elapsed\n offset->gyroscopeOffset = FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient));\n return gyroscope;\n}\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Fusion/FusionOffset.h", "content": "/**\n * @file FusionOffset.h\n * @author Seb Madgwick\n * @brief Gyroscope offset correction algorithm for run-time calibration of the\n * gyroscope offset.\n */\n\n#ifndef FUSION_OFFSET_H\n#define FUSION_OFFSET_H\n\n//------------------------------------------------------------------------------\n// Includes\n\n#include \"FusionMath.h\"\n\n//------------------------------------------------------------------------------\n// Definitions\n\n/**\n * @brief Gyroscope offset algorithm structure. Structure members are used\n * internally and must not be accessed by the application.\n */\ntypedef struct {\n float filterCoefficient;\n unsigned int timeout;\n unsigned int timer;\n FusionVector gyroscopeOffset;\n} FusionOffset;\n\n//------------------------------------------------------------------------------\n// Function declarations\n\nvoid FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate);\n\nFusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope);\n\n#endif\n\n//------------------------------------------------------------------------------\n// End of file\n" }, { "path": "include/Gril_Calib/Gril_Calib.cpp", "content": "#include \"Gril_Calib.h\"\n\n/*\nDescription: Gril-Calib (Heavily adapted from LI-Init by Fangcheng Zhu)\nModifier : Taeyoung Kim (https://github.com/Taeyoung96)\n*/\n\nGril_Calib::Gril_Calib()\n : time_delay_IMU_wtr_Lidar(0.0), time_lag_1(0.0), time_lag_2(0.0), lag_IMU_wtr_Lidar(0) {\n fout_LiDAR_meas.open(FILE_DIR(\"LiDAR_meas.txt\"), ios::out);\n fout_IMU_meas.open(FILE_DIR(\"IMU_meas.txt\"), ios::out);\n fout_before_filt_IMU.open(FILE_DIR(\"IMU_before_filter.txt\"), ios::out);\n fout_before_filt_Lidar.open(FILE_DIR(\"Lidar_before_filter.txt\"), ios::out);\n fout_acc_cost.open(FILE_DIR(\"acc_cost.txt\"), ios::out);\n fout_after_rot.open(FILE_DIR(\"Lidar_omg_after_rot.txt\"), ios::out);\n\n fout_LiDAR_ang_vel.open(FILE_DIR(\"Lidar_ang_vel.txt\"), ios::out);\n fout_IMU_ang_vel.open(FILE_DIR(\"IMU_ang_vel.txt\"), ios::out);\n fout_Jacob_trans.open(FILE_DIR(\"Jacob_trans.txt\"), ios::out);\n\n fout_LiDAR_meas_after.open(FILE_DIR(\"LiDAR_meas_after.txt\"), ios::out);\n\n \n data_accum_length = 300;\n\n trans_IL_x = 0.0;\n trans_IL_y = 0.0;\n trans_IL_z = 0.0;\n bound_th = 0.1; \n set_boundary = false;\n\n Rot_Grav_wrt_Init_Lidar = Eye3d;\n Trans_Lidar_wrt_IMU = Zero3d;\n Rot_Lidar_wrt_IMU = Eye3d;\n gyro_bias = Zero3d;\n acc_bias = Zero3d;\n}\n\nGril_Calib::~Gril_Calib() = default;\n\nvoid Gril_Calib::set_IMU_state(const deque &IMU_states) {\n IMU_state_group.assign(IMU_states.begin(), IMU_states.end() - 1);\n}\n\nvoid Gril_Calib::set_Lidar_state(const deque &Lidar_states) {\n Lidar_state_group.assign(Lidar_states.begin(), Lidar_states.end() - 1);\n}\n\nvoid Gril_Calib::set_states_2nd_filter(const deque &IMU_states, const deque &Lidar_states) {\n for (int i = 0; i < IMU_state_group.size(); i++) {\n IMU_state_group[i].ang_acc = IMU_states[i].ang_acc;\n Lidar_state_group[i].ang_acc = Lidar_states[i].ang_acc;\n Lidar_state_group[i].linear_acc = Lidar_states[i].linear_acc;\n }\n}\n\nvoid Gril_Calib::fout_before_filter() {\n for (auto it_IMU = IMU_state_group.begin(); it_IMU != IMU_state_group.end() - 1; it_IMU++) {\n fout_before_filt_IMU << setprecision(15) << it_IMU->ang_vel.transpose() << \" \" << it_IMU->ang_vel.norm() << \" \"\n << it_IMU->linear_acc.transpose() << \" \" << it_IMU->timeStamp << endl;\n }\n for (auto it = Lidar_state_group.begin(); it != Lidar_state_group.end() - 1; it++) {\n fout_before_filt_Lidar << setprecision(15) << it->ang_vel.transpose() << \" \" << it->ang_vel.norm() << \" \"\n << it->timeStamp << endl;\n }\n}\n\nvoid Gril_Calib::fout_check_lidar() {\n auto it_Lidar_state = Lidar_state_group.begin() + 1;\n for (; it_Lidar_state != Lidar_state_group.end() - 2; it_Lidar_state++) {\n fout_LiDAR_meas_after << setprecision(12) << it_Lidar_state->ang_vel.transpose() << \" \"\n << it_Lidar_state->ang_vel.norm()\n << \" \" <<\n it_Lidar_state->linear_acc.transpose() << \" \"\n << it_Lidar_state->ang_acc.transpose()\n << \" \" << it_Lidar_state->timeStamp << endl;\n }\n}\n\n\nvoid Gril_Calib::push_ALL_IMU_CalibState(const sensor_msgs::Imu::ConstPtr &msg, const double &mean_acc_norm) {\n CalibState IMUstate;\n double invert = -1.0;\n IMUstate.ang_vel = V3D(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);\n IMUstate.linear_acc =\n V3D(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z) / mean_acc_norm *\n G_m_s2;\n \n IMUstate.timeStamp = msg->header.stamp.toSec();\n IMU_state_group_ALL.push_back(IMUstate);\n}\n\nvoid Gril_Calib::push_IMU_CalibState(const V3D &omg, const V3D &acc, const double ×tamp) {\n CalibState IMUstate;\n IMUstate.ang_vel = omg;\n IMUstate.linear_acc = acc;\n IMUstate.timeStamp = timestamp;\n IMU_state_group.push_back(IMUstate);\n}\n\nvoid Gril_Calib::push_Lidar_CalibState(const M3D &rot, const V3D &pos, const V3D &omg, const V3D &linear_vel, const double ×tamp) {\n CalibState Lidarstate;\n Lidarstate.rot_end = rot;\n Lidarstate.pos_end = pos;\n Lidarstate.ang_vel = omg;\n Lidarstate.linear_vel = linear_vel;\n Lidarstate.timeStamp = timestamp;\n Lidar_state_group.push_back(Lidarstate);\n}\n\nvoid Gril_Calib::push_Plane_Constraint(const Eigen::Quaterniond &q_lidar, const Eigen::Quaterniond &q_imu, const V3D &normal_lidar,\n const double &distance_lidar) {\n Lidar_wrt_ground_group.push_back(q_lidar);\n IMU_wrt_ground_group.push_back(q_imu);\n normal_vector_wrt_lidar_group.push_back(normal_lidar);\n distance_Lidar_wrt_ground_group.push_back(distance_lidar);\n}\n\nvoid Gril_Calib::downsample_interpolate_IMU(const double &move_start_time) {\n\n while (IMU_state_group_ALL.front().timeStamp < move_start_time - 3.0)\n IMU_state_group_ALL.pop_front();\n while (Lidar_state_group.front().timeStamp < move_start_time - 3.0)\n Lidar_state_group.pop_front();\n\n // Original IMU measurements\n deque IMU_states_all_origin;\n IMU_states_all_origin.assign(IMU_state_group_ALL.begin(), IMU_state_group_ALL.end() - 1);\n\n // Mean filter to attenuate noise\n int mean_filt_size = 3;\n for (int i = mean_filt_size; i < IMU_state_group_ALL.size() - mean_filt_size; i++) {\n V3D acc_real = Zero3d;\n for (int k = -mean_filt_size; k < mean_filt_size + 1; k++)\n acc_real += (IMU_states_all_origin[i + k].linear_acc - acc_real) / (k + mean_filt_size + 1);\n IMU_state_group_ALL[i].linear_acc = acc_real;\n }\n\n // Down-sample and interpolation，Fig.4 in the paper\n for (int i = 0; i < Lidar_state_group.size(); i++) {\n for (int j = 1; j < IMU_state_group_ALL.size(); j++) {\n if (IMU_state_group_ALL[j - 1].timeStamp <= Lidar_state_group[i].timeStamp\n && IMU_state_group_ALL[j].timeStamp > Lidar_state_group[i].timeStamp) {\n CalibState IMU_state_interpolation;\n double delta_t = IMU_state_group_ALL[j].timeStamp - IMU_state_group_ALL[j - 1].timeStamp;\n double delta_t_right = IMU_state_group_ALL[j].timeStamp - Lidar_state_group[i].timeStamp;\n double s = delta_t_right / delta_t;\n\n IMU_state_interpolation.ang_vel = s * IMU_state_group_ALL[j - 1].ang_vel +\n (1 - s) * IMU_state_group_ALL[j].ang_vel;\n\n IMU_state_interpolation.linear_acc = s * IMU_state_group_ALL[j - 1].linear_acc +\n (1 - s) * IMU_state_group_ALL[j].linear_acc;\n push_IMU_CalibState(IMU_state_interpolation.ang_vel, IMU_state_interpolation.linear_acc,\n Lidar_state_group[i].timeStamp);\n break;\n }\n }\n }\n\n}\n\n// Calculates the angular acceleration of the IMU, LiDAR, and linear acceleration.\nvoid Gril_Calib::central_diff() {\n auto it_IMU_state = IMU_state_group.begin() + 1;\n for (; it_IMU_state != IMU_state_group.end() - 2; it_IMU_state++) {\n auto last_imu = it_IMU_state - 1;\n auto next_imu = it_IMU_state + 1;\n double dt_imu = next_imu->timeStamp - last_imu->timeStamp;\n it_IMU_state->ang_acc =\n (next_imu->ang_vel - last_imu->ang_vel) / dt_imu;\n fout_IMU_meas << setprecision(12) << it_IMU_state->ang_vel.transpose() << \" \" << it_IMU_state->ang_vel.norm()\n << \" \"\n <<\n it_IMU_state->linear_acc.transpose() << \" \" << it_IMU_state->ang_acc.transpose() << \" \"\n << it_IMU_state->timeStamp << endl;\n }\n\n auto it_Lidar_state = Lidar_state_group.begin() + 1;\n for (; it_Lidar_state != Lidar_state_group.end() - 2; it_Lidar_state++) {\n auto last_lidar = it_Lidar_state - 1;\n auto next_lidar = it_Lidar_state + 1;\n double dt_lidar = next_lidar->timeStamp - last_lidar->timeStamp;\n it_Lidar_state->ang_acc =\n (next_lidar->ang_vel - last_lidar->ang_vel) / dt_lidar;\n it_Lidar_state->linear_acc =\n (next_lidar->linear_vel - last_lidar->linear_vel) / dt_lidar;\n\n fout_LiDAR_meas << setprecision(12) << it_Lidar_state->ang_vel.transpose() << \" \"\n << it_Lidar_state->ang_vel.norm()\n << \" \" <<\n (it_Lidar_state->linear_acc - STD_GRAV).transpose() << \" \"\n << it_Lidar_state->ang_acc.transpose()\n << \" \" << it_Lidar_state->timeStamp << endl;\n }\n}\n\n// Temporal calibration by Cross-Correlation : calculate time_lag_1\nvoid Gril_Calib::xcorr_temporal_init(const double &odom_freq) {\n int N = IMU_state_group.size();\n //Calculate mean value of IMU and LiDAR angular velocity\n double mean_IMU_ang_vel = 0, mean_LiDAR_ang_vel = 0;\n for (int i = 0; i < N; i++) {\n mean_IMU_ang_vel += (IMU_state_group[i].ang_vel.norm() - mean_IMU_ang_vel) / (i + 1);\n mean_LiDAR_ang_vel += (Lidar_state_group[i].ang_vel.norm() - mean_LiDAR_ang_vel) / (i + 1);\n }\n\n //Calculate zero-centered cross correlation\n double max_corr = -DBL_MAX;\n for (int lag = -N + 1; lag < N; lag++) {\n double corr = 0;\n int cnt = 0;\n for (int i = 0; i < N; i++) {\n int j = i + lag;\n if (j < 0 || j > N - 1)\n continue;\n else {\n cnt++;\n corr += (IMU_state_group[i].ang_vel.norm() - mean_IMU_ang_vel) *\n (Lidar_state_group[j].ang_vel.norm() - mean_LiDAR_ang_vel); // Zero-centered cross correlation\n }\n }\n\n if (corr > max_corr) {\n max_corr = corr;\n lag_IMU_wtr_Lidar = -lag;\n }\n }\n\n time_lag_1 = lag_IMU_wtr_Lidar / odom_freq;\n cout << \"Max Cross-correlation: IMU lag wtr Lidar : \" << -lag_IMU_wtr_Lidar << endl;\n cout << \"Time lag 1: IMU lag wtr Lidar : \" << time_lag_1 << endl;\n}\n\nvoid Gril_Calib::IMU_time_compensate(const double &lag_time, const bool &is_discard) {\n if (is_discard) {\n // Discard first 10 Lidar estimations and corresponding IMU measurements due to long time interval\n int i = 0;\n while (i < 10) {\n Lidar_state_group.pop_front();\n IMU_state_group.pop_front();\n i++;\n }\n }\n\n auto it_IMU_state = IMU_state_group.begin();\n for (; it_IMU_state != IMU_state_group.end() - 1; it_IMU_state++) {\n it_IMU_state->timeStamp = it_IMU_state->timeStamp - lag_time;\n }\n\n while (Lidar_state_group.front().timeStamp < IMU_state_group.front().timeStamp)\n Lidar_state_group.pop_front(); \n\n while (Lidar_state_group.front().timeStamp > IMU_state_group[1].timeStamp)\n IMU_state_group.pop_front(); \n\n // align the size of two sequences\n while (IMU_state_group.size() > Lidar_state_group.size())\n IMU_state_group.pop_back();\n while (IMU_state_group.size() < Lidar_state_group.size())\n Lidar_state_group.pop_back();\n}\n\n\nvoid Gril_Calib::cut_sequence_tail() {\n for (int i = 0; i < 20; ++i) {\n Lidar_state_group.pop_back();\n IMU_state_group.pop_back();\n }\n while (Lidar_state_group.front().timeStamp < IMU_state_group.front().timeStamp)\n Lidar_state_group.pop_front();\n while (Lidar_state_group.front().timeStamp > IMU_state_group[1].timeStamp)\n IMU_state_group.pop_front();\n\n //Align the size of two sequences\n while (IMU_state_group.size() > Lidar_state_group.size())\n IMU_state_group.pop_back();\n while (IMU_state_group.size() < Lidar_state_group.size())\n Lidar_state_group.pop_back();\n}\n\nvoid Gril_Calib::acc_interpolate() {\n //Interpolation to get acc_I(t_L)\n for (int i = 1; i < Lidar_state_group.size() - 1; i++) {\n double deltaT = Lidar_state_group[i].timeStamp - IMU_state_group[i].timeStamp;\n if (deltaT > 0) {\n double DeltaT = IMU_state_group[i + 1].timeStamp - IMU_state_group[i].timeStamp;\n double s = deltaT / DeltaT;\n IMU_state_group[i].linear_acc = s * IMU_state_group[i + 1].linear_acc +\n (1 - s) * IMU_state_group[i].linear_acc;\n IMU_state_group[i].timeStamp += deltaT;\n } else {\n double DeltaT = IMU_state_group[i].timeStamp - IMU_state_group[i - 1].timeStamp;\n double s = -deltaT / DeltaT;\n IMU_state_group[i].linear_acc = s * IMU_state_group[i - 1].linear_acc +\n (1 - s) * IMU_state_group[i].linear_acc;\n IMU_state_group[i].timeStamp += deltaT;\n }\n }\n}\n\n// Butterworth filter (Low-pass filter)\nvoid Gril_Calib::Butter_filt(const deque &signal_in, deque &signal_out) {\n Gril_Calib::Butterworth butter;\n butter.extend_num = 10 * (butter.Coeff_size - 1);\n auto it_front = signal_in.begin() + butter.extend_num;\n auto it_back = signal_in.end() - 1 - butter.extend_num;\n\n deque extend_front;\n deque extend_back;\n\n for (int idx = 0; idx < butter.extend_num; idx++) {\n extend_front.push_back(*it_front);\n extend_back.push_front(*it_back);\n it_front--;\n it_back++;\n }\n\n deque sig_extended(signal_in);\n while (!extend_front.empty()) {\n sig_extended.push_front(extend_front.back());\n extend_front.pop_back();\n }\n while (!extend_back.empty()) {\n sig_extended.push_back(extend_back.front());\n extend_back.pop_front();\n }\n\n deque sig_out(sig_extended);\n // One-direction Butterworth filter Starts (all states)\n for (int i = butter.Coeff_size; i < sig_extended.size() - butter.extend_num; i++) {\n CalibState temp_state;\n for (int j = 0; j < butter.Coeff_size; j++) {\n auto it_sig_ext = *(sig_extended.begin() + i - j);\n temp_state += it_sig_ext * butter.Coeff_b[j];\n }\n for (int jj = 1; jj < butter.Coeff_size; jj++) {\n auto it_sig_out = *(sig_out.begin() + i - jj);\n temp_state -= it_sig_out * butter.Coeff_a[jj];\n }\n sig_out[i] = temp_state;\n }\n\n for (auto it = sig_out.begin() + butter.extend_num; it != sig_out.end() - butter.extend_num; it++) {\n signal_out.push_back(*it);\n }\n}\n\n// zero phase low-pass filter //\nvoid Gril_Calib::zero_phase_filt(const deque &signal_in, deque &signal_out) {\n deque sig_out1;\n Butter_filt(signal_in, sig_out1); // signal_in에 대해 Butterworth filter를 적용한 결과를 sig_out1에 저장\n\n deque sig_rev(sig_out1);\n reverse(sig_rev.begin(), sig_rev.end()); //Reverse the elements\n\n Butter_filt(sig_rev, signal_out);\n reverse(signal_out.begin(), signal_out.end()); //Reverse the elements\n}\n\n\n// To obtain a rough initial value of rotation matrix //\nvoid Gril_Calib::solve_Rotation_only() {\n double R_LI_quat[4];\n\n R_LI_quat[0] = 1;\n R_LI_quat[1] = 0;\n R_LI_quat[2] = 0;\n R_LI_quat[3] = 0;\n\n ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization();\n ceres::Problem problem_rot;\n problem_rot.AddParameterBlock(R_LI_quat, 4, quatParam);\n\n for (int i = 0; i < IMU_state_group.size(); i++) {\n M3D Lidar_angvel_skew;\n Lidar_angvel_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_vel);\n problem_rot.AddResidualBlock(Angular_Vel_Cost_only_Rot::Create(IMU_state_group[i].ang_vel,\n Lidar_state_group[i].ang_vel),\n nullptr,\n R_LI_quat);\n }\n\n ceres::Solver::Options options_quat;\n ceres::Solver::Summary summary_quat;\n ceres::Solve(options_quat, &problem_rot, &summary_quat);\n Eigen::Quaterniond q_LI(R_LI_quat[0], R_LI_quat[1], R_LI_quat[2], R_LI_quat[3]);\n Rot_Lidar_wrt_IMU = q_LI.matrix(); // LiDAR angulr velocity in IMU frame (from LiDAR to IMU)\n\n}\n\n\n\n// Proposed algorithm (Rotation + Translation)\nvoid Gril_Calib::solve_Rot_Trans_calib(double &timediff_imu_wrt_lidar, const double &imu_sensor_height) {\n\n M3D R_IL_init = Rot_Lidar_wrt_IMU.transpose(); // Initial value of Rotation of IL (from IMU frame to LiDAR frame)\n Eigen::Quaterniond quat(R_IL_init);\n double R_IL_quat[4];\n R_IL_quat[0] = quat.w();\n R_IL_quat[1] = quat.x();\n R_IL_quat[2] = quat.y();\n R_IL_quat[3] = quat.z();\n\n double Trans_IL[3]; // Initial value of Translation of IL (IMU with respect to Lidar) - ceres solver input\n Trans_IL[0] = trans_IL_x;\n Trans_IL[1] = trans_IL_y;\n Trans_IL[2] = trans_IL_z;\n\n double bias_g[3]; // Initial value of gyro bias\n bias_g[0] = 0;\n bias_g[1] = 0;\n bias_g[2] = 0;\n\n double bias_aL[3]; // Initial value of acc bias\n bias_aL[0] = 0;\n bias_aL[1] = 0;\n bias_aL[2] = 0;\n\n double time_lag2 = 0; // Second time lag (IMU wtr Lidar)\n\n // Define problem\n ceres::Problem problem_Ex_calib;\n ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization();\n \n // Define Loss function\n ceres::LossFunction *loss_function_angular = new ceres::CauchyLoss(0.5);\n ceres::ScaledLoss *loss_function_angular_scaled = new ceres::ScaledLoss(loss_function_angular, 0.5, ceres::TAKE_OWNERSHIP);\n\n ceres::LossFunction *loss_function_acc = new ceres::CauchyLoss(0.5);\n ceres::ScaledLoss *scaled_loss_acc = new ceres::ScaledLoss(loss_function_acc, 0.2, ceres::TAKE_OWNERSHIP);\n\n ceres::LossFunction *loss_function_plain_motion = new ceres::HuberLoss(0.5);\n ceres::ScaledLoss *loss_function_plain_motion_scaled = new ceres::ScaledLoss(loss_function_plain_motion, 0.3, ceres::TAKE_OWNERSHIP);\n\n // Add Parameter Block\n problem_Ex_calib.AddParameterBlock(R_IL_quat, 4, quatParam);\n problem_Ex_calib.AddParameterBlock(Trans_IL, 3);\n problem_Ex_calib.AddParameterBlock(bias_g, 3);\n problem_Ex_calib.AddParameterBlock(bias_aL, 3);\n\n //Jacobian of acc_bias, gravity, Translation\n int Jaco_size = 3 * Lidar_state_group.size();\n MatrixXd Jacobian(Jaco_size, 9);\n Jacobian.setZero();\n\n // Jacobian of Translation\n MatrixXd Jaco_Trans(Jaco_size, 3);\n Jaco_Trans.setZero();\n\n // Add Residual Block\n for (int i = 0; i < IMU_state_group.size(); i++) {\n double deltaT = Lidar_state_group[i].timeStamp - IMU_state_group[i].timeStamp;\n \n problem_Ex_calib.AddResidualBlock(Ground_Plane_Cost_IL::Create(Lidar_wrt_ground_group[i],\n IMU_wrt_ground_group[i],\n distance_Lidar_wrt_ground_group[i],\n imu_sensor_height),\n loss_function_plain_motion_scaled,\n R_IL_quat,\n Trans_IL);\n\n problem_Ex_calib.AddResidualBlock(Angular_Vel_IL_Cost::Create(IMU_state_group[i].ang_vel,\n IMU_state_group[i].ang_acc,\n Lidar_state_group[i].ang_vel,\n deltaT),\n loss_function_angular_scaled,\n R_IL_quat,\n bias_g,\n &time_lag2);\n\n problem_Ex_calib.AddResidualBlock(Linear_acc_Rot_Cost_without_Gravity::Create(Lidar_state_group[i],\n IMU_state_group[i].linear_acc,\n Lidar_wrt_ground_group[i]),\n scaled_loss_acc,\n R_IL_quat,\n bias_aL,\n Trans_IL);\n\n Jacobian.block<3, 3>(3 * i, 0) = -Lidar_state_group[i].rot_end;\n Jacobian.block<3, 3>(3 * i, 3) << SKEW_SYM_MATRX(STD_GRAV);\n M3D omg_skew, angacc_skew;\n omg_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_vel);\n angacc_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_acc);\n M3D Jaco_trans_i = -omg_skew * omg_skew - angacc_skew;\n Jaco_Trans.block<3, 3>(3 * i, 0) = Jaco_trans_i;\n Jacobian.block<3, 3>(3 * i, 6) = Jaco_trans_i;\n }\n\n // Set boundary\n for (int index = 0; index < 3; ++index) {\n problem_Ex_calib.SetParameterUpperBound(bias_aL, index, 0.01);\n problem_Ex_calib.SetParameterLowerBound(bias_aL, index, -0.01);\n\n problem_Ex_calib.SetParameterUpperBound(bias_g, index, 0.01);\n problem_Ex_calib.SetParameterLowerBound(bias_g, index, -0.01);\n }\n\n if(set_boundary) {\n for (int index = 0; index < 3; ++index) {\n problem_Ex_calib.SetParameterUpperBound(Trans_IL, index, Trans_IL[index] + bound_th);\n problem_Ex_calib.SetParameterLowerBound(Trans_IL, index, Trans_IL[index] - bound_th);\n }\n }\n \n // Solver options\n ceres::Solver::Options options_Ex_calib;\n options_Ex_calib.num_threads = 1;\n options_Ex_calib.use_explicit_schur_complement = true;\n options_Ex_calib.linear_solver_type = ceres::ITERATIVE_SCHUR;\n options_Ex_calib.preconditioner_type = ceres::SCHUR_JACOBI;\n options_Ex_calib.minimizer_progress_to_stdout = false;\n\n // Solve\n ceres::Solver::Summary summary_Ex_calib; \n ceres::Solve(options_Ex_calib, &problem_Ex_calib, &summary_Ex_calib);\n\n // std::cout << summary_Ex_calib.FullReport() << \"\\n\";\n \n //** Update the result **//\n\n // Rotation matrix\n Eigen::Quaterniond q_IL_final(R_IL_quat[0], R_IL_quat[1], R_IL_quat[2], R_IL_quat[3]); // quaternion from IMU frame to Lidar frame\n Rot_Lidar_wrt_IMU = q_IL_final.matrix().transpose();\n V3D euler_angle = RotMtoEuler(Rot_Lidar_wrt_IMU);\n\n // Translation vector\n V3D Trans_IL_vec(Trans_IL[0], Trans_IL[1], Trans_IL[2]);\n Trans_Lidar_wrt_IMU = -1.0 * Rot_Lidar_wrt_IMU * Trans_IL_vec;\n\n // gravity vector - not used\n M3D R_WLO = Lidar_wrt_ground_group[0].matrix();\n Grav_L0 = R_WLO * STD_GRAV; // gravity in first lidar frame\n\n // bias acc\n V3D bias_a_Lidar(bias_aL[0], bias_aL[1], bias_aL[2]);\n acc_bias = Rot_Lidar_wrt_IMU * bias_a_Lidar;\n\n // bias gyro\n gyro_bias = V3D(bias_g[0], bias_g[1], bias_g[2]);\n\n // time offset\n time_lag_2 = time_lag2;\n time_delay_IMU_wtr_Lidar = time_lag_1 + time_lag_2;\n\n time_offset_result = time_delay_IMU_wtr_Lidar;\n \n //The second temporal compensation\n IMU_time_compensate(get_lag_time_2(), false);\n\n // For debug\n for (int i = 0; i < IMU_state_group.size(); i++) {\n M3D R_GL = Lidar_wrt_ground_group[i].matrix();\n V3D Grav_L = R_GL * STD_GRAV;\n\n V3D acc_I = Lidar_state_group[i].rot_end * Rot_Lidar_wrt_IMU.transpose() * IMU_state_group[i].linear_acc -\n Lidar_state_group[i].rot_end * bias_a_Lidar;\n V3D acc_L = Lidar_state_group[i].linear_acc +\n Lidar_state_group[i].rot_end * Jaco_Trans.block<3, 3>(3 * i, 0) * Trans_IL_vec - Grav_L;\n fout_acc_cost << setprecision(10) << acc_I.transpose() << \" \" << acc_L.transpose() << \" \"\n << IMU_state_group[i].timeStamp << \" \" << Lidar_state_group[i].timeStamp << endl;\n }\n}\n\nvoid Gril_Calib::normalize_acc(deque &signal_in) {\n V3D mean_acc(0, 0, 0);\n\n for (int i = 1; i < 10; i++) {\n mean_acc += (signal_in[i].linear_acc - mean_acc) / i;\n }\n\n for (int i = 0; i < signal_in.size(); i++) {\n signal_in[i].linear_acc = signal_in[i].linear_acc / mean_acc.norm() * G_m_s2;\n }\n}\n\n// Align the size of various states\nvoid Gril_Calib::align_Group(const deque &IMU_states, deque &Lidar_wrt_ground_states, \n deque &IMU_wrt_ground_states, deque &normal_vector_wrt_lidar_group, deque &distance_Lidar_ground_states) {\n\n // Align the size of two sequences\n while(IMU_states.size() < Lidar_wrt_ground_states.size()) {\n Lidar_wrt_ground_states.pop_back();\n IMU_wrt_ground_states.pop_back();\n normal_vector_wrt_lidar_group.pop_back();\n distance_Lidar_ground_states.pop_back();\n }\n\n}\n\nbool Gril_Calib::data_sufficiency_assess(MatrixXd &Jacobian_rot, int &frame_num, V3D &lidar_omg, int &orig_odom_freq,\n int &cut_frame_num, QD &lidar_q, QD &imu_q, double &lidar_estimate_height) {\n //Calculation of Rotation Jacobian\n M3D lidar_omg_skew;\n lidar_omg_skew << SKEW_SYM_MATRX(lidar_omg);\n Jacobian_rot.block<3, 3>(3 * frame_num, 0) = lidar_omg_skew;\n bool data_sufficient = false;\n\n //Give a Data Appraisal every second\n if (frame_num % orig_odom_freq * cut_frame_num == 0) {\n M3D Hessian_rot = Jacobian_rot.transpose() * Jacobian_rot;\n EigenSolver es(Hessian_rot);\n V3D EigenValue = es.eigenvalues().real();\n M3D EigenVec_mat = es.eigenvectors().real();\n\n M3D EigenMatCwise = EigenVec_mat.cwiseProduct(EigenVec_mat);\n std::vector EigenMat_1_col{EigenMatCwise(0, 0), EigenMatCwise(1, 0), EigenMatCwise(2, 0)};\n std::vector EigenMat_2_col{EigenMatCwise(0, 1), EigenMatCwise(1, 1), EigenMatCwise(2, 1)};\n std::vector EigenMat_3_col{EigenMatCwise(0, 2), EigenMatCwise(1, 2), EigenMatCwise(2, 2)};\n\n // Find the maximum value of each column\n int maxPos[3] = {0};\n maxPos[0] = max_element(EigenMat_1_col.begin(), EigenMat_1_col.end()) - EigenMat_1_col.begin();\n maxPos[1] = max_element(EigenMat_2_col.begin(), EigenMat_2_col.end()) - EigenMat_2_col.begin();\n maxPos[2] = max_element(EigenMat_3_col.begin(), EigenMat_3_col.end()) - EigenMat_3_col.begin();\n\n V3D Scaled_Eigen = EigenValue / data_accum_length; // the larger data_accum_length is, the more data is needed\n V3D Rot_percent(Scaled_Eigen[1] * Scaled_Eigen[2],\n Scaled_Eigen[0] * Scaled_Eigen[2],\n Scaled_Eigen[0] * Scaled_Eigen[1]);\n\n int axis[3];\n axis[2] = max_element(maxPos, maxPos + 3) - maxPos;\n axis[0] = min_element(maxPos, maxPos + 3) - maxPos;\n axis[1] = 3 - (axis[0] + axis[2]);\n\n double percentage_x = Rot_percent[axis[0]] < x_accumulate ? Rot_percent[axis[0]] : 1;\n double percentage_y = Rot_percent[axis[1]] < y_accumulate ? Rot_percent[axis[1]] : 1;\n double percentage_z = Rot_percent[axis[2]] < z_accumulate ? Rot_percent[axis[2]] : 1;\n \n clear(); //clear the screen\n printf(\"\\033[3A\\r\");\n\n printProgress(percentage_x, 88);\n printProgress(percentage_y, 89);\n printProgress(percentage_z, 90);\n\n if(verbose){\n M3D R_GL = lidar_q.toRotationMatrix();\n M3D R_GI = imu_q.toRotationMatrix();\n\n printf(BOLDREDPURPLE \"[Rotation matrix Ground to LiDAR (euler)] \" RESET);\n cout << setprecision(4) << RotMtoEuler(R_GL).transpose() * 57.3 << \" deg\" << '\\n';\n \n printf(BOLDREDPURPLE \"[Rotation matrix Ground to IMU (euler)] \" RESET);\n cout << setprecision(4) << RotMtoEuler(R_GI).transpose() * 57.3 << \" deg\" << '\\n';\n\n printf(BOLDREDPURPLE \"[Estimated LiDAR sensor height] : \" RESET);\n cout << setprecision(4) << lidar_estimate_height << \" m\" << '\\n';\n }\n \n\n fflush(stdout);\n if (Rot_percent[axis[0]] > x_accumulate && Rot_percent[axis[1]] > y_accumulate && Rot_percent[axis[2]] > z_accumulate) {\n printf(BOLDCYAN \"[calibration] Data accumulation finished, Lidar IMU calibration begins.\\n\\n\" RESET);\n printf(BOLDBLUE\"============================================================ \\n\\n\" RESET);\n data_sufficient = true;\n }\n }\n\n if (data_sufficient)\n return true;\n else\n return false;\n}\n\n\nvoid Gril_Calib::printProgress(double percentage, int axis_ascii) {\n int val = (int) (percentage * 100);\n int lpad = (int) (percentage * PBWIDTH);\n int rpad = PBWIDTH - lpad;\n printf(BOLDCYAN \"[Data accumulation] \");\n if (percentage < 1) {\n printf(BOLDYELLOW \"Rotation around Lidar %c Axis: \", char(axis_ascii));\n printf(YELLOW \"%3d%% [%.*s%*s]\\n\", val, lpad, PBSTR, rpad, \"\");\n cout << RESET;\n } else {\n printf(BOLDGREEN \"Rotation around Lidar %c Axis complete! \", char(axis_ascii));\n cout << RESET << \"\\n\";\n }\n}\n\nvoid Gril_Calib::clear() {\n // CSI[2J clears screen, CSI[H moves the cursor to top-left corner\n cout << \"\\x1B[2J\\x1B[H\";\n}\n\n//** main function in LiDAR IMU calibration **//\nvoid Gril_Calib::LI_Calibration(int &orig_odom_freq, int &cut_frame_num, double &timediff_imu_wrt_lidar,\n const double &move_start_time) {\n\n TimeConsuming time(\"Batch optimization\");\n\n downsample_interpolate_IMU(move_start_time); \n fout_before_filter(); \n IMU_time_compensate(0.0, true); \n\n deque IMU_after_zero_phase;\n deque Lidar_after_zero_phase;\n zero_phase_filt(get_IMU_state(), IMU_after_zero_phase); // zero phase low-pass filter\n normalize_acc(IMU_after_zero_phase); \n zero_phase_filt(get_Lidar_state(), Lidar_after_zero_phase);\n set_IMU_state(IMU_after_zero_phase);\n set_Lidar_state(Lidar_after_zero_phase);\n cut_sequence_tail(); \n\n xcorr_temporal_init(orig_odom_freq * cut_frame_num);\n IMU_time_compensate(get_lag_time_1(), false);\n\n\n central_diff(); \n\n deque IMU_after_2nd_zero_phase;\n deque Lidar_after_2nd_zero_phase;\n zero_phase_filt(get_IMU_state(), IMU_after_2nd_zero_phase);\n zero_phase_filt(get_Lidar_state(), Lidar_after_2nd_zero_phase);\n\n \n set_states_2nd_filter(IMU_after_2nd_zero_phase, Lidar_after_2nd_zero_phase); \n fout_check_lidar(); // file output for visualizing lidar low pass filter\n\n \n solve_Rotation_only();\n acc_interpolate();\n align_Group(IMU_state_group, Lidar_wrt_ground_group, IMU_wrt_ground_group,\n normal_vector_wrt_lidar_group, distance_Lidar_wrt_ground_group);\n\n // Calibration at once\n solve_Rot_Trans_calib(timediff_imu_wrt_lidar, imu_sensor_height);\n\n printf(BOLDBLUE\"============================================================ \\n\\n\" RESET);\n double time_L_I = timediff_imu_wrt_lidar + time_delay_IMU_wtr_Lidar;\n print_calibration_result(time_L_I, Rot_Lidar_wrt_IMU, Trans_Lidar_wrt_IMU, gyro_bias, acc_bias, Grav_L0);\n\n printf(BOLDBLUE\"============================================================ \\n\\n\" RESET);\n printf(BOLDCYAN \"Gril-Calib : Ground Robot IMU-LiDAR calibration done.\\n\");\n printf(\"\" RESET);\n \n // For debug\n // plot_result();\n}\n\nvoid Gril_Calib::print_calibration_result(double &time_L_I, M3D &R_L_I, V3D &p_L_I, V3D &bias_g, V3D &bias_a, V3D gravity) {\n cout.setf(ios::fixed);\n printf(BOLDCYAN \"[Calibration Result] \" RESET);\n cout << setprecision(6)\n << \"Rotation matrix from LiDAR frame to IMU frame = \" << RotMtoEuler(R_L_I).transpose() * 57.3 << \" deg\" << endl;\n printf(BOLDCYAN \"[Calibration Result] \" RESET);\n cout << \"Translation vector from LiDAR frame to IMU frame = \" << p_L_I.transpose() << \" m\" << endl;\n printf(BOLDCYAN \"[Calibration Result] \" RESET);\n printf(\"Time Lag IMU to LiDAR = %.8lf s \\n\", time_L_I);\n printf(BOLDCYAN \"[Calibration Result] \" RESET);\n cout << \"Bias of Gyroscope = \" << bias_g.transpose() << \" rad/s\" << endl;\n printf(BOLDCYAN \"[Calibration Result] \" RESET);\n cout << \"Bias of Accelerometer = \" << bias_a.transpose() << \" m/s^2\" << endl;\n}\n\nvoid Gril_Calib::plot_result() {\n vector> IMU_omg(3), IMU_acc(3), IMU_ang_acc(3), Lidar_omg(3), Lidar_acc(3), Lidar_ang_acc(3), Lidar_vel(3), Lidar_pos(3);\n for (auto it_IMU_state = IMU_state_group.begin(); it_IMU_state != IMU_state_group.end() - 1; it_IMU_state++) {\n for (int i = 0; i < 3; i++) {\n IMU_omg[i].push_back(it_IMU_state->ang_vel[i]);\n IMU_ang_acc[i].push_back(it_IMU_state->ang_acc[i]);\n IMU_acc[i].push_back(it_IMU_state->linear_acc[i]);\n }\n }\n for (auto it_Lidar_state = Lidar_state_group.begin();\n it_Lidar_state != Lidar_state_group.end() - 1; it_Lidar_state++) {\n for (int i = 0; i < 3; i++) {\n Lidar_pos[i].push_back(it_Lidar_state->pos_end[i]);\n Lidar_omg[i].push_back(it_Lidar_state->ang_vel[i]);\n Lidar_acc[i].push_back(it_Lidar_state->linear_acc[i]);\n Lidar_ang_acc[i].push_back(it_Lidar_state->ang_acc[i]);\n Lidar_vel[i].push_back(it_Lidar_state->linear_vel[i]);\n }\n }\n\n plt::figure(1);\n plt::subplot(2, 3, 1);\n plt::title(\"IMU Angular Velocity\");\n plt::named_plot(\"IMU omg x\", IMU_omg[0]);\n plt::named_plot(\"IMU omg y\", IMU_omg[1]);\n plt::named_plot(\"IMU omg z\", IMU_omg[2]);\n plt::legend();\n plt::grid(true);\n\n plt::subplot(2, 3, 2);\n plt::title(\"IMU Linear Acceleration\");\n plt::named_plot(\"IMU acc x\", IMU_acc[0]);\n plt::named_plot(\"IMU acc y\", IMU_acc[1]);\n plt::named_plot(\"IMU acc z\", IMU_acc[2]);\n plt::legend();\n plt::grid(true);\n\n plt::subplot(2, 3, 3);\n plt::title(\"IMU Angular Acceleration\");\n plt::named_plot(\"IMU ang acc x\", IMU_ang_acc[0]);\n plt::named_plot(\"IMU ang acc y\", IMU_ang_acc[1]);\n plt::named_plot(\"IMU ang acc z\", IMU_ang_acc[2]);\n plt::legend();\n plt::grid(true);\n\n plt::subplot(2, 3, 4);\n plt::title(\"LiDAR Angular Velocity\");\n plt::named_plot(\"Lidar omg x\", Lidar_omg[0]);\n plt::named_plot(\"Lidar omg y\", Lidar_omg[1]);\n plt::named_plot(\"Lidar omg z\", Lidar_omg[2]);\n plt::legend();\n plt::grid(true);\n\n plt::subplot(2, 3, 5);\n plt::title(\"LiDAR Linear Acceleration\");\n plt::named_plot(\"Lidar acc x\", Lidar_acc[0]);\n plt::named_plot(\"Lidar acc y\", Lidar_acc[1]);\n plt::named_plot(\"Lidar acc z\", Lidar_acc[2]);\n plt::legend();\n plt::grid(true);\n\n plt::subplot(2, 3, 6);\n plt::title(\"LiDAR Angular Acceleration\");\n plt::named_plot(\"Lidar ang acc x\", Lidar_ang_acc[0]);\n plt::named_plot(\"Lidar ang acc y\", Lidar_ang_acc[1]);\n plt::named_plot(\"Lidar ang acc z\", Lidar_ang_acc[2]);\n plt::legend();\n plt::grid(true);\n\n plt::show();\n plt::pause(0);\n plt::close();\n}\n" }, { "path": "include/Gril_Calib/Gril_Calib.h", "content": "#ifndef Gril_Calib_H\n#define Gril_Calib_H\n\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\n#include \n#include \n#include \n#include \n\n#include \n#include \"matplotlibcpp.h\"\n#include \n\n#define FILE_DIR(name) (string(string(ROOT_DIR) + \"Log/\"+ name))\n\nnamespace plt = matplotlibcpp;\nusing namespace std;\nusing namespace Eigen;\n\ntypedef Vector3d V3D;\ntypedef Matrix3d M3D;\ntypedef Eigen::Quaterniond QD;\nconst V3D STD_GRAV = V3D(0, 0, -G_m_s2);\n\nextern double GYRO_FACTOR_;\nextern double ACC_FACTOR_;\nextern double GROUND_FACTOR_;\n\n\n// Lidar IMU calibration\n// States needed by calibration\nstruct CalibState {\n M3D rot_end;\n V3D pos_end;\n V3D ang_vel;\n V3D linear_vel;\n V3D ang_acc;\n V3D linear_acc;\n double timeStamp;\n\n CalibState() {\n rot_end = Eye3d;\n pos_end = Zero3d;\n ang_vel = Zero3d;\n linear_vel = Zero3d;\n ang_acc = Zero3d;\n linear_acc = Zero3d;\n timeStamp = 0.0;\n };\n\n CalibState(const CalibState &b) {\n this->rot_end = b.rot_end;\n this->pos_end = b.pos_end;\n this->ang_vel = b.ang_vel;\n this->ang_acc = b.ang_acc;\n this->linear_vel = b.linear_vel;\n this->linear_acc = b.linear_acc;\n this->timeStamp = b.timeStamp;\n };\n\n CalibState operator*(const double &coeff) {\n CalibState a;\n a.ang_vel = this->ang_vel * coeff;\n a.ang_acc = this->ang_acc * coeff;\n a.linear_vel = this->linear_vel * coeff;\n a.linear_acc = this->linear_acc * coeff;\n return a;\n };\n\n CalibState &operator+=(const CalibState &b) {\n this->ang_vel += b.ang_vel;\n this->ang_acc += b.ang_acc;\n this->linear_vel += b.linear_vel;\n this->linear_acc += b.linear_acc;\n return *this;\n };\n\n CalibState &operator-=(const CalibState &b) {\n this->ang_vel -= b.ang_vel;\n this->ang_acc -= b.ang_acc;\n this->linear_vel -= b.linear_vel;\n this->linear_acc -= b.linear_acc;\n return *this;\n };\n\n CalibState &operator=(const CalibState &b) {\n this->ang_vel = b.ang_vel;\n this->ang_acc = b.ang_acc;\n this->linear_vel = b.linear_vel;\n this->linear_acc = b.linear_acc;\n return *this;\n };\n};\n\nstruct Angular_Vel_Cost_only_Rot {\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Angular_Vel_Cost_only_Rot(V3D IMU_ang_vel_, V3D Lidar_ang_vel_) :\n IMU_ang_vel(IMU_ang_vel_), Lidar_ang_vel(Lidar_ang_vel_) {}\n\n template\n bool operator()(const T *q, T *residual) const {\n Eigen::Matrix IMU_ang_vel_T = IMU_ang_vel.cast();\n Eigen::Matrix Lidar_ang_vel_T = Lidar_ang_vel.cast();\n Eigen::Quaternion q_LI{q[0], q[1], q[2], q[3]};\n Eigen::Matrix R_LI = q_LI.toRotationMatrix(); //Rotation (from LiDAR to IMU)\n Eigen::Matrix resi = R_LI * Lidar_ang_vel_T - IMU_ang_vel_T;\n residual[0] = resi[0];\n residual[1] = resi[1];\n residual[2] = resi[2];\n return true;\n }\n\n static ceres::CostFunction *Create(const V3D IMU_ang_vel_, const V3D Lidar_ang_vel_) {\n return (new ceres::AutoDiffCostFunction(\n new Angular_Vel_Cost_only_Rot(IMU_ang_vel_, Lidar_ang_vel_)));\n }\n\n V3D IMU_ang_vel;\n V3D Lidar_ang_vel;\n};\n\nstruct Angular_Vel_IL_Cost {\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Angular_Vel_IL_Cost(V3D IMU_ang_vel_, V3D IMU_ang_acc_, V3D Lidar_ang_vel_, double deltaT_LI_) :\n IMU_ang_vel(IMU_ang_vel_), IMU_ang_acc(IMU_ang_acc_), Lidar_ang_vel(Lidar_ang_vel_),\n deltaT_LI(deltaT_LI_) {}\n\n template\n bool operator()(const T *q, const T *b_g, const T *t, T *residual) const {\n //Known parameters used for Residual Construction\n Eigen::Matrix IMU_ang_vel_T = IMU_ang_vel.cast();\n Eigen::Matrix IMU_ang_acc_T = IMU_ang_acc.cast();\n Eigen::Matrix Lidar_ang_vel_T = Lidar_ang_vel.cast();\n T deltaT_LI_T{deltaT_LI};\n\n //Unknown Parameters, needed to be estimated\n Eigen::Quaternion q_IL{q[0], q[1], q[2], q[3]};\n Eigen::Matrix R_IL = q_IL.toRotationMatrix(); //Rotation\n Eigen::Matrix bias_g{b_g[0], b_g[1], b_g[2]}; //Bias of gyroscope\n T td{t[0]}; //Time lag (IMU wtr Lidar)\n\n // original Residual\n Eigen::Matrix resi = R_IL.transpose() * Lidar_ang_vel_T - IMU_ang_vel_T - (deltaT_LI_T + td) * IMU_ang_acc_T + bias_g;\n residual[0] = T(GYRO_FACTOR_) * resi[0];\n residual[1] = T(GYRO_FACTOR_) * resi[1];\n residual[2] = T(GYRO_FACTOR_) * resi[2];\n return true;\n }\n\n static ceres::CostFunction *\n Create(const V3D IMU_ang_vel_, const V3D IMU_ang_acc_, const V3D Lidar_ang_vel_, const double deltaT_LI_) {\n // 3 residual, 4 parameter (q), 3 parameter (bias), 1 parameter (time lag)\n return (new ceres::AutoDiffCostFunction(\n new Angular_Vel_IL_Cost(IMU_ang_vel_, IMU_ang_acc_, Lidar_ang_vel_, deltaT_LI_)));\n }\n\n V3D IMU_ang_vel;\n V3D IMU_ang_acc;\n V3D Lidar_ang_vel;\n double deltaT_LI;\n};\n\nstruct Ground_Plane_Cost_IL {\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Ground_Plane_Cost_IL(QD Lidar_wrt_ground_, QD IMU_wrt_ground_, \n const double distance_Lidar_wrt_ground_, const double imu_sensor_height_) :\n Lidar_wrt_ground(Lidar_wrt_ground_), IMU_wrt_ground(IMU_wrt_ground_), \n distance_Lidar_wrt_ground(distance_Lidar_wrt_ground_), imu_sensor_height(imu_sensor_height_) {}\n\n template\n bool operator()(const T *q, const T *trans, T *residual) const {\n // Known parameters used for Residual Construction\n Eigen::Quaternion Lidar_wrt_ground_T = Lidar_wrt_ground.cast(); // from ground frame to LiDAR plane frame\n Eigen::Matrix R_GL = Lidar_wrt_ground_T.toRotationMatrix(); // Rotation matrix (from Ground to LiDAR)\n Eigen::Quaternion IMU_wrt_ground_T = IMU_wrt_ground.cast(); // from ground frame (earth frame) to IMU frame\n Eigen::Matrix R_GI = IMU_wrt_ground_T.toRotationMatrix(); // Rotation matrix (from Ground to IMU)\n \n T distance_Lidar_wrt_ground_T = T(distance_Lidar_wrt_ground); // Distance of LiDAR plane from ground\n T imu_sensor_height_T = T(imu_sensor_height); // Height of IMU sensor from ground\n\n // Unknown Parameters, needed to be estimated\n Eigen::Quaternion q_IL{q[0], q[1], q[2], q[3]};\n Eigen::Matrix R_IL = q_IL.toRotationMatrix(); // Rotation IMU frame to LiDAR frame\n Eigen::Matrix T_IL{trans[0], trans[1], trans[2]}; // Translation of I-L (IMU wtr Lidar)\n \n // Plane motion constraint Residual\n Eigen::Matrix R_plane = R_IL.transpose() * R_GI.transpose() * R_GL;\n Eigen::Matrix e3 = Eigen::Matrix::UnitZ(); // (0,0,1)\n Eigen::Matrix resi_plane = R_plane * e3;\n\n // Distance constraint Residual - TODO : Generalized\n Eigen::Matrix imu_height_vec = imu_sensor_height_T * e3;\n Eigen::Matrix lidar_height_vec = distance_Lidar_wrt_ground_T * e3;\n Eigen::Matrix tmp_distance = R_IL * R_GI * imu_height_vec - R_GL * lidar_height_vec;\n\n T resi_distance = T_IL[2] - tmp_distance[2];\n\n // Residual\n residual[0] = T(GROUND_FACTOR_) * resi_plane[0]; \n residual[1] = T(GROUND_FACTOR_) * resi_plane[1]; \n residual[2] = T(GROUND_FACTOR_) * resi_distance;\n\n return true;\n }\n\n static ceres::CostFunction *\n Create(const QD Lidar_wrt_ground_, const QD IMU_wrt_ground_, \n const double distance_Lidar_wrt_ground, const double imu_sensor_height) {\n // 3 residual, 4 parameter (q), 3 paramter (translation)\n return (new ceres::AutoDiffCostFunction(\n new Ground_Plane_Cost_IL(Lidar_wrt_ground_, IMU_wrt_ground_, \n distance_Lidar_wrt_ground, imu_sensor_height)));\n }\n\n QD Lidar_wrt_ground;\n QD IMU_wrt_ground;\n double distance_Lidar_wrt_ground;\n double imu_sensor_height;\n};\n\nstruct Linear_acc_Rot_Cost_without_Gravity {\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Linear_acc_Rot_Cost_without_Gravity(CalibState LidarState_, V3D IMU_linear_acc_, QD Lidar_wrt_ground_) :\n LidarState(LidarState_), IMU_linear_acc(IMU_linear_acc_), Lidar_wrt_ground(Lidar_wrt_ground_) {}\n\n template\n bool operator()(const T *q,const T *b_a, const T *trans, T *residual) const {\n // Known parameters used for Residual Construction\n Eigen::Matrix R_LL0_T = LidarState.rot_end.cast(); \n Eigen::Matrix IMU_linear_acc_T = IMU_linear_acc.cast(); // Linear acceleration of IMU\n Eigen::Matrix Lidar_linear_acc_T = LidarState.linear_acc.cast(); // lidar linear acceleration\n Eigen::Quaternion Lidar_wrt_ground_T = Lidar_wrt_ground.cast(); // from ground frame to LiDAR plane frame\n Eigen::Matrix R_GL = Lidar_wrt_ground_T.toRotationMatrix(); // Rotation matrix from Ground frame to LiDAR frame\n\n // Unknown Parameters, needed to be estimated\n Eigen::Matrix bias_aL{b_a[0], b_a[1], b_a[2]}; // Bias of Linear acceleration\n Eigen::Matrix T_IL{trans[0], trans[1], trans[2]}; // Translation of I-L (IMU wtr Lidar)\n Eigen::Quaternion q_IL{q[0], q[1], q[2], q[3]}; // Rotation from IMU frame to LiDAR frame\n Eigen::Matrix R_IL = q_IL.toRotationMatrix();\n\n // Residual Construction\n M3D Lidar_omg_SKEW, Lidar_angacc_SKEW;\n Lidar_omg_SKEW << SKEW_SYM_MATRX(LidarState.ang_vel);\n Lidar_angacc_SKEW << SKEW_SYM_MATRX(LidarState.ang_acc);\n\n M3D Jacob_trans = Lidar_omg_SKEW * Lidar_omg_SKEW + Lidar_angacc_SKEW;\n Eigen::Matrix Jacob_trans_T = Jacob_trans.cast();\n \n Eigen::Matrix resi = R_LL0_T * R_IL * IMU_linear_acc_T - R_LL0_T * bias_aL\n + R_GL * STD_GRAV - Lidar_linear_acc_T - R_LL0_T * Jacob_trans_T * T_IL;\n\n \n residual[0] = T(ACC_FACTOR_) * resi[0];\n residual[1] = T(ACC_FACTOR_) * resi[1];\n residual[2] = T(ACC_FACTOR_) * resi[2];\n \n return true;\n }\n\n static ceres::CostFunction *Create(const CalibState LidarState_, const V3D IMU_linear_acc_, const QD Lidar_wrt_ground_) {\n // residual 3, quaternion 4, bias 3, translation 3\n return (new ceres::AutoDiffCostFunction(\n new Linear_acc_Rot_Cost_without_Gravity(LidarState_, IMU_linear_acc_, Lidar_wrt_ground_)));\n }\n\n CalibState LidarState;\n V3D IMU_linear_acc;\n QD Lidar_wrt_ground;\n};\n\nclass Gril_Calib {\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n ofstream fout_LiDAR_meas, fout_IMU_meas, fout_before_filt_IMU, fout_before_filt_Lidar, fout_acc_cost, fout_after_rot;\n ofstream fout_LiDAR_ang_vel, fout_IMU_ang_vel, fout_Jacob_trans;\n ofstream fout_LiDAR_meas_after;\n\n double data_accum_length = 0.0;\n double x_accumulate = 0.0;\n double y_accumulate = 0.0;\n double z_accumulate = 0.0;\n double svd_threshold = 0.01;\n double imu_sensor_height = 0.0;\n double trans_IL_x, trans_IL_y, trans_IL_z; \n \n double bound_th;\n bool set_boundary = false;\n bool verbose = false;\n\n\n Gril_Calib();\n\n ~Gril_Calib();\n\n // original\n struct Butterworth {\n // Coefficients of 6 order butterworth low pass filter, omega = 0.15 \n double Coeff_b[7] = {0.0001,0.0005,0.0011,0.0015,0.0011,0.0005,0.0001};\n double Coeff_a[7] = {1,-4.1824,7.4916,-7.3136,4.0893,-1.2385,0.1584};\n\n int Coeff_size = 7;\n int extend_num = 0;\n };\n\n void plot_result();\n\n void push_ALL_IMU_CalibState(const sensor_msgs::Imu::ConstPtr &msg, const double &mean_acc_norm);\n\n void push_IMU_CalibState(const V3D &omg, const V3D &acc, const double ×tamp);\n\n void push_Lidar_CalibState(const M3D &rot, const V3D &pos, const V3D &omg, const V3D &linear_vel, const double ×tamp);\n\n void push_Plane_Constraint(const Eigen::Quaterniond &q_lidar, const Eigen::Quaterniond &q_imu, const V3D &normal_lidar, const double &distance_lidar);\n\n void downsample_interpolate_IMU(const double &move_start_time);\n\n void central_diff();\n\n void xcorr_temporal_init(const double &odom_freq);\n\n void IMU_time_compensate(const double &lag_time, const bool &is_discard);\n\n void acc_interpolate();\n\n void Butter_filt(const deque &signal_in, deque &signal_out);\n\n void zero_phase_filt(const deque &signal_in, deque &signal_out);\n\n void cut_sequence_tail();\n\n void set_IMU_state(const deque &IMU_states);\n\n void set_Lidar_state(const deque &Lidar_states);\n\n void set_states_2nd_filter(const deque &IMU_states, const deque &Lidar_states);\n\n void solve_Rot_Trans_calib(double &timediff_imu_wrt_lidar, const double &imu_sensor_height);\n\n void normalize_acc(deque &signal_in);\n\n void align_Group(const deque &IMU_states, deque &Lidar_wrt_ground_states, \n deque &IMU_wrt_ground_states, deque &normal_vector_wrt_lidar_group, \n deque &distance_Lidar_ground_states);\n\n bool data_sufficiency_assess(MatrixXd &Jacobian_rot, int &frame_num, V3D &lidar_omg, int &orig_odom_freq, int &cut_frame_num, QD &lidar_q, QD &imu_q, double &lidar_estimate_height);\n\n void solve_Rotation_only();\n\n void printProgress(double percentage, int axis);\n\n void clear();\n\n void fout_before_filter();\n\n void fout_check_lidar();\n\n void LI_Calibration(int &orig_odom_freq, int &cut_frame_num, double &timediff_imu_wrt_lidar, const double &move_start_time);\n\n void print_calibration_result(double &time_L_I, M3D &R_L_I, V3D &p_L_I, V3D &bias_g, V3D &bias_a, V3D gravity);\n\n inline double get_lag_time_1() {\n return time_lag_1;\n }\n\n inline double get_lag_time_2() {\n return time_lag_2;\n }\n\n inline double get_total_time_lag() {\n return time_delay_IMU_wtr_Lidar;\n }\n\n inline double get_time_result() {\n return time_offset_result;\n }\n\n inline V3D get_Grav_L0() {\n return Grav_L0;\n }\n\n inline M3D get_R_LI() {\n return Rot_Lidar_wrt_IMU;\n }\n\n inline V3D get_T_LI() {\n return Trans_Lidar_wrt_IMU;\n }\n\n inline V3D get_gyro_bias() {\n return gyro_bias;\n }\n\n inline V3D get_acc_bias() {\n return acc_bias;\n }\n\n inline void IMU_buffer_clear() {\n IMU_state_group_ALL.clear();\n }\n\n deque get_IMU_state() {\n return IMU_state_group;\n }\n\n deque get_Lidar_state() {\n return Lidar_state_group;\n }\n\nprivate:\n deque IMU_state_group; // LiDAR와 interpolation을 진행한 IMU data 결과를 가지고 있는 groups\n deque Lidar_state_group; // LiDAR state \n deque IMU_state_group_ALL; // 모든 IMU data (ROS topic)를 가지고 있는 groups\n deque Lidar_wrt_ground_group; // from ground frame (earth frame) to LiDAR plane frame\n deque IMU_wrt_ground_group; // from ground frame (earth frame) to IMU frame\n deque normal_vector_wrt_lidar_group; // normal vector of LiDAR ground segmentation\n deque distance_Lidar_wrt_ground_group; // distance from ground frame (earth frame) to LiDAR frame\n V3D Grav_L0; // Gravity vector in the initial Lidar frame L_0\n \n /// Parameters needed to be calibrated\n M3D Rot_Grav_wrt_Init_Lidar; // Rotation from inertial frame G to initial Lidar frame L_0\n M3D Rot_Lidar_wrt_IMU; // Rotation from Lidar frame L to IMU frame I\n V3D Trans_Lidar_wrt_IMU; // Translation from Lidar frame L to IMU frame I\n V3D gyro_bias; // gyro bias\n V3D acc_bias; // acc bias\n\n double time_delay_IMU_wtr_Lidar; //(Soft) time delay between IMU and Lidar = time_lag_1 + time_lag_2\n double time_offset_result; // Time offset between IMU and Lidar (final result)\n double time_lag_1; // Time offset estimated by cross-correlation\n double time_lag_2; // Time offset estimated by unified optimization\n int lag_IMU_wtr_Lidar; // positive: timestamp of IMU is larger than that of LiDAR\n\n // Previous calibration results\n M3D R_IL_prev; // Rotation from Lidar frame L to IMU frame I\n V3D T_IL_prev; // Translation from Lidar frame L to IMU frame I\n V3D gyro_bias_prev; // gyro bias\n V3D acc_bias_prev; // acc bias\n double time_lag_2_prev; // Time offset estimated by unified optimization\n\n};\n\n#endif" }, { "path": "include/GroundSegmentation/patchworkpp.hpp", "content": "/**\n * @file patchworkpp.hpp\n * @author Seungjae Lee\n * @brief \n * @version 0.1\n * @date 2022-07-20\n * \n * @copyright Copyright (c) 2022\n * \n */\n#ifndef PATCHWORKPP_H\n#define PATCHWORKPP_H\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\n#include \n\n#define MARKER_Z_VALUE -2.2\n#define UPRIGHT_ENOUGH 0.55\n#define FLAT_ENOUGH 0.2\n#define TOO_HIGH_ELEVATION 0.0\n#define TOO_TILTED 1.0\n\n#define NUM_HEURISTIC_MAX_PTS_IN_PATCH 3000\n\nusing Eigen::MatrixXf;\nusing Eigen::JacobiSVD;\nusing Eigen::VectorXf;\n\nusing namespace std;\n\n\n/*\n* @brief: This function is for preprocessing of pointcloud data. (XYZI >> XYZ)) \n*/\nvoid xyzi2xyz(pcl::PointCloud::Ptr XYZI, pcl::PointCloud::Ptr XYZ) {\n (*XYZ).points.resize((*XYZI).size());\n for (size_t i = 0; i < (*XYZI).points.size(); i++) {\n (*XYZ).points[i].x = (*XYZI).points[i].x;\n (*XYZ).points[i].y = (*XYZI).points[i].y;\n (*XYZ).points[i].z = (*XYZI).points[i].z;\n }\n}\n\n/*\n @brief PathWork ROS Node.\n*/\ntemplate \nbool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }\n\ntemplate \nstruct RevertCandidate \n{ \n int concentric_idx;\n int sector_idx;\n double ground_flatness;\n double line_variable;\n Eigen::Vector4f pc_mean;\n pcl::PointCloud regionwise_ground;\n \n RevertCandidate(int _c_idx, int _s_idx, double _flatness, double _line_var, Eigen::Vector4f _pc_mean, pcl::PointCloud _ground)\n : concentric_idx(_c_idx), sector_idx(_s_idx), ground_flatness(_flatness), line_variable(_line_var), pc_mean(_pc_mean), regionwise_ground(_ground) {}\n};\n\ntemplate \nclass PatchWorkpp {\n\npublic:\n typedef std::vector> Ring;\n typedef std::vector Zone;\n\n PatchWorkpp() {};\n\n PatchWorkpp(ros::NodeHandle *nh) : node_handle_(*nh) {\n // Init ROS related\n ROS_INFO(\"Inititalizing PatchWork++...\");\n\n node_handle_.param(\"/patchworkpp/verbose\", verbose_, false);\n\n node_handle_.param(\"/patchworkpp/sensor_height\", sensor_height_, 1.723);\n node_handle_.param(\"/patchworkpp/num_iter\", num_iter_, 3);\n node_handle_.param(\"/patchworkpp/num_lpr\", num_lpr_, 20);\n node_handle_.param(\"/patchworkpp/num_min_pts\", num_min_pts_, 10); \n node_handle_.param(\"/patchworkpp/th_seeds\", th_seeds_, 0.4);\n node_handle_.param(\"/patchworkpp/th_dist\", th_dist_, 0.3);\n node_handle_.param(\"/patchworkpp/th_seeds_v\", th_seeds_v_, 0.4);\n node_handle_.param(\"/patchworkpp/th_dist_v\", th_dist_v_, 0.3);\n node_handle_.param(\"/patchworkpp/max_r\", max_range_, 80.0);\n node_handle_.param(\"/patchworkpp/min_r\", min_range_, 2.7);\n node_handle_.param(\"/patchworkpp/uprightness_thr\", uprightness_thr_, 0.5);\n node_handle_.param(\"/patchworkpp/adaptive_seed_selection_margin\", adaptive_seed_selection_margin_, -1.1);\n node_handle_.param(\"/patchworkpp/RNR_ver_angle_thr\", RNR_ver_angle_thr_, -15.0);\n node_handle_.param(\"/patchworkpp/RNR_intensity_thr\", RNR_intensity_thr_, 0.2);\n node_handle_.param(\"/patchworkpp/max_flatness_storage\", max_flatness_storage_, 1000);\n node_handle_.param(\"/patchworkpp/max_elevation_storage\", max_elevation_storage_, 1000);\n node_handle_.param(\"/patchworkpp/enable_RNR\", enable_RNR_, true);\n node_handle_.param(\"/patchworkpp/enable_RVPF\", enable_RVPF_, true);\n node_handle_.param(\"/patchworkpp/enable_TGR\", enable_TGR_, true);\n\n ROS_INFO(\"Sensor Height: %f\", sensor_height_);\n ROS_INFO(\"Num of Iteration: %d\", num_iter_);\n ROS_INFO(\"Num of LPR: %d\", num_lpr_);\n ROS_INFO(\"Num of min. points: %d\", num_min_pts_);\n ROS_INFO(\"Seeds Threshold: %f\", th_seeds_);\n ROS_INFO(\"Distance Threshold: %f\", th_dist_);\n ROS_INFO(\"Max. range:: %f\", max_range_);\n ROS_INFO(\"Min. range:: %f\", min_range_);\n ROS_INFO(\"Normal vector threshold: %f\", uprightness_thr_);\n ROS_INFO(\"adaptive_seed_selection_margin: %f\", adaptive_seed_selection_margin_);\n\n // CZM denotes 'Concentric Zone Model'. Please refer to our paper\n node_handle_.getParam(\"/patchworkpp/czm/num_zones\", num_zones_);\n node_handle_.getParam(\"/patchworkpp/czm/num_sectors_each_zone\", num_sectors_each_zone_);\n node_handle_.getParam(\"/patchworkpp/czm/mum_rings_each_zone\", num_rings_each_zone_);\n node_handle_.getParam(\"/patchworkpp/czm/elevation_thresholds\", elevation_thr_);\n node_handle_.getParam(\"/patchworkpp/czm/flatness_thresholds\", flatness_thr_);\n\n ROS_INFO(\"Num. zones: %d\", num_zones_);\n\n if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) {\n throw invalid_argument(\"Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone\");\n }\n if (elevation_thr_.size() != flatness_thr_.size()) {\n throw invalid_argument(\"Some parameters are wrong! Check the elevation/flatness_thresholds\");\n }\n\n cout << (boost::format(\"Num. sectors: %d, %d, %d, %d\") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] %\n num_sectors_each_zone_[2] %\n num_sectors_each_zone_[3]).str() << endl;\n cout << (boost::format(\"Num. rings: %01d, %01d, %01d, %01d\") % num_rings_each_zone_[0] %\n num_rings_each_zone_[1] %\n num_rings_each_zone_[2] %\n num_rings_each_zone_[3]).str() << endl;\n cout << (boost::format(\"elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f \") % elevation_thr_[0] % elevation_thr_[1] %\n elevation_thr_[2] %\n elevation_thr_[3]).str() << endl;\n cout << (boost::format(\"flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f \") % flatness_thr_[0] % flatness_thr_[1] %\n flatness_thr_[2] %\n flatness_thr_[3]).str() << endl;\n num_rings_of_interest_ = elevation_thr_.size();\n\n node_handle_.param(\"/patchworkpp/visualize\", visualize_, true);\n\n int num_polygons = std::inner_product(num_rings_each_zone_.begin(), num_rings_each_zone_.end(), num_sectors_each_zone_.begin(), 0);\n poly_list_.header.frame_id = \"map\";\n poly_list_.polygons.reserve(num_polygons);\n\n revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);\n ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);\n regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);\n regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);\n\n PlaneViz = node_handle_.advertise(\"/patchworkpp/plane\", 100, true);\n pub_revert_pc = node_handle_.advertise(\"/patchworkpp/revert_pc\", 100, true);\n pub_reject_pc = node_handle_.advertise(\"/patchworkpp/reject_pc\", 100, true);\n pub_normal = node_handle_.advertise(\"/patchworkpp/normals\", 100, true);\n pub_noise = node_handle_.advertise(\"/patchworkpp/noise\", 100, true);\n pub_vertical = node_handle_.advertise(\"/patchworkpp/vertical\", 100, true);\n\n min_range_z2_ = (7 * min_range_ + max_range_) / 8.0;\n min_range_z3_ = (3 * min_range_ + max_range_) / 4.0;\n min_range_z4_ = (min_range_ + max_range_) / 2.0;\n\n min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_};\n ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0),\n (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1),\n (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2),\n (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)};\n sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1),\n 2 * M_PI / num_sectors_each_zone_.at(2),\n 2 * M_PI / num_sectors_each_zone_.at(3)};\n\n cout << \"INITIALIZATION COMPLETE\" << endl;\n\n for (int i = 0; i < num_zones_; i++) {\n Zone z;\n initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]);\n ConcentricZoneModel_.push_back(z);\n }\n }\n\n void estimate_ground(pcl::PointCloud cloud_in,\n pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, double &time_taken);\n\nprivate:\n\n // Every private member variable is written with the undescore(\"_\") in its end.\n\n ros::NodeHandle node_handle_;\n\n std::recursive_mutex mutex_;\n\n int num_iter_;\n int num_lpr_;\n int num_min_pts_;\n int num_zones_;\n int num_rings_of_interest_;\n\n double sensor_height_;\n double th_seeds_;\n double th_dist_;\n double th_seeds_v_;\n double th_dist_v_;\n double max_range_;\n double min_range_;\n double uprightness_thr_;\n double adaptive_seed_selection_margin_;\n double min_range_z2_; // 12.3625\n double min_range_z3_; // 22.025\n double min_range_z4_; // 41.35\n double RNR_ver_angle_thr_;\n double RNR_intensity_thr_;\n\n bool verbose_;\n bool enable_RNR_;\n bool enable_RVPF_;\n bool enable_TGR_;\n\n int max_flatness_storage_, max_elevation_storage_;\n std::vector update_flatness_[4];\n std::vector update_elevation_[4];\n\n float d_;\n\n VectorXf normal_;\n MatrixXf pnormal_;\n VectorXf singular_values_;\n Eigen::Matrix3f cov_;\n Eigen::Vector4f pc_mean_;\n\n // For visualization\n bool visualize_;\n\n vector num_sectors_each_zone_;\n vector num_rings_each_zone_;\n\n vector sector_sizes_;\n vector ring_sizes_;\n vector min_ranges_;\n vector elevation_thr_;\n vector flatness_thr_;\n\n vector ConcentricZoneModel_;\n\n jsk_recognition_msgs::PolygonArray poly_list_;\n\n ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical;\n pcl::PointCloud revert_pc_, reject_pc_, noise_pc_, vertical_pc_;\n pcl::PointCloud ground_pc_;\n\n pcl::PointCloud normals_; \n\n pcl::PointCloud regionwise_ground_, regionwise_nonground_;\n\n void initialize_zone(Zone &z, int num_sectors, int num_rings);\n\n void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings);\n void flush_patches(std::vector &czm);\n\n void pc2czm(const pcl::PointCloud &src, std::vector &czm);\n\n void reflected_noise_removal(pcl::PointCloud &cloud, pcl::PointCloud &cloud_nonground);\n \n void temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground,\n std::vector ring_flatness, std::vector> candidates,\n int concentric_idx);\n \n void calc_mean_stdev(std::vector vec, double &mean, double &stdev);\n\n void update_elevation_thr();\n void update_flatness_thr();\n\n double xy2theta(const double &x, const double &y);\n\n double xy2radius(const double &x, const double &y);\n\n void estimate_plane(const pcl::PointCloud &ground);\n\n void extract_piecewiseground(\n const int zone_idx, const pcl::PointCloud &src,\n pcl::PointCloud &dst,\n pcl::PointCloud &non_ground_dst);\n\n void extract_initial_seeds(\n const int zone_idx, const pcl::PointCloud &p_sorted,\n pcl::PointCloud &init_seeds);\n\n void extract_initial_seeds(\n const int zone_idx, const pcl::PointCloud &p_sorted,\n pcl::PointCloud &init_seeds, double th_seed);\n\n /***\n * For visulization of Ground Likelihood Estimation\n */\n geometry_msgs::PolygonStamped set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split);\n\n void set_ground_likelihood_estimation_status(\n const int zone_idx, const int ring_idx,\n const int concentric_idx,\n const double z_vec,\n const double z_elevation,\n const double ground_flatness);\n\n};\n\ntemplate inline\nvoid PatchWorkpp::initialize_zone(Zone &z, int num_sectors, int num_rings) {\n z.clear();\n pcl::PointCloud cloud;\n cloud.reserve(1000);\n Ring ring;\n for (int i = 0; i < num_sectors; i++) {\n ring.emplace_back(cloud);\n }\n for (int j = 0; j < num_rings; j++) {\n z.emplace_back(ring);\n }\n}\n\ntemplate inline\nvoid PatchWorkpp::flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings) {\n for (int i = 0; i < num_sectors; i++) {\n for (int j = 0; j < num_rings; j++) {\n if (!patches[j][i].points.empty()) patches[j][i].points.clear();\n }\n }\n}\n\ntemplate inline\nvoid PatchWorkpp::flush_patches(vector &czm) {\n for (int k = 0; k < num_zones_; k++) {\n for (int i = 0; i < num_rings_each_zone_[k]; i++) {\n for (int j = 0; j < num_sectors_each_zone_[k]; j++) {\n if (!czm[k][i][j].points.empty()) czm[k][i][j].points.clear();\n }\n }\n }\n\n if( verbose_ ) cout << \"Flushed patches\" << endl;\n}\n\ntemplate inline\nvoid PatchWorkpp::estimate_plane(const pcl::PointCloud &ground) {\n pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_);\n // Singular Value Decomposition: SVD\n Eigen::JacobiSVD svd(cov_, Eigen::DecompositionOptions::ComputeFullU);\n singular_values_ = svd.singularValues();\n\n // use the least singular vector as normal\n normal_ = (svd.matrixU().col(2));\n\n if (normal_(2) < 0) { for(int i=0; i<3; i++) normal_(i) *= -1; }\n\n // mean ground seeds value\n Eigen::Vector3f seeds_mean = pc_mean_.head<3>();\n\n // according to normal.T*[x,y,z] = -d\n d_ = -(normal_.transpose() * seeds_mean)(0, 0);\n}\n\ntemplate inline\nvoid PatchWorkpp::extract_initial_seeds(\n const int zone_idx, const pcl::PointCloud &p_sorted,\n pcl::PointCloud &init_seeds, double th_seed) {\n init_seeds.points.clear();\n\n // LPR is the mean of low point representative\n double sum = 0;\n int cnt = 0;\n\n int init_idx = 0;\n if (zone_idx == 0) {\n for (int i = 0; i < p_sorted.points.size(); i++) {\n if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) {\n ++init_idx;\n } else {\n break;\n }\n }\n }\n\n // Calculate the mean height value.\n for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) {\n sum += p_sorted.points[i].z;\n cnt++;\n }\n double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0\n\n // iterate pointcloud, filter those height is less than lpr.height+th_seeds_\n for (int i = 0; i < p_sorted.points.size(); i++) {\n if (p_sorted.points[i].z < lpr_height + th_seed) {\n init_seeds.points.push_back(p_sorted.points[i]);\n }\n }\n}\n\ntemplate inline\nvoid PatchWorkpp::extract_initial_seeds(\n const int zone_idx, const pcl::PointCloud &p_sorted,\n pcl::PointCloud &init_seeds) {\n init_seeds.points.clear();\n\n // LPR is the mean of low point representative\n double sum = 0;\n int cnt = 0;\n\n int init_idx = 0;\n if (zone_idx == 0) {\n for (int i = 0; i < p_sorted.points.size(); i++) {\n if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) {\n ++init_idx;\n } else {\n break;\n }\n }\n }\n \n // Calculate the mean height value.\n for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) {\n sum += p_sorted.points[i].z;\n cnt++;\n }\n double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0\n\n // iterate pointcloud, filter those height is less than lpr.height+th_seeds_\n for (int i = 0; i < p_sorted.points.size(); i++) {\n if (p_sorted.points[i].z < lpr_height + th_seeds_) {\n init_seeds.points.push_back(p_sorted.points[i]);\n }\n }\n}\n\ntemplate inline\nvoid PatchWorkpp::reflected_noise_removal(pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_nonground)\n{\n for (int i=0; i::min();\n }\n }\n \n cloud_nonground += noise_pc_;\n \n if (verbose_) cout << \"[ RNR ] Num of noises : \" << noise_pc_.points.size() << endl;\n}\n\n/*\n @brief Velodyne pointcloud callback function. The main GPF pipeline is here.\n PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud\n ->error points removal -> extract ground seeds -> ground plane fit mainloop\n*/\n\ntemplate inline\nvoid PatchWorkpp::estimate_ground(\n pcl::PointCloud cloud_in,\n pcl::PointCloud &cloud_ground,\n pcl::PointCloud &cloud_nonground,\n double &time_taken) {\n \n unique_lock lock(mutex_);\n\n poly_list_.header.stamp = ros::Time::now();\n if (!poly_list_.polygons.empty()) poly_list_.polygons.clear();\n if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear();\n\n static double start, t0, t1, t2, end;\n\n double pca_time_ = 0.0;\n double t_revert = 0.0;\n double t_total_ground = 0.0;\n double t_total_estimate = 0.0;\n\n start = ros::Time::now().toSec();\n \n cloud_ground.clear();\n cloud_nonground.clear();\n\n // 1. Reflected Noise Removal (RNR)\n if (enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground);\n\n t1 = ros::Time::now().toSec();\n\n // 2. Concentric Zone Model (CZM)\n flush_patches(ConcentricZoneModel_);\n pc2czm(cloud_in, ConcentricZoneModel_);\n\n t2 = ros::Time::now().toSec();\n \n int concentric_idx = 0;\n\n double t_sort = 0;\n\n std::vector> candidates;\n std::vector ringwise_flatness;\n \n for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) {\n \n auto zone = ConcentricZoneModel_[zone_idx];\n\n for (int ring_idx = 0; ring_idx < num_rings_each_zone_[zone_idx]; ++ring_idx) {\n for (int sector_idx = 0; sector_idx < num_sectors_each_zone_[zone_idx]; ++sector_idx) {\n \n if (zone[ring_idx][sector_idx].points.size() < num_min_pts_)\n {\n cloud_nonground += zone[ring_idx][sector_idx];\n continue;\n }\n\n // --------- region-wise sorting (faster than global sorting method) ---------------- //\n double t_sort_0 = ros::Time::now().toSec();\n \n sort(zone[ring_idx][sector_idx].points.begin(), zone[ring_idx][sector_idx].points.end(), point_z_cmp);\n \n double t_sort_1 = ros::Time::now().toSec();\n t_sort += (t_sort_1 - t_sort_0);\n // ---------------------------------------------------------------------------------- //\n\n double t_tmp0 = ros::Time::now().toSec();\n extract_piecewiseground(zone_idx, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_);\n \n double t_tmp1 = ros::Time::now().toSec();\n t_total_ground += t_tmp1 - t_tmp0;\n pca_time_ += t_tmp1 - t_tmp0;\n\n // Status of each patch\n // used in checking uprightness, elevation, and flatness, respectively\n const double ground_uprightness = normal_(2);\n const double ground_elevation = pc_mean_(2, 0);\n const double ground_flatness = singular_values_.minCoeff();\n const double line_variable = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits::max();\n \n double heading = 0.0;\n for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i);\n\n if (visualize_) {\n auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3);\n polygons.header = poly_list_.header;\n poly_list_.polygons.push_back(polygons);\n set_ground_likelihood_estimation_status(zone_idx, ring_idx, concentric_idx, ground_uprightness, ground_elevation, ground_flatness);\n\n pcl::PointXYZINormal tmp_p;\n tmp_p.x = pc_mean_(0,0);\n tmp_p.y = pc_mean_(1,0);\n tmp_p.z = pc_mean_(2,0);\n tmp_p.normal_x = normal_(0);\n tmp_p.normal_y = normal_(1);\n tmp_p.normal_z = normal_(2);\n normals_.points.emplace_back(tmp_p);\n }\n\n double t_tmp2 = ros::Time::now().toSec();\n\n /* \n About 'is_heading_outside' condidition, heading should be smaller than 0 theoretically.\n ( Imagine the geometric relationship between the surface normal vector on the ground plane and \n the vector connecting the sensor origin and the mean point of the ground plane )\n\n However, when the patch is far awaw from the sensor origin, \n heading could be larger than 0 even if it's ground due to lack of amount of ground plane points.\n \n Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition )\n */\n bool is_upright = ground_uprightness > uprightness_thr_;\n bool is_not_elevated = ground_elevation < elevation_thr_[concentric_idx];\n bool is_flat = ground_flatness < flatness_thr_[concentric_idx];\n bool is_near_zone = concentric_idx < num_rings_of_interest_;\n bool is_heading_outside = heading < 0.0;\n\n /*\n Store the elevation & flatness variables\n for A-GLE (Adaptive Ground Likelihood Estimation)\n and TGR (Temporal Ground Revert). More information in the paper Patchwork++.\n */\n if (is_upright && is_not_elevated && is_near_zone)\n {\n update_elevation_[concentric_idx].push_back(ground_elevation);\n update_flatness_[concentric_idx].push_back(ground_flatness);\n\n ringwise_flatness.push_back(ground_flatness);\n }\n\n // Ground estimation based on conditions\n if (!is_upright)\n {\n cloud_nonground += regionwise_ground_;\n }\n else if (!is_near_zone)\n {\n cloud_ground += regionwise_ground_;\n }\n else if (!is_heading_outside)\n {\n cloud_nonground += regionwise_ground_;\n }\n else if (is_not_elevated || is_flat)\n {\n cloud_ground += regionwise_ground_;\n }\n else\n {\n RevertCandidate candidate(concentric_idx, sector_idx, ground_flatness, line_variable, pc_mean_, regionwise_ground_);\n candidates.push_back(candidate);\n }\n // Every regionwise_nonground is considered nonground.\n cloud_nonground += regionwise_nonground_;\n\n double t_tmp3 = ros::Time::now().toSec();\n t_total_estimate += t_tmp3 - t_tmp2;\n }\n\n double t_bef_revert = ros::Time::now().toSec();\n\n if (!candidates.empty())\n {\n if (enable_TGR_)\n {\n temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx);\n }\n else\n {\n for (size_t i=0; i inline\nvoid PatchWorkpp::update_elevation_thr(void)\n{\n for (int i=0; i 0) update_elevation_[i].erase(update_elevation_[i].begin(), update_elevation_[i].begin() + exceed_num);\n }\n \n if (verbose_)\n {\n cout << \"sensor height: \" << sensor_height_ << endl;\n cout << (boost::format(\"elevation_thr_ : %0.4f, %0.4f, %0.4f, %0.4f\")\n % elevation_thr_[0] % elevation_thr_[1] % elevation_thr_[2] % elevation_thr_[3]).str() << endl;\n }\n \n return;\n}\n\ntemplate inline\nvoid PatchWorkpp::update_flatness_thr(void)\n{\n for (int i=0; i 0) update_flatness_[i].erase(update_flatness_[i].begin(), update_flatness_[i].begin() + exceed_num);\n }\n \n if (verbose_)\n {\n cout << (boost::format(\"flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f\") \n % flatness_thr_[0] % flatness_thr_[1] % flatness_thr_[2] % flatness_thr_[3]).str() << endl;\n }\n \n return;\n}\n\ntemplate inline\nvoid PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground,\n std::vector ring_flatness, std::vector> candidates,\n int concentric_idx)\n{\n if (verbose_) std::cout << \"\\033[1;34m\" << \"=========== Temporal Ground Revert (TGR) ===========\" << \"\\033[0m\" << endl;\n\n double mean_flatness = 0.0, stdev_flatness = 0.0;\n calc_mean_stdev(ring_flatness, mean_flatness, stdev_flatness);\n \n if (verbose_)\n {\n cout << \"[\" << candidates[0].concentric_idx << \", \" << candidates[0].sector_idx << \"]\"\n << \" mean_flatness: \" << mean_flatness << \", stdev_flatness: \" << stdev_flatness << std::endl;\n }\n\n for( size_t i=0; i candidate = candidates[i];\n \n // Debug\n if(verbose_)\n {\n cout << \"\\033[1;33m\" << candidate.sector_idx << \"th flat_sector_candidate\"\n << \" / flatness: \" << candidate.ground_flatness\n << \" / line_variable: \" << candidate.line_variable\n << \" / ground_num : \" << candidate.regionwise_ground.size() \n << \"\\033[0m\" << endl;\n }\n\n double mu_flatness = mean_flatness + 1.5*stdev_flatness;\n double prob_flatness = 1/(1+exp( (candidate.ground_flatness-mu_flatness)/(mu_flatness/10) ));\n\n if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < th_dist_*th_dist_) prob_flatness = 1.0;\n\n double prob_line = 1.0;\n if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)// candidate.ground_elevation > elevation_thr_[concentric_idx]) \n {\n // if (verbose_) cout << \"line_dir: \" << candidate.line_dir << endl;\n prob_line = 0.0;\n }\n\n bool revert = prob_line*prob_flatness > 0.5;\n\n if ( concentric_idx < num_rings_of_interest_ )\n {\n if (revert)\n {\n if (verbose_)\n {\n cout << \"\\033[1;32m\" << \"REVERT TRUE\" << \"\\033[0m\" << endl;\n }\n \n revert_pc_ += candidate.regionwise_ground;\n cloud_ground += candidate.regionwise_ground;\n }\n else\n {\n if (verbose_) \n {\n cout << \"\\033[1;31m\" << \"FINAL REJECT\" << \"\\033[0m\" << endl;\n }\n reject_pc_ += candidate.regionwise_ground;\n cloud_nonground += candidate.regionwise_ground;\n } \n }\n }\n\n if (verbose_) std::cout << \"\\033[1;34m\" << \"====================================================\" << \"\\033[0m\" << endl;\n}\n\n// For adaptive\ntemplate inline\nvoid PatchWorkpp::extract_piecewiseground(\n const int zone_idx, const pcl::PointCloud &src,\n pcl::PointCloud &dst,\n pcl::PointCloud &non_ground_dst) {\n \n // 0. Initialization\n if (!ground_pc_.empty()) ground_pc_.clear();\n if (!dst.empty()) dst.clear();\n if (!non_ground_dst.empty()) non_ground_dst.clear();\n \n // 1. Region-wise Vertical Plane Fitting (R-VPF) \n // : removes potential vertical plane under the ground plane\n pcl::PointCloud src_wo_verticals;\n src_wo_verticals = src;\n\n if (enable_RVPF_)\n {\n for (int i = 0; i < num_iter_; i++)\n {\n extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_);\n estimate_plane(ground_pc_);\n\n if (zone_idx == 0 && normal_(2) < uprightness_thr_)\n {\n pcl::PointCloud src_tmp;\n src_tmp = src_wo_verticals;\n src_wo_verticals.clear();\n \n Eigen::MatrixXf points(src_tmp.points.size(), 3);\n int j = 0;\n for (auto &p:src_tmp.points) {\n points.row(j++) << p.x, p.y, p.z;\n }\n // ground plane model\n Eigen::VectorXf result = points * normal_;\n\n for (int r = 0; r < result.rows(); r++) {\n if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) {\n non_ground_dst.points.push_back(src_tmp[r]);\n vertical_pc_.points.push_back(src_tmp[r]);\n } else {\n src_wo_verticals.points.push_back(src_tmp[r]);\n }\n }\n }\n else break;\n }\n }\n \n extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_);\n estimate_plane(ground_pc_);\n\n // 2. Region-wise Ground Plane Fitting (R-GPF)\n // : fits the ground plane\n\n //pointcloud to matrix\n Eigen::MatrixXf points(src_wo_verticals.points.size(), 3);\n int j = 0;\n for (auto &p:src_wo_verticals.points) {\n points.row(j++) << p.x, p.y, p.z;\n }\n\n for (int i = 0; i < num_iter_; i++) {\n\n ground_pc_.clear();\n \n // ground plane model\n Eigen::VectorXf result = points * normal_;\n // threshold filter\n for (int r = 0; r < result.rows(); r++) {\n if (i < num_iter_ - 1) {\n if (result[r] < th_dist_ - d_ ) {\n ground_pc_.points.push_back(src_wo_verticals[r]);\n }\n } else { // Final stage\n if (result[r] < th_dist_ - d_ ) {\n dst.points.push_back(src_wo_verticals[r]);\n } else {\n non_ground_dst.points.push_back(src_wo_verticals[r]);\n }\n }\n }\n\n if (i < num_iter_ -1) estimate_plane(ground_pc_);\n else estimate_plane(dst);\n }\n}\n\ntemplate inline\ngeometry_msgs::PolygonStamped PatchWorkpp::set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split) {\n geometry_msgs::PolygonStamped polygons;\n // Set point of polygon. Start from RL and ccw\n geometry_msgs::Point32 point;\n\n // RL\n double zone_min_range = min_ranges_[zone_idx];\n double r_len = r_idx * ring_sizes_[zone_idx] + zone_min_range;\n double angle = theta_idx * sector_sizes_[zone_idx];\n\n point.x = r_len * cos(angle);\n point.y = r_len * sin(angle);\n point.z = MARKER_Z_VALUE;\n polygons.polygon.points.push_back(point);\n // RU\n r_len = r_len + ring_sizes_[zone_idx];\n point.x = r_len * cos(angle);\n point.y = r_len * sin(angle);\n point.z = MARKER_Z_VALUE;\n polygons.polygon.points.push_back(point);\n\n // RU -> LU\n for (int idx = 1; idx <= num_split; ++idx) {\n angle = angle + sector_sizes_[zone_idx] / num_split;\n point.x = r_len * cos(angle);\n point.y = r_len * sin(angle);\n point.z = MARKER_Z_VALUE;\n polygons.polygon.points.push_back(point);\n }\n\n r_len = r_len - ring_sizes_[zone_idx];\n point.x = r_len * cos(angle);\n point.y = r_len * sin(angle);\n point.z = MARKER_Z_VALUE;\n polygons.polygon.points.push_back(point);\n\n for (int idx = 1; idx < num_split; ++idx) {\n angle = angle - sector_sizes_[zone_idx] / num_split;\n point.x = r_len * cos(angle);\n point.y = r_len * sin(angle);\n point.z = MARKER_Z_VALUE;\n polygons.polygon.points.push_back(point);\n }\n\n return polygons;\n}\n\ntemplate inline\nvoid PatchWorkpp::set_ground_likelihood_estimation_status(\n const int zone_idx, const int ring_idx,\n const int concentric_idx,\n const double z_vec,\n const double z_elevation,\n const double ground_flatness) {\n if (z_vec > uprightness_thr_) { //orthogonal\n if (concentric_idx < num_rings_of_interest_) {\n if (z_elevation > elevation_thr_[concentric_idx]) {\n if (flatness_thr_[concentric_idx] > ground_flatness) {\n poly_list_.likelihood.push_back(FLAT_ENOUGH);\n } else {\n poly_list_.likelihood.push_back(TOO_HIGH_ELEVATION);\n }\n } else {\n poly_list_.likelihood.push_back(UPRIGHT_ENOUGH);\n }\n } else {\n poly_list_.likelihood.push_back(UPRIGHT_ENOUGH);\n }\n } else { // tilted\n poly_list_.likelihood.push_back(TOO_TILTED);\n }\n}\n\ntemplate inline\nvoid PatchWorkpp::calc_mean_stdev(std::vector vec, double &mean, double &stdev)\n{\n if (vec.size() <= 1) return;\n\n mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size();\n\n for (int i=0; i inline\ndouble PatchWorkpp::xy2theta(const double &x, const double &y) { // 0 ~ 2 * PI\n // if (y >= 0) {\n // return atan2(y, x); // 1, 2 quadrant\n // } else {\n // return 2 * M_PI + atan2(y, x);// 3, 4 quadrant\n // }\n\n double angle = atan2(y, x);\n return angle > 0 ? angle : 2*M_PI+angle;\n}\n\ntemplate inline\ndouble PatchWorkpp::xy2radius(const double &x, const double &y) {\n return sqrt(pow(x, 2) + pow(y, 2));\n}\n\ntemplate inline\nvoid PatchWorkpp::pc2czm(const pcl::PointCloud &src, std::vector &czm) {\n\n for (auto const &pt : src.points) {\n // int ring_idx, sector_idx;\n if (pt.z == std::numeric_limits::min()) continue;\n\n double r = xy2radius(pt.x, pt.y);\n if ((r <= max_range_) && (r > min_range_)) {\n double theta = xy2theta(pt.x, pt.y);\n \n int zone_idx = 0;\n if ( r < min_ranges_[1] ) zone_idx = 0;\n else if ( r < min_ranges_[2] ) zone_idx = 1;\n else if ( r < min_ranges_[3] ) zone_idx = 2;\n else zone_idx = 3;\n \n int ring_idx = min(static_cast(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1);\n int sector_idx = min(static_cast((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1);\n \n czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt);\n }\n\n }\n\n if (verbose_) cout << \"[ CZM ] Divides pointcloud into the concentric zone model\" << endl;\n}\n\n#endif\n" }, { "path": "include/GroundSegmentation/utils.hpp", "content": "#ifndef COMMON_H\n#define COMMON_H\n\n#include \"math.h\"\n#include \n#include \n#include \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\n#include \n// CLASSES\n#define SENSOR_HEIGHT 1.73\n\n#define UNLABELED 0\n#define OUTLIER 1\n#define NUM_ALL_CLASSES 34\n#define ROAD 40\n#define PARKING 44\n#define SIDEWALKR 48\n#define OTHER_GROUND 49\n#define BUILDING 50\n#define FENSE 51\n#define LANE_MARKING 60\n#define VEGETATION 70\n#define TERRAIN 72\n\n#define TRUEPOSITIVE 3\n#define TRUENEGATIVE 2\n#define FALSEPOSITIVE 1\n#define FALSENEGATIVE 0\n\nint NUM_ZEROS = 5;\n\nusing namespace std;\n\ndouble VEGETATION_THR = - SENSOR_HEIGHT * 3 / 4;\n /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */\nstruct PointXYZILID\n{\n PCL_ADD_POINT4D; // quad-word XYZ\n float intensity; ///< laser intensity reading\n uint16_t label; ///< point label\n uint16_t id;\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment\n} EIGEN_ALIGN16;\n\n// Register custom point struct according to PCL\nPOINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID,\n (float, x, x)\n (float, y, y)\n (float, z, z)\n (float, intensity, intensity)\n (uint16_t, label, label)\n (uint16_t, id, id))\n\n\nvoid PointXYZILID2XYZI(pcl::PointCloud& src,\n pcl::PointCloud::Ptr dst){\n dst->points.clear();\n for (const auto &pt: src.points){\n pcl::PointXYZI pt_xyzi;\n pt_xyzi.x = pt.x;\n pt_xyzi.y = pt.y;\n pt_xyzi.z = pt.z;\n pt_xyzi.intensity = pt.intensity;\n dst->points.push_back(pt_xyzi);\n }\n}\nstd::vector outlier_classes = {UNLABELED, OUTLIER};\nstd::vector ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, VEGETATION, TERRAIN};\nstd::vector ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING};\nstd::vector traversable_ground_classes = {ROAD, PARKING, LANE_MARKING, OTHER_GROUND};\n\nint count_num_ground(const pcl::PointCloud& pc){\n int num_ground = 0;\n\n std::vector::iterator iter;\n\n for (auto const& pt: pc.points){\n iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label);\n if (iter != ground_classes.end()){ // corresponding class is in ground classes\n if (pt.label == VEGETATION){\n if (pt.z < VEGETATION_THR){\n num_ground ++;\n }\n }else num_ground ++;\n }\n }\n return num_ground;\n}\n\nint count_num_ground_without_vegetation(const pcl::PointCloud& pc){\n int num_ground = 0;\n\n std::vector::iterator iter;\n\n std::vector classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, TERRAIN};\n\n for (auto const& pt: pc.points){\n iter = std::find(classes.begin(), classes.end(), pt.label);\n if (iter != classes.end()){ // corresponding class is in ground classes\n num_ground ++;\n }\n }\n return num_ground;\n}\n\nstd::map set_initial_gt_counts(std::vector& gt_classes){\n map gt_counts;\n for (int i = 0; i< gt_classes.size(); ++i){\n gt_counts.insert(pair(gt_classes.at(i), 0));\n }\n return gt_counts;\n}\n\nstd::map count_num_each_class(const pcl::PointCloud& pc){\n int num_ground = 0;\n auto gt_counts = set_initial_gt_counts(ground_classes);\n std::vector::iterator iter;\n\n for (auto const& pt: pc.points){\n iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label);\n if (iter != ground_classes.end()){ // corresponding class is in ground classes\n if (pt.label == VEGETATION){\n if (pt.z < VEGETATION_THR){\n gt_counts.find(pt.label)->second++;\n }\n }else gt_counts.find(pt.label)->second++;\n }\n }\n return gt_counts;\n}\n\nint count_num_outliers(const pcl::PointCloud& pc){\n int num_outliers = 0;\n\n std::vector::iterator iter;\n\n for (auto const& pt: pc.points){\n iter = std::find(outlier_classes.begin(), outlier_classes.end(), pt.label);\n if (iter != outlier_classes.end()){ // corresponding class is in ground classes\n num_outliers ++;\n }\n }\n return num_outliers;\n}\n\n\nvoid discern_ground(const pcl::PointCloud& src, pcl::PointCloud& ground, pcl::PointCloud& non_ground){\n ground.clear();\n non_ground.clear();\n std::vector::iterator iter;\n for (auto const& pt: src.points){\n if (pt.label == UNLABELED || pt.label == OUTLIER) continue;\n iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label);\n if (iter != ground_classes.end()){ // corresponding class is in ground classes\n if (pt.label == VEGETATION){\n if (pt.z < VEGETATION_THR){\n ground.push_back(pt);\n }else non_ground.push_back(pt);\n }else ground.push_back(pt);\n }else{\n non_ground.push_back(pt);\n }\n }\n}\n\nvoid discern_ground_without_vegetation(const pcl::PointCloud& src, pcl::PointCloud& ground, pcl::PointCloud& non_ground){\n ground.clear();\n non_ground.clear();\n std::vector::iterator iter;\n for (auto const& pt: src.points){\n if (pt.label == UNLABELED || pt.label == OUTLIER) continue;\n iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label);\n if (iter != ground_classes.end()){ // corresponding class is in ground classes\n if (pt.label != VEGETATION) ground.push_back(pt);\n }else{\n non_ground.push_back(pt);\n }\n }\n}\n\n\nvoid calculate_precision_recall(const pcl::PointCloud& pc_curr,\n pcl::PointCloud& ground_estimated,\n double & precision,\n double& recall,\n bool consider_outliers=true){\n\n int num_ground_est = ground_estimated.points.size();\n int num_ground_gt = count_num_ground(pc_curr);\n int num_TP = count_num_ground(ground_estimated);\n if (consider_outliers){\n int num_outliers_est = count_num_outliers(ground_estimated);\n precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100;\n recall = (double)(num_TP)/num_ground_gt * 100;\n }else{\n precision = (double)(num_TP)/num_ground_est * 100;\n recall = (double)(num_TP)/num_ground_gt * 100;\n }\n}\n\nvoid calculate_precision_recall_without_vegetation(const pcl::PointCloud& pc_curr,\n pcl::PointCloud& ground_estimated,\n double & precision,\n double& recall,\n bool consider_outliers=true){\n int num_veg = 0;\n for (auto const& pt: ground_estimated.points)\n {\n if (pt.label == VEGETATION) num_veg++;\n }\n\n int num_ground_est = ground_estimated.size() - num_veg;\n int num_ground_gt = count_num_ground_without_vegetation(pc_curr);\n int num_TP = count_num_ground_without_vegetation(ground_estimated);\n if (consider_outliers){\n int num_outliers_est = count_num_outliers(ground_estimated);\n precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100;\n recall = (double)(num_TP)/num_ground_gt * 100;\n }else{\n precision = (double)(num_TP)/num_ground_est * 100;\n recall = (double)(num_TP)/num_ground_gt * 100;\n }\n}\n\n\nint save_all_labels(const pcl::PointCloud& pc, string ABS_DIR, string seq, int count){\n\n std::string count_str = std::to_string(count);\n std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str;\n std::string output_filename = ABS_DIR + \"/\" + seq + \"/\" + count_str_padded + \".csv\";\n ofstream sc_output(output_filename);\n\n vector labels(NUM_ALL_CLASSES, 0);\n for (auto const& pt: pc.points){\n if (pt.label == 0) labels[0]++;\n else if (pt.label == 1) labels[1]++;\n else if (pt.label == 10) labels[2]++;\n else if (pt.label == 11) labels[3]++;\n else if (pt.label == 13) labels[4]++;\n else if (pt.label == 15) labels[5]++;\n else if (pt.label == 16) labels[6]++;\n else if (pt.label == 18) labels[7]++;\n else if (pt.label == 20) labels[8]++;\n else if (pt.label == 30) labels[9]++;\n else if (pt.label == 31) labels[10]++;\n else if (pt.label == 32) labels[11]++;\n else if (pt.label == 40) labels[12]++;\n else if (pt.label == 44) labels[13]++;\n else if (pt.label == 48) labels[14]++;\n else if (pt.label == 49) labels[15]++;\n else if (pt.label == 50) labels[16]++;\n else if (pt.label == 51) labels[17]++;\n else if (pt.label == 52) labels[18]++;\n else if (pt.label == 60) labels[19]++;\n else if (pt.label == 70) labels[20]++;\n else if (pt.label == 71) labels[21]++;\n else if (pt.label == 72) labels[22]++;\n else if (pt.label == 80) labels[23]++;\n else if (pt.label == 81) labels[24]++;\n else if (pt.label == 99) labels[25]++;\n else if (pt.label == 252) labels[26]++;\n else if (pt.label == 253) labels[27]++;\n else if (pt.label == 254) labels[28]++;\n else if (pt.label == 255) labels[29]++;\n else if (pt.label == 256) labels[30]++;\n else if (pt.label == 257) labels[31]++;\n else if (pt.label == 258) labels[32]++;\n else if (pt.label == 259) labels[33]++;\n }\n\n for (uint8_t i=0; i < NUM_ALL_CLASSES;++i){\n if (i!=33){\n sc_output<& pc_curr,\n pcl::PointCloud& ground_estimated, string acc_filename,\n double& accuracy, map&pc_curr_gt_counts, map&g_est_gt_counts){\n\n\n// std::cout<<\"debug: \"<(num_TP + (num_False - num_FP)) / num_total_gt * 100.0;\n\n pc_curr_gt_counts = count_num_each_class(pc_curr);\n g_est_gt_counts = count_num_each_class(ground_estimated);\n\n // save output\n for (auto const& class_id: ground_classes){\n sc_output2<second<<\",\"<second<<\",\";\n }\n sc_output2<& TP, const pcl::PointCloud& FP,\n const pcl::PointCloud& FN, const pcl::PointCloud& TN,\n std::string pcd_filename){\n pcl::PointCloud pc_out;\n\n for (auto const pt: TP.points){\n pcl::PointXYZI pt_est;\n pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;\n pt_est.intensity = TRUEPOSITIVE;\n pc_out.points.push_back(pt_est);\n }\n for (auto const pt: FP.points){\n pcl::PointXYZI pt_est;\n pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;\n pt_est.intensity = FALSEPOSITIVE;\n pc_out.points.push_back(pt_est);\n }\n for (auto const pt: FN.points){\n pcl::PointXYZI pt_est;\n pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;\n pt_est.intensity = FALSENEGATIVE;\n pc_out.points.push_back(pt_est);\n }\n for (auto const pt: TN.points){\n pcl::PointXYZI pt_est;\n pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z;\n pt_est.intensity = TRUENEGATIVE;\n pc_out.points.push_back(pt_est);\n }\n pc_out.width = pc_out.points.size();\n pc_out.height = 1;\n pcl::io::savePCDFileASCII(pcd_filename, pc_out);\n\n}\n\n\n#endif\n" }, { "path": "include/IMU_Processing.hpp", "content": "#ifndef _IMU_PROCESSING_HPP\n#define _IMU_PROCESSING_HPP\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#include \n#include \n\n/// *************Preconfiguration\n\n#define MAX_INI_COUNT (200)\n\nconst bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};\n\n/// *************IMU Process and undistortion\nclass ImuProcess\n{\n public:\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_R_LI_cov(const V3D &R_LI_cov);\n void set_T_LI_cov(const V3D &T_LI_cov);\n void set_gyr_cov(const V3D &scaler);\n void set_acc_cov(const V3D &scaler);\n void set_mean_acc_norm(const double &mean_acc_norm);\n void set_gyr_bias_cov(const V3D &b_g);\n void set_acc_bias_cov(const V3D &b_a);\n void Process(const MeasureGroup &meas, StatesGroup &state, PointCloudXYZI::Ptr pcl_un_);\n\n\n// ros::NodeHandle nh;\n ofstream fout_imu;\n V3D cov_acc;\n V3D cov_gyr;\n V3D cov_R_LI;\n V3D cov_T_LI;\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 int lidar_type;\n bool imu_en;\n bool Gril_Calib_done = false;\n double IMU_mean_acc_norm;\n\n private:\n void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N);\n void propagation_and_undist(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_in_out);\n void Forward_propagation_without_imu(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out);\n PointCloudXYZI::Ptr cur_pcl_un_;\n sensor_msgs::ImuConstPtr last_imu_;\n deque v_imu_;\n vector IMUpose;\n V3D mean_acc;\n V3D mean_gyr;\n V3D angvel_last;\n V3D acc_s_last;\n double last_lidar_end_time_;\n double time_last_scan;\n int init_iter_num = 1;\n bool b_first_frame_ = true;\n bool imu_need_init_ = true;\n};\n\nImuProcess::ImuProcess()\n : b_first_frame_(true), imu_need_init_(true)\n{\n imu_en = true;\n init_iter_num = 1;\n cov_acc = V3D(0.1, 0.1, 0.1);\n cov_gyr = V3D(0.1, 0.1, 0.1);\n cov_R_LI = V3D(0.00001, 0.00001, 0.00001);\n cov_T_LI = V3D(0.0001, 0.0001, 0.0001);\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 last_imu_.reset(new sensor_msgs::Imu());\n fout_imu.open(DEBUG_FILE_DIR(\"imu.txt\"),ios::out);\n}\n\nImuProcess::~ImuProcess() {}\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;\n init_iter_num = 1;\n v_imu_.clear();\n IMUpose.clear();\n last_imu_.reset(new sensor_msgs::Imu());\n cur_pcl_un_.reset(new PointCloudXYZI());\n}\n\n\nvoid ImuProcess::set_gyr_cov(const V3D &scaler)\n{\n cov_gyr_scale = scaler;\n}\n\nvoid ImuProcess::set_acc_cov(const V3D &scaler)\n{\n cov_acc_scale = scaler;\n}\n\nvoid ImuProcess::set_R_LI_cov(const V3D &R_LI_cov)\n{\n cov_R_LI = R_LI_cov;\n}\n\nvoid ImuProcess::set_T_LI_cov(const V3D &T_LI_cov)\n{\n cov_T_LI = T_LI_cov;\n}\n\nvoid ImuProcess::set_mean_acc_norm(const double &mean_acc_norm){\n IMU_mean_acc_norm = mean_acc_norm;\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, StatesGroup &state_inout, int &N)\n{\n /** 1. initializing the gravity, gyro bias, acc and gyro covariance\n ** 2. normalize the acceleration measurements to unit gravity **/\n ROS_INFO(\"IMU Initializing: %.1f %%\", double(N) / MAX_INI_COUNT * 100);\n V3D cur_acc, cur_gyr;\n \n if (b_first_frame_)\n {\n Reset();\n N = 1;\n b_first_frame_ = false;\n const auto &imu_acc = meas.imu.front()->linear_acceleration;\n const auto &gyr_acc = meas.imu.front()->angular_velocity;\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;\n }\n\n for (const auto &imu : meas.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\n cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);\n cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);\n\n N ++;\n }\n\n state_inout.gravity = - mean_acc / mean_acc.norm() * G_m_s2;\n state_inout.rot_end = Eye3d;\n state_inout.bias_g.setZero();\n last_imu_ = meas.imu.back();\n}\n\n\n\nvoid ImuProcess::Forward_propagation_without_imu(const MeasureGroup &meas, StatesGroup &state_inout,\n PointCloudXYZI &pcl_out) {\n pcl_out = *(meas.lidar);\n /*** sort point clouds by offset time ***/\n const double &pcl_beg_time = meas.lidar_beg_time;\n sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);\n const double &pcl_end_offset_time = pcl_out.points.back().curvature / double(1000);\n\n MD(DIM_STATE, DIM_STATE) F_x, cov_w;\n double dt = 0.0;\n\n if (b_first_frame_) {\n dt = 0.1;\n b_first_frame_ = false;\n } else {\n dt = pcl_beg_time - time_last_scan;\n time_last_scan = pcl_beg_time;\n }\n\n /* covariance propagation */\n F_x.setIdentity();\n cov_w.setZero();\n /** In CV model, bias_g represents angular velocity **/\n /** In CV model，bias_a represents linear acceleration **/\n M3D Exp_f = Exp(state_inout.bias_g, dt);\n F_x.block<3, 3>(0, 0) = Exp(state_inout.bias_g, -dt);\n F_x.block<3, 3>(0, 15) = Eye3d * dt;\n F_x.block<3, 3>(3, 12) = Eye3d * dt;\n\n\n cov_w.block<3, 3>(15, 15).diagonal() = cov_gyr_scale * dt * dt;\n cov_w.block<3, 3>(12, 12).diagonal() = cov_acc_scale * dt * dt;\n\n /** Forward propagation of covariance**/\n state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w;\n\n /** Forward propagation of attitude **/\n state_inout.rot_end = state_inout.rot_end * Exp_f;\n\n /** Position Propagation **/\n state_inout.pos_end += state_inout.vel_end * dt;\n\n /**CV model： un-distort pcl using linear interpolation **/\n if(lidar_type != L515){\n auto it_pcl = pcl_out.points.end() - 1;\n double dt_j = 0.0;\n for(; it_pcl != pcl_out.points.begin(); it_pcl --)\n {\n dt_j= pcl_end_offset_time - it_pcl->curvature/double(1000);\n M3D R_jk(Exp(state_inout.bias_g, - dt_j));\n V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z);\n // Using rotation and translation to un-distort points\n V3D p_jk;\n p_jk = - state_inout.rot_end.transpose() * state_inout.vel_end * dt_j;\n\n V3D P_compensate = R_jk * P_j + p_jk;\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 }\n}\n\nvoid ImuProcess::propagation_and_undist(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out)\n{\n /*** add the imu of the last frame-tail to the current frame-head ***/\n pcl_out = *(meas.lidar);\n auto v_imu = meas.imu;\n v_imu.push_front(last_imu_);\n double imu_end_time = v_imu.back()->header.stamp.toSec();\n double pcl_beg_time, pcl_end_time;\n\n if (lidar_type == L515)\n {\n pcl_beg_time = last_lidar_end_time_;\n pcl_end_time = meas.lidar_beg_time;\n }\n else\n {\n pcl_beg_time = meas.lidar_beg_time;\n /*** sort point clouds by offset time ***/\n sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);\n pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000);\n }\n\n\n /*** Initialize IMU pose ***/\n IMUpose.clear();\n IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end));\n\n /*** forward propagation at each imu point ***/\n V3D acc_imu, angvel_avr, acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end);\n M3D R_imu(state_inout.rot_end);\n MD(DIM_STATE, DIM_STATE) F_x, cov_w;\n \n double dt = 0;\n for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)\n {\n auto &&head = *(it_imu);\n auto &&tail = *(it_imu + 1);\n\n if (tail->header.stamp.toSec() < last_lidar_end_time_) continue;\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\n\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 V3D angvel_now(head->angular_velocity.x, head->angular_velocity.y, head->angular_velocity.z);\n V3D acc_now(head->linear_acceleration.x, head->linear_acceleration.y, head->linear_acceleration.z);\n fout_imu << setw(10) << head->header.stamp.toSec() << \" \" << angvel_now.transpose()<< \" \" << acc_now.transpose() << endl;\n\n angvel_avr -= state_inout.bias_g;\n acc_avr = acc_avr / IMU_mean_acc_norm * G_m_s2 - state_inout.bias_a;\n\n if(head->header.stamp.toSec() < last_lidar_end_time_)\n dt = tail->header.stamp.toSec() - last_lidar_end_time_;\n else\n dt = tail->header.stamp.toSec() - head->header.stamp.toSec();\n \n /* covariance propagation */\n M3D acc_avr_skew;\n M3D Exp_f = Exp(angvel_avr, dt);\n acc_avr_skew<(0,0) = Exp(angvel_avr, -dt);\n F_x.block<3,3>(0,15) = - Eye3d * dt;\n F_x.block<3,3>(3,12) = Eye3d * dt;\n F_x.block<3,3>(12,0) = - R_imu * acc_avr_skew * dt;\n F_x.block<3,3>(12,18) = - R_imu * dt;\n F_x.block<3,3>(12,21) = Eye3d * dt;\n\n cov_w.block<3,3>(0,0).diagonal() = cov_gyr * dt * dt;\n cov_w.block<3,3>(6,6).diagonal() = cov_R_LI * dt * dt; \n cov_w.block<3,3>(9,9).diagonal() = cov_T_LI * dt * dt;\n cov_w.block<3,3>(12,12) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt;\n cov_w.block<3,3>(15,15).diagonal() = cov_bias_gyr * dt * dt; // bias gyro covariance\n cov_w.block<3,3>(18,18).diagonal() = cov_bias_acc * dt * dt; // bias acc covariance\n\n state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w;\n\n /* propagation of IMU attitude (global frame)*/\n R_imu = R_imu * Exp_f;\n\n /* Specific acceleration (global frame) of IMU */\n acc_imu = R_imu * acc_avr + state_inout.gravity;\n\n /* propagation of IMU position (global frame)*/\n pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt;\n\n /* velocity of IMU (global frame)*/\n vel_imu = vel_imu + acc_imu * dt;\n\n /* save the poses at each IMU measurements (global frame)*/\n angvel_last = angvel_avr;\n acc_s_last = acc_imu;\n double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;\n IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu));\n }\n\n /*** calculated the pos and attitude prediction at the frame-end ***/\n double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;\n dt = note * (pcl_end_time - imu_end_time);\n state_inout.vel_end = vel_imu + note * acc_imu * dt;\n state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt);\n state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * acc_imu * dt * dt;\n\n\n last_imu_ = meas.imu.back();\n last_lidar_end_time_ = pcl_end_time;\n\n if (lidar_type != L515)\n {\n auto pos_liD_e = state_inout.pos_end + state_inout.rot_end * state_inout.offset_T_L_I;\n // auto R_liD_e = state_inout.rot_end * Lidar_R_to_IMU;\n\n #ifdef DEBUG_PRINT\n cout<<\"[ IMU Process ]: vel \"<rot);\n acc_imu<acc);\n vel_imu<vel);\n pos_imu<pos);\n angvel_avr<gyr);\n\n for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)\n {\n dt = it_pcl->curvature / double(1000) - head->offset_time; //dt = t_j - t_i > 0\n\n /* Transform to the 'scan-end' IMU frame（I_k frame）using only rotation\n * Note: Compensation direction is INVERSE of Frame's moving direction\n * So if we want to compensate a point at timestamp-i to the frame-e\n * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */\n\n M3D R_i(R_imu * Exp(angvel_avr, dt));\n V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt + R_i * state_inout.offset_T_L_I - pos_liD_e);\n\n V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);\n V3D P_compensate = state_inout.rot_end.transpose() * (R_i * P_i + T_ei);\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()) break;\n }\n }\n }\n\n}\n\n\nvoid ImuProcess::Process(const MeasureGroup &meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_)\n{\n\n if (imu_en)\n {\n if(meas.imu.empty()) return;\n ROS_ASSERT(meas.lidar != nullptr);\n\n if (imu_need_init_)\n {\n if(!Gril_Calib_done){\n /// The very first lidar frame\n IMU_init(meas, stat, init_iter_num);\n imu_need_init_ = true;\n last_imu_ = meas.imu.back();\n if (init_iter_num > MAX_INI_COUNT)\n {\n cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);\n imu_need_init_ = false;\n\n cov_acc = cov_acc_scale;\n cov_gyr = cov_gyr_scale;\n\n ROS_INFO(\"IMU Initialization Done: Gravity: %.4f %.4f %.4f, Acc norm: %.4f\", stat.gravity[0], stat.gravity[1], stat.gravity[2], mean_acc.norm());\n IMU_mean_acc_norm = mean_acc.norm();\n }\n }\n else{\n cout << endl;\n printf(BOLDMAGENTA \"[Refinement] Switch to LIO mode, online refinement begins.\\n\\n\" RESET);\n last_imu_ = meas.imu.back();\n imu_need_init_ = false;\n cov_acc = cov_acc_scale;\n cov_gyr = cov_gyr_scale;\n }\n return;\n }\n // propagation_and_undist(meas, stat, *cur_pcl_un_);\n }\n\n else\n {\n Forward_propagation_without_imu(meas, stat, *cur_pcl_un_);\n }\n}\n#endif" }, { "path": "include/color.h", "content": "#ifndef COLOR_H\n#define COLOR_H\n\n#define RESET \"\\033[0m\"\n#define BLACK \"\\033[30m\" /* Black */\n#define RED \"\\033[31m\" /* Red */\n#define GREEN \"\\033[32m\" /* Green */\n#define YELLOW \"\\033[33m\" /* Yellow */\n#define BLUE \"\\033[34m\" /* Blue */\n#define MAGENTA \"\\033[35m\" /* Magenta */\n#define CYAN \"\\033[36m\" /* Cyan */\n#define WHITE \"\\033[37m\" /* White */\n#define REDPURPLE \"\\033[95m\" /* Red Purple */\n#define BOLDBLACK \"\\033[1m\\033[30m\" /* Bold Black */\n#define BOLDRED \"\\033[1m\\033[31m\" /* Bold Red */\n#define BOLDGREEN \"\\033[1m\\033[32m\" /* Bold Green */\n#define BOLDYELLOW \"\\033[1m\\033[33m\" /* Bold Yellow */\n#define BOLDBLUE \"\\033[1m\\033[34m\" /* Bold Blue */\n#define BOLDMAGENTA \"\\033[1m\\033[35m\" /* Bold Magenta */\n#define BOLDCYAN \"\\033[1m\\033[36m\" /* Bold Cyan */\n#define BOLDWHITE \"\\033[1m\\033[37m\" /* Bold White */\n#define BOLDREDPURPLE \"\\033[1m\\033[95m\" /* Bold Red Purple */\n\n#endif\n" }, { "path": "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#include \n#include \n#include \n\n// For ImageProjection.hpp\n#include \n#include \n#include \n#include \n\nusing namespace std;\nusing namespace Eigen;\n\n#define PBSTR \"||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\"\n#define PBWIDTH 30\n#define PI_M (3.14159265358)\n#define G_m_s2 (9.81) // Gravity const in GuangDong/China\n#define DIM_STATE (24) // Dimension of states (Let Dim(SO(3)) = 3)\n\n#define LIDAR_SP_LEN (2)\n#define INIT_COV (1)\n#define NUM_MATCH_POINTS (5)\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\n#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + \"Log/\"+ name))\n#define RESULT_FILE_DIR(name) (string(string(ROOT_DIR) + \"result/\"+ name))\n\ntypedef gril_calib::Pose6D Pose6D;\ntypedef pcl::PointXYZINormal PointType;\ntypedef pcl::PointXYZRGB PointTypeRGB;\ntypedef pcl::PointCloud PointCloudXYZI;\ntypedef pcl::PointCloud PointCloudXYZRGB;\ntypedef vector> PointVector;\ntypedef Vector3d V3D;\ntypedef Matrix3d M3D;\ntypedef Vector3f V3F;\ntypedef Vector2d V2D;\ntypedef Matrix2d M2D;\n\n#define MD(a,b) Matrix\n#define VD(a) Matrix\n\nconst M3D Eye3d(M3D::Identity());\nconst V3D Zero3d(0, 0, 0);\n\n// Vector3d Lidar_offset_to_IMU(0.05512, 0.02226, -0.0297); // Horizon\n// Vector3d Lidar_offset_to_IMU(0.04165, 0.02326, -0.0284); // Avia\n\nenum LID_TYPE{AVIA = 1, VELO, OUSTER, L515, PANDAR, VELO_NCLT, VELO_without_Time}; //{1, 2, 3}\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\t\tthis->rot_end = M3D::Identity();\n\t\tthis->pos_end = Zero3d;\n this->offset_R_L_I = M3D::Identity();\n this->offset_T_L_I = 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>(15,15) = MD(9,9)::Identity() * 0.00001;\n\t};\n\n StatesGroup(const StatesGroup& b) {\n\t\tthis->rot_end = b.rot_end;\n\t\tthis->pos_end = b.pos_end;\n this->offset_R_L_I = b.offset_R_L_I;\n this->offset_T_L_I = b.offset_T_L_I;\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\t};\n\n StatesGroup& operator=(const StatesGroup& b)\n\t{\n this->rot_end = b.rot_end;\n\t\tthis->pos_end = b.pos_end;\n this->offset_R_L_I = b.offset_R_L_I;\n this->offset_T_L_I = b.offset_T_L_I;\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\t};\n\n StatesGroup operator+(const Matrix &state_add)\n\t{\n StatesGroup a;\n\t\ta.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));\n\t\ta.pos_end = this->pos_end + state_add.block<3,1>(3,0);\n a.offset_R_L_I = this->offset_R_L_I * Exp(state_add(6,0), state_add(7,0), state_add(8,0));\n a.offset_T_L_I = this->offset_T_L_I + state_add.block<3,1>(9,0);\n a.vel_end = this->vel_end + state_add.block<3,1>(12,0);\n a.bias_g = this->bias_g + state_add.block<3,1>(15,0);\n a.bias_a = this->bias_a + state_add.block<3,1>(18,0);\n a.gravity = this->gravity + state_add.block<3,1>(21,0);\n a.cov = this->cov;\n\t\treturn a;\n\t};\n\n StatesGroup& operator+=(const Matrix &state_add)\n\t{\n this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));\n\t\tthis->pos_end += state_add.block<3,1>(3,0);\n this->offset_R_L_I = this->offset_R_L_I * Exp(state_add(6,0), state_add(7,0), state_add(8,0));\n this->offset_T_L_I += state_add.block<3,1>(9,0);\n this->vel_end += state_add.block<3,1>(12,0);\n this->bias_g += state_add.block<3,1>(15,0);\n this->bias_a += state_add.block<3,1>(18,0);\n this->gravity += state_add.block<3,1>(21,0);\n\t\treturn *this;\n\t};\n\n Matrix operator-(const StatesGroup& b)\n\t{\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 M3D offsetd(b.offset_R_L_I.transpose() * this->offset_R_L_I);\n a.block<3,1>(6,0) = Log(offsetd);\n a.block<3,1>(9,0) = this->offset_T_L_I - b.offset_T_L_I;\n a.block<3,1>(12,0) = this->vel_end - b.vel_end;\n a.block<3,1>(15,0) = this->bias_g - b.bias_g;\n a.block<3,1>(18,0) = this->bias_a - b.bias_a;\n a.block<3,1>(21,0) = this->gravity - b.gravity;\n\t\treturn a;\n\t};\n\n void resetpose()\n {\n this->rot_end = M3D::Identity();\n\t\tthis->pos_end = Zero3d;\n this->vel_end = Zero3d;\n }\n\n\tM3D 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 M3D offset_R_L_I; // Rotation from Lidar frame L to IMU frame I\n V3D offset_T_L_I; // Translation from Lidar frame L to IMU frame I (from IMU to LiDAR)\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\n// Taeyoung Add\n// Convert SI unit to g unit\ntemplate\nT SI2g(T SI)\n{\n return SI / G_m_s2;\n}\n\n// Convert g unit to SI unit\ntemplate\nT g2SI(T g)\n{\n return g * G_m_s2;\n}\n\n// So(2) rotation matrix\n// theta - radian\ntemplate\nM2D SO2(T theta)\n{\n M2D R;\n R << std::cos(theta), -std::sin(theta),\n std::sin(theta), std::cos(theta);\n return R;\n}\n\n// Skew matrix 2D\n// theta - radian\ntemplate\nM2D Skew2D(T theta)\n{\n M2D exp;\n exp << 0, -theta,\n theta, 0;\n return exp;\n}\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++) rot_kp.rot[i*3+j] = R(i,j);\n }\n // Map(rot_kp.rot, 3,3) = R;\n return move(rot_kp);\n}\n\nstatic Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R){\n Eigen::Vector3d n = R.col(0);\n Eigen::Vector3d o = R.col(1);\n Eigen::Vector3d a = R.col(2);\n\n Eigen::Vector3d ypr(3);\n double y = atan2(n(1), n(0));\n double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));\n double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));\n ypr(0) = y;\n ypr(1) = p;\n ypr(2) = r;\n\n return ypr / M_PI * 180.0;\n}\n\ntemplate \nstatic Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr)\n{\n typedef typename Derived::Scalar Scalar_t;\n\n Scalar_t y = ypr(0) / 180.0 * M_PI;\n Scalar_t p = ypr(1) / 180.0 * M_PI;\n Scalar_t r = ypr(2) / 180.0 * M_PI;\n\n Eigen::Matrix Rz;\n Rz << cos(y), -sin(y), 0,\n sin(y), cos(y), 0,\n 0, 0, 1;\n\n Eigen::Matrix Ry;\n Ry << cos(p), 0., sin(p),\n 0., 1., 0.,\n -sin(p), 0., cos(p);\n\n Eigen::Matrix Rx;\n Rx << 1., 0., 0.,\n 0., cos(r), -sin(r),\n 0., sin(r), cos(r);\n\n return Rz * Ry * Rx;\n}\n\n\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\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++)\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 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\n return true;\n}\n\n#endif" }, { "path": "include/ikd-Tree/README.md", "content": "# ikd-Tree\nikd-Tree is an incremental k-d tree for robotic applications.\n" }, { "path": "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 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 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 delete_criterion_param = delete_param;\n}\n\nvoid KD_TREE::Set_balance_criterion_param(float balance_param){\n balance_criterion_param = balance_param;\n}\n\nvoid KD_TREE::set_downsample_param(float downsample_param){\n downsample_size = downsample_param;\n}\n\nvoid KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length){\n Set_delete_criterion_param(delete_param);\n Set_balance_criterion_param(balance_param);\n set_downsample_param(box_length);\n}\n\nvoid KD_TREE::InitTreeNode(KD_TREE_NODE * root){\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_init(&(root->push_down_mutex_lock),NULL);\n} \n\nint KD_TREE::size(){\n int s = 0;\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){\n if (Root_Node != nullptr) {\n return Root_Node->TreeSize;\n } else {\n return 0;\n }\n } else {\n if (!pthread_mutex_trylock(&working_flag_mutex)){\n s = Root_Node->TreeSize;\n pthread_mutex_unlock(&working_flag_mutex);\n return s;\n } else {\n return Treesize_tmp;\n }\n }\n}\n\nBoxPointType KD_TREE::tree_range(){\n BoxPointType range;\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){\n if (Root_Node != nullptr) {\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 } else {\n memset(&range, 0, sizeof(range));\n }\n } else {\n if (!pthread_mutex_trylock(&working_flag_mutex)){\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 } else {\n memset(&range, 0, sizeof(range));\n }\n }\n return range;\n}\n\nint KD_TREE::validnum(){\n int s = 0;\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){\n if (Root_Node != nullptr)\n return (Root_Node->TreeSize - Root_Node->invalid_point_num);\n else \n return 0;\n } else {\n if (!pthread_mutex_trylock(&working_flag_mutex)){\n s = Root_Node->TreeSize-Root_Node->invalid_point_num;\n pthread_mutex_unlock(&working_flag_mutex);\n return s;\n } else {\n return -1;\n }\n }\n}\n\nvoid KD_TREE::root_alpha(float &alpha_bal, float &alpha_del){\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){\n alpha_bal = Root_Node->alpha_bal;\n alpha_del = Root_Node->alpha_del;\n return;\n } else {\n if (!pthread_mutex_trylock(&working_flag_mutex)){\n alpha_bal = Root_Node->alpha_bal;\n alpha_del = Root_Node->alpha_del;\n pthread_mutex_unlock(&working_flag_mutex);\n return;\n } else {\n alpha_bal = alpha_bal_tmp;\n alpha_del = alpha_del_tmp; \n return;\n }\n } \n}\n\nvoid KD_TREE::start_thread(){\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 pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void*) this);\n printf(\"Multi thread started \\n\"); \n}\n\nvoid KD_TREE::stop_thread(){\n pthread_mutex_lock(&termination_flag_mutex_lock);\n termination_flag = true;\n pthread_mutex_unlock(&termination_flag_mutex_lock);\n if (rebuild_thread) pthread_join(rebuild_thread, NULL);\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 KD_TREE * handle = (KD_TREE*) arg;\n handle->multi_thread_rebuild();\n return nullptr; \n} \n\nvoid KD_TREE::multi_thread_rebuild(){\n bool terminated = false;\n KD_TREE_NODE * father_ptr, ** new_node_ptr;\n pthread_mutex_lock(&termination_flag_mutex_lock);\n terminated = termination_flag;\n pthread_mutex_unlock(&termination_flag_mutex_lock);\n while (!terminated){\n pthread_mutex_lock(&rebuild_ptr_mutex_lock);\n pthread_mutex_lock(&working_flag_mutex);\n if (Rebuild_Ptr != nullptr ){ \n /* Traverse and copy */\n if (!Rebuild_Logger.empty()){\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 if (*Rebuild_Ptr == Root_Node) {\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 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 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 if (int(Rebuild_PCL_Storage.size()) > 0){\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 while (!Rebuild_Logger.empty()){\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 run_operation(&new_root_node, Operation);\n tmp_counter ++;\n if (tmp_counter % 10 == 0) 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 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 if (father_ptr->left_son_ptr == *Rebuild_Ptr) {\n father_ptr->left_son_ptr = new_root_node;\n } else if (father_ptr->right_son_ptr == *Rebuild_Ptr){ \n father_ptr->right_son_ptr = new_root_node;\n } else {\n throw \"Error: Father ptr incompatible with current node\\n\";\n }\n if (new_root_node != nullptr) new_root_node->father_ptr = father_ptr;\n (*Rebuild_Ptr) = new_root_node;\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) Root_Node = STATIC_ROOT_NODE->left_son_ptr;\n KD_TREE_NODE * update_root = *Rebuild_Ptr;\n while (update_root != nullptr && update_root != Root_Node){\n update_root = update_root->father_ptr;\n if (update_root->working_flag) break;\n if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) break;\n if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) 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 } else {\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 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) (*root)->down_del_num = (*root)->TreeSize;\n if (operation.tree_deleted) (*root)->invalid_point_num = (*root)->TreeSize;\n else (*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 if (Root_Node != nullptr){\n delete_tree_nodes(&Root_Node);\n }\n if (point_cloud.size() == 0) return;\n STATIC_ROOT_NODE = new KD_TREE_NODE;\n InitTreeNode(STATIC_ROOT_NODE); \n BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size()-1, point_cloud);\n Update(STATIC_ROOT_NODE);\n STATIC_ROOT_NODE->TreeSize = 0;\n Root_Node = STATIC_ROOT_NODE->left_son_ptr; \n}\n\nvoid KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector& Nearest_Points, vector & Point_Distance, double max_dist){ \n MANUAL_HEAP q(2*k_nearest);\n q.clear();\n vector ().swap(Point_Distance);\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){\n Search(Root_Node, k_nearest, point, q, max_dist);\n } else {\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 PointVector ().swap(Nearest_Points);\n vector ().swap(Point_Distance);\n for (int i=0;i < k_found;i++){\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\nint KD_TREE::Add_Points(PointVector & PointToAdd, bool downsample_on){\n int NewPointSize = PointToAdd.size();\n int tree_size = size();\n BoxPointType Box_of_Point;\n PointType downsample_result, mid_point;\n bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH;\n float min_dist, tmp_dist;\n int tmp_counter = 0;\n for (int i=0; i 1 || same_point(PointToAdd[i], downsample_result)){\n if (Downsample_Storage.size() > 0) 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 } else {\n if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)){\n Operation_Logger_Type operation_delete, operation;\n operation_delete.boxpoint = Box_of_Point;\n operation_delete.op = DOWNSAMPLE_DELETE;\n operation.point = downsample_result;\n operation.op = ADD_POINT;\n pthread_mutex_lock(&working_flag_mutex);\n if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, false , true); \n Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis);\n tmp_counter ++;\n if (rebuild_flag){\n pthread_mutex_lock(&rebuild_logger_mutex_lock);\n if (Downsample_Storage.size() > 0) 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 } else {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){\n Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); \n } else {\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 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\nvoid KD_TREE::Add_Point_Boxes(vector & BoxPoints){ \n for (int i=0;i < BoxPoints.size();i++){\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){\n Add_by_range(&Root_Node ,BoxPoints[i], true);\n } else {\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 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\nvoid KD_TREE::Delete_Points(PointVector & PointToDel){ \n for (int i=0;i & BoxPoints){\n int tmp_counter = 0;\n for (int i=0;i < BoxPoints.size();i++){ \n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ \n tmp_counter += Delete_by_range(&Root_Node ,BoxPoints[i], true, false);\n } else {\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 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 pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); \n for (int i = 0; i < Points_deleted.size();i++){\n removed_points.push_back(Points_deleted[i]);\n }\n for (int i = 0; i < Multithread_Points_deleted.size();i++){\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\nvoid KD_TREE::BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage){\n if (l>r) return;\n *root = new KD_TREE_NODE;\n InitTreeNode(*root);\n int mid = (l+r)>>1;\n int div_axis = 0;\n int i;\n // Find the best division Axis\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 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 for (i=0;i<3;i++) dim_range[i] = max_value[i] - min_value[i];\n for (i=1;i<3;i++) if (dim_range[i] > dim_range[div_axis]) div_axis = i;\n // Divide by the division axis and recursively build.\n\n (*root)->division_axis = div_axis;\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 BuildTree(&left_son, l, mid-1, Storage);\n BuildTree(&right_son, mid+1, r, Storage); \n (*root)->left_son_ptr = left_son;\n (*root)->right_son_ptr = right_son;\n Update((*root)); \n return;\n}\n\nvoid KD_TREE::Rebuild(KD_TREE_NODE ** root){ \n KD_TREE_NODE * father_ptr;\n if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) { \n if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)){ \n if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) {\n Rebuild_Ptr = root; \n }\n pthread_mutex_unlock(&rebuild_ptr_mutex_lock);\n }\n } else {\n father_ptr = (*root)->father_ptr;\n int size_rec = (*root)->TreeSize;\n PCL_Storage.clear();\n flatten(*root, PCL_Storage, DELETE_POINTS_REC);\n delete_tree_nodes(root);\n BuildTree(root, 0, PCL_Storage.size()-1, PCL_Storage);\n if (*root != nullptr) (*root)->father_ptr = father_ptr;\n if (*root == Root_Node) STATIC_ROOT_NODE->left_son_ptr = *root;\n } \n return;\n}\n\nint KD_TREE::Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample){ \n if ((*root) == nullptr || (*root)->tree_deleted) return 0;\n (*root)->working_flag = true;\n Push_Down(*root);\n int tmp_counter = 0;\n if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return 0;\n if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return 0;\n if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return 0;\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 (*root)->tree_deleted = true;\n (*root)->point_deleted = true;\n (*root)->need_push_down_to_left = true;\n (*root)->need_push_down_to_right = true;\n tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num;\n (*root)->invalid_point_num = (*root)->TreeSize;\n if (is_downsample){\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 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 (*root)->point_deleted = true;\n tmp_counter += 1;\n if (is_downsample) (*root)->point_downsample_deleted = true;\n }\n Operation_Logger_Type delete_box_log;\n struct timespec Timeout; \n if (is_downsample) delete_box_log.op = DOWNSAMPLE_DELETE;\n else delete_box_log.op = DELETE_BOX;\n delete_box_log.boxpoint = boxpoint;\n if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){\n tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample);\n } else {\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 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 tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample);\n } else {\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 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 Update(*root); \n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; \n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n if (need_rebuild) Rebuild(root);\n if ((*root) != nullptr) (*root)->working_flag = false;\n return tmp_counter;\n}\n\nvoid KD_TREE::Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild){ \n if ((*root) == nullptr || (*root)->tree_deleted) return;\n (*root)->working_flag = true;\n Push_Down(*root);\n if (same_point((*root)->point, point) && !(*root)->point_deleted) { \n (*root)->point_deleted = true;\n (*root)->invalid_point_num += 1;\n if ((*root)->invalid_point_num == (*root)->TreeSize) (*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 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 if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ \n Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); \n } else {\n pthread_mutex_lock(&working_flag_mutex);\n Delete_by_point(&(*root)->left_son_ptr, point,false);\n if (rebuild_flag){\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 } else { \n if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ \n Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); \n } else {\n pthread_mutex_lock(&working_flag_mutex); \n Delete_by_point(&(*root)->right_son_ptr, point, false);\n if (rebuild_flag){\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 Update(*root);\n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; \n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n if (need_rebuild) Rebuild(root);\n if ((*root) != nullptr) (*root)->working_flag = false; \n return;\n}\n\nvoid KD_TREE::Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild){\n if ((*root) == nullptr) return;\n (*root)->working_flag = true;\n Push_Down(*root); \n if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return;\n if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return;\n if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return;\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 (*root)->tree_deleted = false || (*root)->tree_downsample_deleted;\n (*root)->point_deleted = false || (*root)->point_downsample_deleted;\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 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 (*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 if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){\n Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild);\n } else {\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_range(&((*root)->left_son_ptr), boxpoint, false);\n if (rebuild_flag){\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 Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild);\n } else {\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_range(&((*root)->right_son_ptr), boxpoint, false);\n if (rebuild_flag){\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 Update(*root);\n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; \n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n if (need_rebuild) Rebuild(root);\n if ((*root) != nullptr) (*root)->working_flag = false; \n return;\n}\n\nvoid KD_TREE::Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis){ \n if (*root == nullptr){\n *root = new KD_TREE_NODE;\n InitTreeNode(*root);\n (*root)->point = point;\n (*root)->division_axis = (father_axis + 1) % 3;\n Update(*root);\n return;\n }\n (*root)->working_flag = true;\n Operation_Logger_Type add_log;\n struct timespec Timeout; \n add_log.op = ADD_POINT;\n add_log.point = point;\n Push_Down(*root);\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 if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ \n Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis);\n } else {\n pthread_mutex_lock(&working_flag_mutex);\n Add_by_point(&(*root)->left_son_ptr, point, false,(*root)->division_axis);\n if (rebuild_flag){\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 } else { \n if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ \n Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild,(*root)->division_axis);\n } else {\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 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 Update(*root); \n if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; \n bool need_rebuild = allow_rebuild & Criterion_Check((*root));\n if (need_rebuild) Rebuild(root); \n if ((*root) != nullptr) (*root)->working_flag = false; \n return;\n}\n\nvoid KD_TREE::Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist){\n if (root == nullptr || root->tree_deleted) return; \n double cur_dist = calc_box_dist(root, point);\n if (cur_dist > max_dist * max_dist) return; \n int retval; \n if (root->need_push_down_to_left || root->need_push_down_to_right) {\n retval = pthread_mutex_trylock(&(root->push_down_mutex_lock));\n if (retval == 0){\n Push_Down(root);\n pthread_mutex_unlock(&(root->push_down_mutex_lock));\n } else {\n pthread_mutex_lock(&(root->push_down_mutex_lock));\n pthread_mutex_unlock(&(root->push_down_mutex_lock));\n }\n }\n if (!root->point_deleted){\n float dist = calc_dist(point, root->point);\n if (dist <= max_dist && (q.size() < k_nearest || dist < q.top().dist)){\n if (q.size() >= k_nearest) q.pop();\n PointType_CMP current_point{root->point, dist}; \n q.push(current_point); \n }\n } \n int cur_search_counter;\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 if (q.size()< k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist){\n if (dist_left_node <= dist_right_node) {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){\n Search(root->left_son_ptr, k_nearest, point, q, max_dist); \n } else {\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 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){\n Search(root->right_son_ptr, k_nearest, point, q, max_dist); \n } else {\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 } else {\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){\n Search(root->right_son_ptr, k_nearest, point, q, max_dist); \n } else {\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 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){\n Search(root->left_son_ptr, k_nearest, point, q, max_dist); \n } else {\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 } else {\n if (dist_left_node < q.top().dist) { \n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){\n Search(root->left_son_ptr, k_nearest, point, q, max_dist); \n } else {\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 if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){\n Search(root->right_son_ptr, k_nearest, point, q, max_dist); \n } else {\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\nvoid KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector & Storage){\n if (root == nullptr) return;\n Push_Down(root); \n if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) return;\n if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) return;\n if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) return;\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 flatten(root, Storage, NOT_RECORD);\n return;\n }\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 if (!root->point_deleted) Storage.push_back(root->point);\n }\n if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr){\n Search_by_range(root->left_son_ptr, boxpoint, Storage);\n } else {\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 Search_by_range(root->right_son_ptr, boxpoint, Storage);\n } else {\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 if (root->TreeSize <= Minimal_Unbalanced_Tree_Size){\n return false;\n }\n float balance_evaluation = 0.0f;\n float delete_evaluation = 0.0f;\n KD_TREE_NODE * son_ptr = root->left_son_ptr;\n if (son_ptr == nullptr) son_ptr = root->right_son_ptr;\n delete_evaluation = float(root->invalid_point_num)/ root->TreeSize;\n balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize-1); \n if (delete_evaluation > delete_criterion_param){\n return true;\n }\n if (balance_evaluation > balance_criterion_param || balance_evaluation < 1-balance_criterion_param){\n return true;\n } \n return false;\n}\n\nvoid KD_TREE::Push_Down(KD_TREE_NODE *root){\n if (root == nullptr) return;\n Operation_Logger_Type operation;\n operation.op = PUSH_DOWN;\n operation.tree_deleted = root->tree_deleted;\n operation.tree_downsample_deleted = root->tree_downsample_deleted;\n if (root->need_push_down_to_left && root->left_son_ptr != nullptr){\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){\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) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;\n if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;\n else 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 root->need_push_down_to_left = false; \n } else {\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) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;\n if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;\n else 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 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 if (root->need_push_down_to_right && root->right_son_ptr != nullptr){\n if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){\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) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;\n if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;\n else 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 } else {\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) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;\n if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;\n else 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 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\nvoid KD_TREE::Update(KD_TREE_NODE * root){\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 if (left_son_ptr != nullptr && right_son_ptr != nullptr){\n root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1;\n root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted? 1:0);\n root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted? 1:0);\n root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;\n root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted;\n if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)){\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 } else {\n if (!left_son_ptr->tree_deleted){\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 (!right_son_ptr->tree_deleted){\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 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 } else if (left_son_ptr != nullptr){\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 if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)){\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 } else {\n if (!left_son_ptr->tree_deleted){\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 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 } else if (right_son_ptr != nullptr){\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 if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)){\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 } else {\n if (!right_son_ptr->tree_deleted){\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 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 } else {\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 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 if (left_son_ptr != nullptr) left_son_ptr -> father_ptr = root;\n if (right_son_ptr != nullptr) right_son_ptr -> father_ptr = root;\n if (root == Root_Node && root->TreeSize > 3){\n KD_TREE_NODE * son_ptr = root->left_son_ptr;\n if (son_ptr == nullptr) 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\nvoid KD_TREE::flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type){\n if (root == nullptr) return;\n Push_Down(root);\n if (!root->point_deleted) {\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 switch (storage_type)\n {\n case NOT_RECORD:\n break;\n case DELETE_POINTS_REC:\n if (root->point_deleted && !root->point_downsample_deleted) {\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 Multithread_Points_deleted.push_back(root->point);\n }\n break;\n default:\n break;\n } \n return;\n}\n\nvoid KD_TREE::delete_tree_nodes(KD_TREE_NODE ** root){ \n if (*root == nullptr) return;\n Push_Down(*root); \n delete_tree_nodes(&(*root)->left_son_ptr);\n delete_tree_nodes(&(*root)->right_son_ptr); \n pthread_mutex_destroy( &(*root)->push_down_mutex_lock); \n delete *root;\n *root = nullptr; \n\n return;\n}\n\nbool KD_TREE::same_point(PointType a, PointType b){\n return (fabs(a.x-b.x) < EPSS && fabs(a.y-b.y) < EPSS && fabs(a.z-b.z) < EPSS );\n}\n\nfloat KD_TREE::calc_dist(PointType a, PointType b){\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\nfloat KD_TREE::calc_box_dist(KD_TREE_NODE * node, PointType point){\n if (node == nullptr) return INFINITY;\n float min_dist = 0.0;\n if (point.x < node->node_range_x[0]) 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]) 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]) 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]) 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]) 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]) 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 cap = max_capacity;\n heap = new PointType_CMP[max_capacity];\n heap_size = 0;\n}\n\nMANUAL_HEAP::~MANUAL_HEAP(){\n delete[] heap;\n}\n\nvoid MANUAL_HEAP::pop(){\n if (heap_size == 0) return;\n heap[0] = heap[heap_size-1];\n heap_size--;\n MoveDown(0);\n return;\n}\n \nPointType_CMP MANUAL_HEAP::top(){\n return heap[0];\n}\n \nvoid MANUAL_HEAP::push(PointType_CMP point){\n if (heap_size >= cap) return;\n heap[heap_size] = point;\n FloatUp(heap_size);\n heap_size++;\n return;\n}\n \nint MANUAL_HEAP::size(){\n return heap_size;\n}\n \nvoid MANUAL_HEAP::clear(){\n heap_size = 0;\n return;\n}\n\nvoid MANUAL_HEAP::MoveDown(int heap_index){\n int l = heap_index * 2 + 1;\n PointType_CMP tmp = heap[heap_index];\n while (l < heap_size){\n if (l + 1 < heap_size && heap[l] < heap[l+1]) l++;\n if (tmp < heap[l]){\n heap[heap_index] = heap[l];\n heap_index = l;\n l = heap_index * 2 + 1;\n } else break;\n }\n heap[heap_index] = tmp;\n return;\n}\n \nvoid MANUAL_HEAP::FloatUp(int heap_index){\n int ancestor = (heap_index-1)/2;\n PointType_CMP tmp = heap[heap_index];\n while (heap_index > 0){\n if (heap[ancestor] < tmp){\n heap[heap_index] = heap[ancestor];\n heap_index = ancestor;\n ancestor = (heap_index-1)/2;\n } else break;\n }\n heap[heap_index] = tmp;\n return;\n}\n\n// manual queue\nvoid MANUAL_Q::clear(){\n head = 0;\n tail = 0;\n counter = 0;\n is_empty = true;\n return;\n}\n\nvoid MANUAL_Q::pop(){\n if (counter == 0) return;\n head ++;\n head %= Q_LEN;\n counter --;\n if (counter == 0) is_empty = true;\n return;\n}\n\nOperation_Logger_Type MANUAL_Q::front(){\n return q[head];\n}\n\nOperation_Logger_Type MANUAL_Q::back(){\n return q[tail];\n}\n\nvoid MANUAL_Q::push(Operation_Logger_Type op){\n q[tail] = op;\n counter ++;\n if (is_empty) is_empty = false;\n tail ++;\n tail %= Q_LEN;\n}\n\nbool MANUAL_Q::empty(){\n return is_empty;\n}\n\nint MANUAL_Q::size(){\n return counter;\n}\n\n\n" }, { "path": "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\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 PointType point;\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 KD_TREE_NODE *left_son_ptr = nullptr;\n KD_TREE_NODE *right_son_ptr = nullptr;\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 PointType point;\n float dist = 0.0;\n PointType_CMP (PointType p = ZeroP, float d = INFINITY){\n this->point = p;\n this->dist = d;\n };\n bool operator < (const PointType_CMP &a)const{\n if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x;\n else return dist < a.dist;\n } \n};\n\nstruct BoxPointType{\n float vertex_min[3];\n float vertex_max[3];\n};\n\nenum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN};\n\nenum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC};\n\nstruct Operation_Logger_Type{\n PointType point;\n BoxPointType boxpoint;\n bool tree_deleted, tree_downsample_deleted;\n operation_set op;\n};\n\nclass MANUAL_Q{\n private:\n int head = 0,tail = 0, counter = 0;\n Operation_Logger_Type q[Q_LEN];\n bool is_empty;\n public:\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\nclass MANUAL_HEAP\n{\n public:\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 private:\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\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; \n KD_TREE_NODE * Root_Node = nullptr;\n int max_queue_size = 0;\n};\n" }, { "path": "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 calibration 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 calibration 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": "include/preprocess.h", "content": "#ifndef PREPROCESS_H\n#define PREPROCESS_H\n\n#include \"common_lib.h\"\n#include \n#include \n#include \n#include \n\nusing namespace std;\n\n#define SCAN_RATE 10\n\nenum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};\nenum Surround{Prev, Next};\nenum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};\n\nconst bool time_list_cut_frame(PointType &x, PointType &y);\n\nstruct orgtype\n{\n double range;\n double dista; \n double angle[2];\n double intersect;\n E_jump edj[2];\n Feature ftype;\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\nnamespace velodyne_without_time {\n struct EIGEN_ALIGN16 Point {\n PCL_ADD_POINT4D;\n float intensity;\n // uint32_t t;\n uint16_t ring;\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n };\n} // namespace velodyne_without_time\nPOINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_without_time::Point,\n (float, x, x)\n (float, y, y)\n (float, z, z)\n (float, intensity, intensity)\n // (std::uint32_t, t, t)\n (uint16_t, ring, ring)\n)\n\nnamespace velodyne_ros {\n struct EIGEN_ALIGN16 Point {\n PCL_ADD_POINT4D;\n float intensity;\n float time;\n uint16_t ring;\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n };\n} // namespace velodyne_ros\nPOINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,\n (float, x, x)\n (float, y, y)\n (float, z, z)\n (float, intensity, intensity)\n (float, time, time)\n (uint16_t, ring, ring)\n)\n\nnamespace ouster_ros {\n struct EIGEN_ALIGN16 Point {\n PCL_ADD_POINT4D;\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\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// namespace pandar_ros\nnamespace pandar_ros {\n struct EIGEN_ALIGN16 Point {\n PCL_ADD_POINT4D;\n float intensity;\n double timestamp;\n uint16_t ring;\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n };\n}\nPOINT_CLOUD_REGISTER_POINT_STRUCT(pandar_ros::Point,\n (float, x, x)\n (float, y, y)\n (float, z, z)\n (float, intensity, intensity)\n (double, timestamp, timestamp)\n (std::uint16_t, ring, ring)\n)\n\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);\n void process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg, deque &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count);\n void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);\n void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count);\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;;\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);\n void oust_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);\n void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);\n void velodyne_handler_kitti(const sensor_msgs::PointCloud2::ConstPtr &msg);\n void velodyne_handler_nclt(const sensor_msgs::PointCloud2::ConstPtr &msg);\n void l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);\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#endif" }, { "path": "include/scope_timer.hpp", "content": "//\n// Created by yunfan on 2021/3/19.\n// Version: 1.0.0\n//\n\n\n#ifndef SCROPE_TIMER_HPP//SRC_POLY_VISUAL_UTILS_HPP\n#define SCROPE_TIMER_HPP\n\n#include \n#include \"color.h\"\n#include \"cstring\"\n\nusing namespace std;\n\nclass TimeConsuming {\npublic:\n TimeConsuming();\n\n TimeConsuming(string msg, int repeat_time) {\n repeat_time_ = repeat_time;\n msg_ = msg;\n tc_start = std::chrono::high_resolution_clock::now();\n has_shown = false;\n }\n\n TimeConsuming(string msg) {\n msg_ = msg;\n repeat_time_ = 1;\n tc_start = std::chrono::high_resolution_clock::now();\n has_shown = false;\n }\n\n ~TimeConsuming() {\n if (!has_shown && enable_) {\n tc_end = std::chrono::high_resolution_clock::now();\n double dt = std::chrono::duration_cast>(tc_end - tc_start).count();\n double t_us = (double) dt * 1e6 / repeat_time_;\n if (t_us < 1) {\n t_us *= 1000;\n printf(\"[TIMER] %s time consuming \\033[32m %lf ns\\033[0m\\n\", msg_.c_str(), t_us);\n } else if (t_us > 1e6) {\n t_us /= 1e6;\n printf(\"[TIMER] %s time consuming \\033[32m %lf s\\033[0m\\n\", msg_.c_str(), t_us);\n } else if (t_us > 1e3) {\n t_us /= 1e3;\n printf(\"[TIMER] %s time consuming \\033[32m %lf ms\\033[0m\\n\", msg_.c_str(), t_us);\n }else\n printf(\"[TIMER] %s time consuming \\033[32m %lf us\\033[0m\\n\", msg_.c_str(), t_us);\n }\n }\n\n void set_enbale(bool enable){\n enable_ = enable;\n }\n\n void start() {\n tc_start = std::chrono::high_resolution_clock::now();\n }\n\n void stop() {\n if(!enable_){return;}\n has_shown = true;\n tc_end = std::chrono::high_resolution_clock::now();\n double dt = std::chrono::duration_cast>(tc_end - tc_start).count();\n // ROS_WARN(\"%s time consuming %lf us.\",msg_.c_str(),(double)(end_t - start_t).toNSec()/ 1e3);\n double t_us = (double) dt * 1e6 / repeat_time_;\n if (t_us < 1) {\n t_us *= 1000;\n printf(\" -- [TIMER] %s time consuming \\033[32m %lf ns\\033[0m\\n\", msg_.c_str(), t_us);\n } else if (t_us > 1e6) {\n t_us /= 1e6;\n printf(\" -- [TIMER] %s time consuming \\033[32m %lf s\\033[0m\\n\", msg_.c_str(), t_us);\n } else if (t_us > 1e3) {\n t_us /= 1e3;\n printf(\" -- [TIMER] %s time consuming \\033[32m %lf ms\\033[0m\\n\", msg_.c_str(), t_us);\n }else\n printf(\" -- [TIMER] %s time consuming \\033[32m %lf us\\033[0m\\n\", msg_.c_str(), t_us);\n }\n\nprivate:\n std::chrono::high_resolution_clock::time_point tc_start, tc_end;\n string msg_;\n int repeat_time_;\n bool has_shown = false;\n bool enable_{true};\n};\n\n#endif //SRC_POLY_VISUAL_UTILS_HPP\n" }, { "path": "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#define SKEW_2D(w) 0.0, -w, w, 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\ntemplate\nEigen::Matrix A_cal(const Eigen::Matrix & ang_vel)\n{\n T norm = ang_vel.norm();\n Eigen::Matrix Eye3 = Eigen::Matrix::Identity();\n if (norm > 0.00001)\n {\n Eigen::Matrix r_ang = ang_vel / norm;\n Eigen::Matrix K;\n K << SKEW_SYM_MATRX(r_ang);\n return Eye3 + (1.0 - std::cos(norm)/norm) * K + (1.0 - std::sin(norm)/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\n// convert Rotation Matrix to euler angle\n// unit - radian\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// convert euler angle to Rotation Matrix \n// unit - radian\ntemplate\nEigen::Matrix EulerToRotM(const Eigen::Matrix &ang)\n{\n Eigen::Matrix R_x, R_y, R_z;\n R_x << 1, 0, 0, 0, cos(ang[0]), -sin(ang[0]), 0, sin(ang[0]), cos(ang[0]);\n R_y << cos(ang[1]), 0, sin(ang[1]), 0, 1, 0, -sin(ang[1]), 0, cos(ang[1]);\n R_z << cos(ang[2]), -sin(ang[2]), 0, sin(ang[2]), cos(ang[2]), 0, 0, 0, 1;\n return R_z * R_y * R_x;\n}\n\n\n\n#endif\n" }, { "path": "launch/hilti/hilti_basement_3.launch", "content": "\n \n\n \n\n \n\n \n\n \n \n \n\n \n \n \n\n launch-prefix=\"gdb -ex run --args\"\n\n\n" }, { "path": "launch/hilti/hilti_basement_4.launch", "content": "\n \n\n \n\n \n\n \n\n \n \n \n\n \n \n \n\n launch-prefix=\"gdb -ex run --args\"\n\n\n" }, { "path": "launch/m2dgr/m2dgr_gate01.launch", "content": "\n \n\n \n\n \n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n\n" }, { "path": "launch/m2dgr/m2dgr_hall04.launch", "content": "\n \n\n \n\n \n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n\n" }, { "path": "launch/m2dgr/m2dgr_lift04.launch", "content": "\n \n\n \n\n \n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n\n" }, { "path": "launch/m2dgr/m2dgr_rotation02.launch", "content": "\n \n\n \n\n \n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n\n" }, { "path": "launch/m2dgr/m2dgr_street08.launch", "content": "\n \n\n \n\n \n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n" }, { "path": "launch/ouster.launch", "content": "\n \n\n \n\n \n\n \n\n \n \n \n\n \n \n \n\n launch-prefix=\"gdb -ex run --args\"\n\n\n" }, { "path": "launch/s3e/s3e_bob_lab01.launch", "content": "\n \n\n \n\n \n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n\n" }, { "path": "launch/s3e/s3e_bob_lab02.launch", "content": "\n \n\n \n\n \n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n\n" }, { "path": "launch/velodyne.launch", "content": "\n \n\n \n\n \n\n \n\n \n \n \n\n \n \n \n launch-prefix=\"gdb -ex run --args\" \n\n" }, { "path": "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": "msg/States.msg", "content": "Header header # timestamp of the first lidar in a frame\nfloat64[] rot_end # the estimated attitude (rotation matrix) at the end lidar point\nfloat64[] pos_end # the estimated position at the end lidar point (world frame)\nfloat64[] vel_end # the estimated velocity at the end lidar point (world frame)\nfloat64[] bias_gyr # gyroscope bias\nfloat64[] bias_acc # accelerator bias\nfloat64[] gravity # the estimated gravity acceleration\nfloat64[] cov # states covariance\n# Pose6D[] IMUpose # 6D pose at each imu measurements" }, { "path": "package.xml", "content": "\n\n gril_calib\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": "result/traj.txt", "content": "" }, { "path": "rviz_cfg/.gitignore", "content": "" }, { "path": "rviz_cfg/hilti.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /mapping1\n - /mapping1/currPoints1/Autocompute Value Bounds1\n - /Odometry1/Odometry1/Shape1\n Splitter Ratio: 0.6432291865348816\n Tree Height: 288\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: currPoints\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 1\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: 100\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: 2.8887665271759033\n Min Value: -1.7356979846954346\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 238; 238; 236\n Color Transformer: AxisColor\n Decay Time: 0.10000000149011612\n Enabled: false\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: false\n - Alpha: 0.10000000149011612\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 7\n Min Value: -1\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; 255; 255\n Color Transformer: FlatColor\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: Ground\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.029999999329447746\n Style: Flat Squares\n Topic: /patchworkpp/ground\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\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: 0.699999988079071\n Axes Radius: 0.10000000149011612\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: /aft_mapped_to_init\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.6000000238418579\n Line Style: Billboards\n Line Width: 0.05000000074505806\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.07999999821186066\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: 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: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: RawPoints\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: /points_raw\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /planner_normal\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 0.699999988079071\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: true\n Name: Map\n Topic: /projected_map\n Unreliable: false\n Use Timestamp: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /alphasense/cam1/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: LiDAR\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/ThirdPersonFollower\n Distance: 53.233673095703125\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: 15.627415657043457\n Y: -13.479835510253906\n Z: 6.167871106299572e-6\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.5597963333129883\n Target Frame: LiDAR\n Value: ThirdPersonFollower (rviz)\n Yaw: 4.626343727111816\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1025\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 000000ff00000000fd000000040000000000000256000003abfc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000015b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000019c0000024a0000001600fffffffb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006500000002f0000000f60000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002620000019f0000000000000000fb0000000a0049006d0061006700650100000257000001aa0000000000000000fb0000000a0049006d006100670065010000026e0000017800000000000000000000000100000152000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000039fc0100000002fb0000000800540069006d006500000000000000073d0000042700fffffffb0000000800540069006d00650100000000000004500000000000000000000004e1000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\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": "rviz_cfg/m2dgr.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/currPoints1/Autocompute Value Bounds1\n - /Odometry1/Odometry1/Shape1\n Splitter Ratio: 0.6432291865348816\n Tree Height: 452\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: 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: 100\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: 7.801704406738281\n Min Value: -4.55991268157959\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 238; 238; 236\n Color Transformer: AxisColor\n Decay Time: 0.10000000149011612\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.10000000149011612\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 7\n Min Value: -1\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: 0.38099047541618347\n Min Value: -0.8690725564956665\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 Min Color: 0; 0; 0\n Name: Ground\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /patchworkpp/ground\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: 0.699999988079071\n Axes Radius: 0.10000000149011612\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: /aft_mapped_to_init\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.6000000238418579\n Line Style: Billboards\n Line Width: 0.10000000149011612\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.07999999821186066\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.05000000074505806\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 - 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: true\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: /points_raw\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /planner_normal\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /camera/color/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: LiDAR\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/ThirdPersonFollower\n Distance: 25.7800235748291\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: 6.16567850112915\n Y: -7.189380645751953\n Z: 7.152557373046875e-7\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.2247965335845947\n Target Frame: LiDAR\n Value: ThirdPersonFollower (rviz)\n Yaw: 4.431344985961914\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1052\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 000000ff00000000fd0000000400000000000001e7000003c6fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001ff000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000240000001c10000001600fffffffb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002fb000000eb0000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d0061006700650100000252000001af0000000000000000fb0000000a0049006d00610067006501000002aa000001570000000000000000fb0000000a0049006d00610067006501000002930000016e00000000000000000000000100000152000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000039fc0100000002fb0000000800540069006d006500000000000000073d0000042700fffffffb0000000800540069006d0065010000000000000450000000000000000000000593000003c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1920\n X: 1920\n Y: 0\n" }, { "path": "rviz_cfg/s3e.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 126\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /mapping1\n - /mapping1/currPoints1/Autocompute Value Bounds1\n - /Odometry1/Odometry1/Shape1\n - /Path1\n Splitter Ratio: 0.6432989835739136\n Tree Height: 396\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: currPoints\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 1\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: 100\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: 3.7455544471740723\n Min Value: -1.0043435096740723\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 238; 238; 236\n Color Transformer: AxisColor\n Decay Time: 0.10000000149011612\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.10000000149011612\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 7\n Min Value: -1\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: 0.38099047541618347\n Min Value: -0.8690725564956665\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: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: Ground\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /patchworkpp/ground\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\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: 0.699999988079071\n Axes Radius: 0.10000000149011612\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: /aft_mapped_to_init\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.6000000238418579\n Line Style: Billboards\n Line Width: 0.05000000074505806\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.07999999821186066\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.05000000074505806\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 - 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: true\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: /points_raw\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /planner_normal\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /camera/color/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: true\n Image Topic: /Bob/left_camera\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: LiDAR\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/ThirdPersonFollower\n Distance: 21.599872589111328\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: 2.402665138244629\n Y: -5.730081558227539\n Z: 2.6226043701171875e-6\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.589796781539917\n Target Frame: LiDAR\n Value: ThirdPersonFollower (rviz)\n Yaw: 3.696343183517456\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1025\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 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\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1853\n X: 67\n Y: 27\n" }, { "path": "rviz_cfg/spinning.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /mapping1/currPoints1/Autocompute Value Bounds1\n - /Odometry1/Odometry1/Shape1\n Splitter Ratio: 0.6432291865348816\n Tree Height: 475\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: 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: 100\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: 18.853248596191406\n Min Value: -1.1457874774932861\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 238; 238; 236\n Color Transformer: AxisColor\n Decay Time: 0.10000000149011612\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.10000000149011612\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 7\n Min Value: -1\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 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: 0.699999988079071\n Axes Radius: 0.10000000149011612\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: /aft_mapped_to_init\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.6000000238418579\n Line Style: Billboards\n Line Width: 0.05000000074505806\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.07999999821186066\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.05000000074505806\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 - 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: true\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: /points_raw\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Class: rviz/Marker\n Enabled: true\n Marker Topic: /planner_normal\n Name: Marker\n Namespaces:\n {}\n Queue Size: 100\n Value: true\n - Alpha: 0.699999988079071\n Class: rviz/Map\n Color Scheme: map\n Draw Behind: false\n Enabled: true\n Name: Map\n Topic: /projected_map\n Unreliable: false\n Use Timestamp: false\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /camera/infra1/image_rect_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: true\n Image Topic: /camera/infra1/image_rect_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: LiDAR\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/ThirdPersonFollower\n Distance: 68.43583679199219\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.8573362827301025\n Y: 1.0231647491455078\n Z: 1.1920928955078125e-6\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.5297970175743103\n Target Frame: LiDAR\n Value: ThirdPersonFollower (rviz)\n Yaw: 4.016337871551514\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1052\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 000000ff00000000fd000000040000000000000256000003c6fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000216000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000001600fffffffb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002620000019f0000000000000000fb0000000a0049006d0061006700650100000257000001aa0000001600ffffff0000000100000152000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000039fc0100000002fb0000000800540069006d006500000000000000073d0000042700fffffffb0000000800540069006d0065010000000000000450000000000000000000000524000003c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1920\n X: 1920\n Y: 0\n" }, { "path": "src/laserMapping.cpp", "content": "// This is an advanced implementation of the algorithm described in the\n// 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: Livox dev@livoxtech.com\n// Modifier : Taeyoung Kim (https://github.com/Taeyoung96)\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// #include \"ros/package.h\"\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 \"IMU_Processing.hpp\"\n#include \"ros/package.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#include \"preprocess.h\"\n#include \n#include \n\n// For visualize\n#ifndef DEPLOY\n#include \"matplotlibcpp.h\"\nnamespace plt = matplotlibcpp;\n#endif\n\n// For AHRS algorithm\n#include \"Fusion/Fusion.h\"\n#define IMU_Hz (200) // replace this with actual sample rate TODO\nros::Time estimate_timestamp_;\n\n// For ground segmentation\n#include \"GroundSegmentation/patchworkpp.hpp\"\n\nros::Publisher pub_cloud;\nros::Publisher pub_ground;\nros::Publisher pub_non_ground;\n\ndouble time_taken;\npcl::PointCloud curr_points;\npcl::PointCloud ground_points;\npcl::PointCloud non_ground_points;\n\n// For disable PCL complile lib, to use PointXYZIR\n#define PCL_NO_PRECOMPILE\n\n// For surface estimation\n#include \n#include \n\ndouble GYRO_FACTOR_;\ndouble ACC_FACTOR_ ;\ndouble GROUND_FACTOR_;\n\n// Origin LiDAR-IMU init\n#define LASER_POINT_COV (0.001)\n#define MAXN (720000)\n#define PUBFRAME_PERIOD (20)\n\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;\n\nint iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, \\\n effect_feat_num = 0, scan_count = 0, publish_count = 0;\n\ndouble res_mean_last = 0.05;\ndouble gyr_cov = 0.1, acc_cov = 0.1, grav_cov = 0.0001, b_gyr_cov = 0.0001, b_acc_cov = 0.0001, ground_cov = 1000.0;\ndouble last_timestamp_lidar = 0, last_timestamp_imu = 0.0;\ndouble filter_size_surf_min = 0, filter_size_map_min = 0;\ndouble cube_len = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0;\n\n// Time Log Variables\ndouble kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;\nint kdtree_delete_counter = 0, kdtree_size_end = 0, add_point_size = 0;\ndouble search_time_rec[100000];\n\nint lidar_type, pcd_save_interval = -1, pcd_index = 0;\nbool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited = true;\nbool imu_en = false;\nbool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;\nbool runtime_pos_log = false, pcd_save_en = false, extrinsic_est_en = true, path_en = true;\n\n// Trajectory save\nbool traj_save_en = false;\nstring traj_save_path;\n\n// LI-Init Parameters\nbool cut_frame = true, data_accum_finished = false, data_accum_start = false, online_calib_finish = false, refine_print = false;\nint cut_frame_num = 1, orig_odom_freq = 10, frame_num = 0;\ndouble time_lag_IMU_wtr_lidar = 0.0, move_start_time = 0.0, online_calib_starts_time = 0.0, mean_acc_norm = 9.81;\ndouble online_refine_time = 20.0; //unit: s\ndouble time_result = 0.0;\nvector Trans_LI_cov(3, 0.0005);\nvector Rot_LI_cov(3, 0.00005);\nV3D mean_acc = Zero3d;\nofstream fout_result;\n\nvector cub_needrm;\ndeque lidar_buffer;\ndeque time_buffer;\ndeque imu_buffer;\nvector> pointSearchInd_surf;\nvector Nearest_Points;\nbool point_selected_surf[100000] = {0};\nfloat res_last[100000] = {0.0};\ndouble total_residual;\n\n//surf feature in map\nPointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI());\nPointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI());\nPointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());\nPointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI());\nPointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));\nPointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1));\nPointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1));\nPointCloudXYZI::Ptr _featsArray;\n\npcl::VoxelGrid downSizeFilterSurf;\npcl::VoxelGrid downSizeFilterMap;\n\nKD_TREE ikdtree;\n\nM3D last_rot(M3D::Zero());\nV3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0);\nV3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0);\nV3D euler_cur;\nV3D position_last(Zero3d);\nV3D last_odom(Zero3d);\n\n//estimator inputs and output;\nMeasureGroup Measures;\nStatesGroup state;\n\nPointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());\npcl::PCDWriter pcd_writer;\nstring all_points_dir;\n\nnav_msgs::Path path;\nnav_msgs::Odometry odomAftMapped;\ngeometry_msgs::Quaternion geoQuat;\ngeometry_msgs::PoseStamped msg_body_pose;\nsensor_msgs::Imu IMU_sync;\n\nshared_ptr p_pre(new Preprocess());\nshared_ptr Calib_LI(new Gril_Calib());\n\nboost::shared_ptr> PatchworkppGroundSeg;\n\n// visualize ground segmenation result\ntemplate \nsensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id = \"aft_mapped\") {\n sensor_msgs::PointCloud2 cloud_ROS;\n pcl::toROSMsg(cloud, cloud_ROS);\n cloud_ROS.header.frame_id = frame_id;\n return cloud_ROS;\n}\n\nfloat calc_dist(PointType p1, PointType p2) {\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\nvoid calcBodyVar(Eigen::Vector3d &pb, const float range_inc,\n const float degree_inc, Eigen::Matrix3d &var) {\n float range = sqrt(pb[0] * pb[0] + pb[1] * pb[1] + pb[2] * pb[2]);\n float range_var = range_inc * range_inc;\n Eigen::Matrix2d direction_var;\n direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0,\n pow(sin(DEG2RAD(degree_inc)), 2);\n Eigen::Vector3d direction(pb);\n direction.normalize();\n Eigen::Matrix3d direction_hat;\n direction_hat << 0, -direction(2), direction(1), direction(2), 0,\n -direction(0), -direction(1), direction(0), 0;\n Eigen::Vector3d base_vector1(1, 1,\n -(direction(0) + direction(1)) / direction(2));\n base_vector1.normalize();\n Eigen::Vector3d base_vector2 = base_vector1.cross(direction);\n base_vector2.normalize();\n Eigen::Matrix N;\n N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1),\n base_vector1(2), base_vector2(2);\n Eigen::Matrix A = range * direction_hat * N;\n var = direction * range_var * direction.transpose() +\n A * direction_var * A.transpose();\n}\n\nvoid SigHandle(int sig) {\n if (pcd_save_en && pcd_save_interval < 0){\n all_points_dir = string(root_dir + \"/PCD/PCD_all\" + string(\".pcd\"));\n pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);\n }\n flg_exit = true;\n ROS_WARN(\"catch sig %d\", sig);\n sig_buffer.notify_all();\n}\n\ninline void dump_lio_state_to_log(FILE *fp) {\n V3D rot_ang(Log(state.rot_end));\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.pos_end(0), state.pos_end(1), state.pos_end(2)); // Pos\n fprintf(fp, \"%lf %lf %lf \", state.vel_end(0), state.vel_end(1), state.vel_end(2)); // Vel\n fprintf(fp, \"%lf %lf %lf \", 0.0, 0.0, 0.0); // omega\n fprintf(fp, \"%lf %lf %lf \", 0.0, 0.0, 0.0); // Acc\n fprintf(fp, \"%lf %lf %lf \", state.bias_g(0), state.bias_g(1), state.bias_g(2)); // Bias_g\n fprintf(fp, \"%lf %lf %lf \", state.bias_a(0), state.bias_a(1), state.bias_a(2)); // Bias_a\n fprintf(fp, \"%lf %lf %lf \", state.gravity(0), state.gravity(1), state.gravity(2)); // Bias_a \n fprintf(fp, \"\\r\\n\");\n fflush(fp);\n}\n\n\nvoid pointBodyToWorld(PointType const *const pi, PointType *const po) {\n V3D p_body(pi->x, pi->y, pi->z);\n V3D p_global(state.rot_end * (state.offset_R_L_I * p_body + state.offset_T_L_I) + state.pos_end);\n\n po->x = p_global(0);\n po->y = p_global(1);\n po->z = p_global(2);\n po->normal_x = pi->normal_x;\n po->normal_y = pi->normal_y;\n po->normal_z = pi->normal_z;\n po->intensity = pi->intensity;\n}\n\ntemplate\nvoid pointBodyToWorld(const Matrix &pi, Matrix &po) {\n V3D p_body(pi[0], pi[1], pi[2]);\n V3D p_global(state.rot_end * (state.offset_R_L_I * p_body + state.offset_T_L_I) + state.pos_end);\n po[0] = p_global(0);\n po[1] = p_global(1);\n po[2] = p_global(2);\n}\n\nvoid RGBpointBodyToWorld(PointType const *const pi, PointTypeRGB *const po) {\n V3D p_body(pi->x, pi->y, pi->z);\n V3D p_global(state.rot_end * (state.offset_R_L_I * p_body + state.offset_T_L_I) + state.pos_end);\n po->x = p_global(0);\n po->y = p_global(1);\n po->z = p_global(2);\n po->r = pi->normal_x;\n po->g = pi->normal_y;\n po->b = pi->normal_z;\n\n float intensity = pi->intensity;\n intensity = intensity - floor(intensity);\n\n int reflection_map = intensity * 10000;\n}\n\nint points_cache_size = 0;\n\nvoid points_cache_collect() {\n PointVector points_history;\n ikdtree.acquire_removed_points(points_history);\n points_cache_size = points_history.size();\n for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]);\n}\n\n\nBoxPointType LocalMap_Points;\nbool Localmap_Initialized = false;\n\nvoid lasermap_fov_segment() {\n cub_needrm.clear();\n kdtree_delete_counter = 0;\n kdtree_delete_time = 0.0;\n pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);\n V3D pos_LiD = state.pos_end;\n\n if (!Localmap_Initialized) {\n for (int i = 0; i < 3; i++) {\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\n float dist_to_map_edge[3][2];\n bool need_move = false;\n for (int i = 0; i < 3; i++) {\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 if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE ||\n dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)\n need_move = true;\n }\n if (!need_move) return;\n BoxPointType New_LocalMap_Points, tmp_boxpoints;\n New_LocalMap_Points = LocalMap_Points;\n float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9,\n double(DET_RANGE * (MOV_THRESHOLD - 1)));\n for (int i = 0; i < 3; i++) {\n tmp_boxpoints = LocalMap_Points;\n if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) {\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 } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) {\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 if (cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);\n kdtree_delete_time = omp_get_wtime() - delete_begin;\n printf(\"Delete time: %0.6f, delete size: %d\\n\", kdtree_delete_time, kdtree_delete_counter);\n}\n\ndouble timediff_imu_wrt_lidar = 0.0;\nbool timediff_set_flg = false;\n\n// ** livox LiDAR callback ** // \nvoid livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) {\n mtx_buffer.lock();\n double preprocess_start_time = omp_get_wtime();\n scan_count++;\n if (msg->header.stamp.toSec() < last_timestamp_lidar) {\n ROS_WARN(\"lidar loop back, clear buffer\");\n lidar_buffer.clear();\n time_buffer.clear();\n }\n last_timestamp_lidar = msg->header.stamp.toSec();\n\n if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_buffer.empty()) {\n timediff_set_flg = true;\n timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar;\n printf(\"Self sync IMU and LiDAR, HARD time lag is %.10lf \\n \\n\", timediff_imu_wrt_lidar);\n }\n\n if (cut_frame) {\n deque ptr;\n deque timestamp_lidar;\n p_pre->process_cut_frame_livox(msg, ptr, timestamp_lidar, cut_frame_num, scan_count);\n\n while (!ptr.empty() && !timestamp_lidar.empty()) {\n lidar_buffer.push_back(ptr.front());\n ptr.pop_front();\n time_buffer.push_back(timestamp_lidar.front() / double(1000));//unit:s\n timestamp_lidar.pop_front();\n }\n } else {\n PointCloudXYZI::Ptr ptr(new PointCloudXYZI());\n p_pre->process(msg, ptr);\n lidar_buffer.push_back(ptr);\n time_buffer.push_back(last_timestamp_lidar);\n }\n mtx_buffer.unlock();\n sig_buffer.notify_all();\n}\n\n// ** mechnical LiDAR point cloud callback ** //\nvoid standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) {\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 ROS_ERROR(\"lidar loop back, clear Lidar buffer.\");\n lidar_buffer.clear();\n time_buffer.clear();\n }\n\n last_timestamp_lidar = msg->header.stamp.toSec();\n if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_buffer.empty()) {\n timediff_set_flg = true;\n timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar;\n printf(\"Self sync IMU and LiDAR, HARD time lag is %.10lf \\n \\n\", timediff_imu_wrt_lidar);\n }\n\n if ((lidar_type == VELO || lidar_type == OUSTER || lidar_type == PANDAR || lidar_type == VELO_without_Time) && cut_frame) {\n deque ptr;\n deque timestamp_lidar;\n p_pre->process_cut_frame_pcl2(msg, ptr, timestamp_lidar, cut_frame_num, scan_count);\n while (!ptr.empty() && !timestamp_lidar.empty()) {\n lidar_buffer.push_back(ptr.front());\n ptr.pop_front();\n time_buffer.push_back(timestamp_lidar.front() / double(1000)); //unit:s\n timestamp_lidar.pop_front();\n }\n } \n else {\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 }\n\n // Ground Segmentation TODO : other lidar type\n if (lidar_type == VELO || lidar_type == VELO_NCLT || lidar_type == OUSTER || lidar_type == PANDAR || lidar_type == VELO_without_Time) {\n pcl::fromROSMsg(*msg, curr_points);\n\n PatchworkppGroundSeg->estimate_ground(curr_points, ground_points, non_ground_points, time_taken);\n\n pub_cloud.publish(cloud2msg(curr_points));\n pub_ground.publish(cloud2msg(ground_points));\n pub_non_ground.publish(cloud2msg(non_ground_points));\n }\n \n\n mtx_buffer.unlock();\n sig_buffer.notify_all();\n}\n\n// ** IMU callback ** //\nvoid imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in, const ros::Publisher &pubIMU_sync) {\n publish_count++;\n mtx_buffer.lock();\n\n static double IMU_period, time_msg_in, last_time_msg_in;\n static int imu_cnt = 0;\n time_msg_in = msg_in->header.stamp.toSec();\n\n\n if (imu_cnt < 100) {\n imu_cnt++;\n mean_acc += (V3D(msg_in->linear_acceleration.x, msg_in->linear_acceleration.y, msg_in->linear_acceleration.z) -\n mean_acc) / (imu_cnt);\n if (imu_cnt > 1) {\n IMU_period += (time_msg_in - last_time_msg_in - IMU_period) / (imu_cnt - 1);\n }\n if (imu_cnt == 99) {\n cout << endl << \"Acceleration norm : \" << mean_acc.norm() << endl;\n if (IMU_period > 0.01) {\n cout << \"IMU data frequency : \" << 1 / IMU_period << \" Hz\" << endl;\n ROS_WARN(\"IMU data frequency too low. Higher than 150 Hz is recommended.\");\n }\n cout << endl;\n }\n }\n last_time_msg_in = time_msg_in;\n\n\n sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));\n\n //IMU Time Compensation\n msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar);\n double timestamp = msg->header.stamp.toSec();\n\n if (timestamp < last_timestamp_imu) {\n ROS_WARN(\"IMU loop back, clear IMU buffer.\");\n imu_buffer.clear();\n Calib_LI->IMU_buffer_clear();\n }\n\n last_timestamp_imu = timestamp;\n \n imu_buffer.push_back(msg);\n\n // push all IMU meas into Calib_LI\n if (!imu_en && !data_accum_finished)\n Calib_LI->push_ALL_IMU_CalibState(msg, mean_acc_norm);\n\n mtx_buffer.unlock();\n sig_buffer.notify_all();\n}\n\nbool sync_packages(MeasureGroup &meas) {\n if (lidar_buffer.empty() || imu_buffer.empty())\n return false;\n\n /** push a lidar scan **/\n if (!lidar_pushed) {\n meas.lidar = lidar_buffer.front();\n\n if (meas.lidar->points.size() <= 1) {\n ROS_WARN(\"Too few input point cloud!\\n\");\n lidar_buffer.pop_front();\n time_buffer.pop_front();\n return false;\n }\n\n meas.lidar_beg_time = time_buffer.front(); //unit:s\n\n if (lidar_type == L515)\n lidar_end_time = meas.lidar_beg_time;\n else\n lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); //unit:s\n\n lidar_pushed = true;\n }\n\n if (last_timestamp_imu < lidar_end_time)\n return false;\n\n /** push imu data, and pop from imu buffer **/\n double imu_time = imu_buffer.front()->header.stamp.toSec();\n meas.imu.clear();\n while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) {\n imu_time = imu_buffer.front()->header.stamp.toSec();\n if (imu_time > lidar_end_time) break;\n meas.imu.push_back(imu_buffer.front());\n imu_buffer.pop_front();\n }\n lidar_buffer.pop_front();\n time_buffer.pop_front();\n lidar_pushed = false;\n return true;\n}\n\nvoid map_incremental() {\n PointVector PointToAdd;\n PointVector PointNoNeedDownsample;\n PointToAdd.reserve(feats_down_size);\n PointNoNeedDownsample.reserve(feats_down_size);\n for (int i = 0; i < feats_down_size; i++) {\n /* transform to world frame */\n pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));\n /* decide if need add to map */\n if (!Nearest_Points[i].empty() && flg_EKF_inited) {\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 mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min +\n 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 +\n 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 +\n 0.5 * filter_size_map_min;\n float dist = calc_dist(feats_down_world->points[i], mid_point);\n if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min &&\n fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min &&\n fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min) {\n PointNoNeedDownsample.push_back(feats_down_world->points[i]);\n continue;\n }\n for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i++) {\n if (points_near.size() < NUM_MATCH_POINTS) break;\n if (calc_dist(points_near[readd_i], mid_point) < dist) {\n need_add = false;\n break;\n }\n }\n if (need_add) PointToAdd.push_back(feats_down_world->points[i]);\n } else {\n PointToAdd.push_back(feats_down_world->points[i]);\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();\n kdtree_incremental_time = omp_get_wtime() - st_time;\n}\n\nvoid publish_frame_world(const ros::Publisher &pubLaserCloudFullRes) {\n if (scan_pub_en) {\n PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);\n int size = laserCloudFullRes->points.size();\n\n PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB(size, 1));\n PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1));\n\n for (int i = 0; i < size; i++) {\n if (lidar_type == L515)\n RGBpointBodyToWorld(&laserCloudFullRes->points[i], \\\n &laserCloudWorldRGB->points[i]);\n else\n pointBodyToWorld(&laserCloudFullRes->points[i], \\\n &laserCloudWorld->points[i]);\n }\n\n sensor_msgs::PointCloud2 laserCloudmsg;\n if (lidar_type == L515)\n pcl::toROSMsg(*laserCloudWorldRGB, laserCloudmsg);\n else\n pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);\n\n laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);\n laserCloudmsg.header.frame_id = \"LiDAR\";\n pubLaserCloudFullRes.publish(laserCloudmsg);\n publish_count -= PUBFRAME_PERIOD;\n }\n\n\n /**************** save map ****************/\n /* 1. make sure you have enough memories\n 2. noted that pcd save will influence the real-time performences **/\n if (pcd_save_en) {\n boost::filesystem::create_directories(root_dir + \"/PCD\");\n int size = feats_undistort->points.size();\n PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1));\n for (int i = 0; i < size; i++) {\n pointBodyToWorld(&feats_undistort->points[i], &laserCloudWorld->points[i]);\n }\n\n *pcl_wait_save += *laserCloudWorld;\n static int scan_wait_num = 0;\n scan_wait_num++;\n if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) {\n pcd_index++;\n all_points_dir = string(root_dir + \"/PCD/PCD\") + to_string(pcd_index) + string(\".pcd\");\n cout << \"current scan saved to \" << all_points_dir << endl;\n pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);\n pcl_wait_save->clear();\n scan_wait_num = 0;\n }\n }\n}\n\nvoid publish_frame_body(const ros::Publisher &pubLaserCloudFullRes_body) {\n PointCloudXYZI::Ptr laserCloudFullRes(feats_undistort);\n sensor_msgs::PointCloud2 laserCloudmsg;\n pcl::toROSMsg(*feats_undistort, laserCloudmsg);\n laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);\n laserCloudmsg.header.frame_id = \"LiDAR\";\n pubLaserCloudFullRes_body.publish(laserCloudmsg);\n}\n\nvoid publish_effect_world(const ros::Publisher &pubLaserCloudEffect) {\n PointCloudXYZI::Ptr laserCloudWorld(\\\n new PointCloudXYZI(effect_feat_num, 1));\n for (int i = 0; i < effect_feat_num; i++) {\n pointBodyToWorld(&laserCloudOri->points[i], &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 = \"LiDAR\";\n pubLaserCloudEffect.publish(laserCloudFullRes3);\n}\n\nvoid publish_map(const ros::Publisher &pubLaserCloudMap) {\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 = \"LiDAR\";\n pubLaserCloudMap.publish(laserCloudMap);\n}\n\ntemplate\nvoid set_posestamp(T &out) {\n if (!imu_en) {\n out.position.x = state.pos_end(0);\n out.position.y = state.pos_end(1);\n out.position.z = state.pos_end(2);\n } else {\n //Pubulish LiDAR's pose and position\n V3D pos_cur_lidar = state.rot_end * state.offset_T_L_I + state.pos_end;\n out.position.x = pos_cur_lidar(0);\n out.position.y = pos_cur_lidar(1);\n out.position.z = pos_cur_lidar(2);\n }\n out.orientation.x = geoQuat.x;\n out.orientation.y = geoQuat.y;\n out.orientation.z = geoQuat.z;\n out.orientation.w = geoQuat.w;\n}\n\nvoid publish_odometry(const ros::Publisher &pubOdomAftMapped) {\n odomAftMapped.header.frame_id = \"LiDAR\";\n odomAftMapped.child_frame_id = \"aft_mapped\";\n odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);\n set_posestamp(odomAftMapped.pose.pose);\n\n pubOdomAftMapped.publish(odomAftMapped);\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, \"LiDAR\", \"aft_mapped\"));\n}\n\nvoid publish_path(const ros::Publisher pubPath) {\n set_posestamp(msg_body_pose.pose);\n msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);\n msg_body_pose.header.frame_id = \"LiDAR\";\n static int jjj = 0;\n jjj++;\n if (jjj % 5 == 0) // if path is too large, the RVIZ will crash\n {\n path.poses.push_back(msg_body_pose);\n pubPath.publish(path);\n }\n}\n\nvoid fileout_calib_result() {\n fout_result.setf(ios::fixed);\n fout_result << setprecision(6)\n << \"Rotation LiDAR to IMU (degree) = \" << RotMtoEuler(state.offset_R_L_I).transpose() * 57.3\n << endl;\n fout_result << \"Translation LiDAR to IMU (meter) = \" << state.offset_T_L_I.transpose() << endl;\n fout_result << \"Time Lag IMU to LiDAR (second) = \" << time_result << endl;\n fout_result << \"Bias of Gyroscope (rad/s) = \" << state.bias_g.transpose() << endl;\n fout_result << \"Bias of Accelerometer (meters/s^2) = \" << state.bias_a.transpose() << endl;\n fout_result << endl;\n\n MD(4, 4) Transform;\n Transform.setIdentity();\n Transform.block<3, 3>(0, 0) = state.offset_R_L_I;\n Transform.block<3, 1>(0, 3) = state.offset_T_L_I;\n fout_result << \"Homogeneous Transformation Matrix from LiDAR frmae L to IMU frame I: \" << endl;\n fout_result << Transform << endl << endl << endl;\n}\n\nvoid printProgress(double percentage) {\n int val = (int) (percentage * 100);\n int lpad = (int) (percentage * PBWIDTH);\n int rpad = PBWIDTH - lpad;\n printf(\"\\033[1A\\r\");\n printf(BOLDMAGENTA \"[Refinement] \");\n if (percentage < 1) {\n printf(BOLDYELLOW \"Online Refinement: \");\n printf(YELLOW \"%3d%% [%.*s%*s]\\n\", val, lpad, PBSTR, rpad, \"\");\n cout << RESET;\n } else {\n printf(BOLDGREEN \" Online Refinement \");\n printf(GREEN \"%3d%% [%.*s%*s]\\n\", val, lpad, PBSTR, rpad, \"\");\n cout << RESET;\n }\n fflush(stdout);\n}\n\n/*\n* @brief : Save the whole trajectory to a txt file\n*/\nvoid saveTrajectory(const std::string &traj_file) {\n std::string filename(traj_file);\n std::fstream output_fstream;\n\n output_fstream.open(filename, std::ios_base::out);\n\n if (!output_fstream.is_open()) {\n std::cerr << \"Failed to open \" << filename << '\\n';\n }\n\n else {\n output_fstream << \"#timestamp x y z q_x q_y q_z q_w\" << std::endl;\n for (const auto &p : path.poses) {\n output_fstream << std::setprecision(15) << p.header.stamp.toSec() << \" \"\n << p.pose.position.x << \" \"\n << p.pose.position.y << \" \"\n << p.pose.position.z << \" \"\n << p.pose.orientation.x << \" \"\n << p.pose.orientation.y << \" \"\n << p.pose.orientation.z << \" \"\n << p.pose.orientation.w << std::endl;\n }\n }\n}\n\n/*\n* @brief : Caculate the rotation matrix (quaternion)) \n* from LiDAR ground point cloud normal vector frame to earth gravity vector frame\n* @output : quaternion (from world gravity frame to LiDAR plane frame)\n* @output : estimated height of LiDAR sensor\n*/\nvoid get_quat_LiDAR_plane_to_gravity(Eigen::Quaterniond &lidar_q, V3D& normal_vector, double &lidar_estimate_height) {\n \n /** Normal vector estimation as a result of ground segmentation **/\n Eigen::Vector4f plane_parameters;\n float curvature;\n computePointNormal(ground_points, plane_parameters, curvature);\n\n /** Roll, pitch, and yaw estimation with respect to the Earth's gravity direction vector using the ground plane normal vector **/\n V3D normal_plane = {plane_parameters[0], plane_parameters[1], plane_parameters[2]};\n normal_plane.normalize();\n\n V3D normal_gravity{0, 0, 1.0};\n\n // the built rotation represent a rotation sending the line of direction a to the line of direction b, both lines passing through the origin.\n // (from world gravity frame to LiDAR plane frame)\n lidar_q = Eigen::Quaterniond::FromTwoVectors(normal_gravity, normal_plane);\n lidar_estimate_height = plane_parameters[3];\n normal_vector = normal_plane;\n}\n\n\nint main(int argc, char **argv) {\n ros::init(argc, argv, \"laserMapping\");\n ros::NodeHandle nh;\n\n nh.param(\"max_iteration\", NUM_MAX_ITERATIONS, 4);\n nh.param(\"point_filter_num\", p_pre->point_filter_num, 2);\n nh.param(\"map_file_path\", map_file_path, \"\");\n\n nh.param(\"common/lid_topic\", lid_topic, \"/livox/lidar\");\n nh.param(\"common/imu_topic\", imu_topic, \"/livox/imu\");\n\n nh.param(\"mapping/filter_size_surf\", filter_size_surf_min, 0.5);\n nh.param(\"mapping/filter_size_map\", filter_size_map_min, 0.5);\n nh.param(\"cube_side_length\", cube_len, 200);\n nh.param(\"mapping/det_range\", DET_RANGE, 300.f);\n nh.param(\"mapping/gyr_cov\", gyr_cov, 0.1);\n nh.param(\"mapping/acc_cov\", acc_cov, 0.1);\n nh.param(\"mapping/grav_cov\", grav_cov, 0.001);\n nh.param(\"mapping/b_gyr_cov\", b_gyr_cov, 0.0001);\n nh.param(\"mapping/b_acc_cov\", b_acc_cov, 0.0001);\n nh.param(\"mapping/ground_cov\", ground_cov, 1000.0);\n\n nh.param(\"preprocess/blind\", p_pre->blind, 1.0);\n nh.param(\"preprocess/lidar_type\", lidar_type, AVIA);\n nh.param(\"preprocess/scan_line\", p_pre->N_SCANS, 16);\n nh.param(\"preprocess/feature_extract_en\", p_pre->feature_enabled, 0);\n\n nh.param(\"calibration/cut_frame\", cut_frame, true);\n nh.param(\"calibration/cut_frame_num\", cut_frame_num, 1);\n nh.param(\"calibration/orig_odom_freq\", orig_odom_freq, 10);\n nh.param(\"calibration/mean_acc_norm\", mean_acc_norm, 9.81);\n\n nh.param(\"calibration/data_accum_length\", Calib_LI->data_accum_length, 300);\n nh.param(\"calibration/x_accumulate\", Calib_LI->x_accumulate, 0.1);\n nh.param(\"calibration/y_accumulate\", Calib_LI->y_accumulate, 0.1);\n nh.param(\"calibration/z_accumulate\", Calib_LI->z_accumulate, 0.1);\n\n nh.param(\"calibration/imu_sensor_height\", Calib_LI->imu_sensor_height, 0.1);\n nh.param(\"calibration/verbose\", Calib_LI->verbose , false);\n\n nh.param(\"calibration/trans_IL_x\", Calib_LI->trans_IL_x, 0.0);\n nh.param(\"calibration/trans_IL_y\", Calib_LI->trans_IL_y, 0.0);\n nh.param(\"calibration/trans_IL_z\", Calib_LI->trans_IL_z, 0.0);\n nh.param(\"calibration/bound_th\", Calib_LI->bound_th, 0.1);\n nh.param(\"calibration/set_boundary\", Calib_LI->set_boundary, false);\n\n nh.param(\"calibration/gyro_factor\", GYRO_FACTOR_, 1.0);\n nh.param(\"calibration/acc_factor\", ACC_FACTOR_, 1.0);\n nh.param(\"calibration/ground_factor\", GROUND_FACTOR_, 1.0);\n\n nh.param(\"publish/path_en\", path_en, true);\n nh.param(\"publish/scan_publish_en\", scan_pub_en, 1);\n nh.param(\"publish/dense_publish_en\", dense_pub_en, 1);\n nh.param(\"publish/scan_bodyframe_pub_en\", scan_body_pub_en, 1);\n nh.param(\"runtime_pos_log_enable\", runtime_pos_log, 0);\n nh.param(\"pcd_save/pcd_save_en\", pcd_save_en, false);\n nh.param(\"pcd_save/interval\", pcd_save_interval, -1);\n\n nh.param(\"trajectory_save/traj_save_en\", traj_save_en, false);\n nh.param(\"trajectory_save/traj_file_path\", traj_save_path, \"/home/catkin_ws/src/result/traj.txt\");\n\n cout << \"lidar_type: \" << lidar_type << endl;\n cout << \"LiDAR-only odometry starts.\" << endl;\n\n path.header.stamp = ros::Time().fromSec(lidar_end_time);\n path.header.frame_id = \"LiDAR\";\n\n\n /*** variables definition ***/\n VD(DIM_STATE) solution;\n MD(DIM_STATE, DIM_STATE) G, H_T_H, I_STATE;\n V3D rot_add, T_add, vel_add, gyr_add;\n\n StatesGroup state_propagat;\n PointType pointOri, pointSel, coeff;\n\n double deltaT, deltaR;\n bool flg_EKF_converged, EKF_stop_flg = 0;\n\n _featsArray.reset(new PointCloudXYZI());\n\n memset(point_selected_surf, true, sizeof(point_selected_surf));\n memset(res_last, -1000.0f, sizeof(res_last));\n downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);\n downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);\n memset(point_selected_surf, true, sizeof(point_selected_surf));\n memset(res_last, -1000.0f, sizeof(res_last));\n\n shared_ptr p_imu(new ImuProcess());\n\n p_imu->lidar_type = p_pre->lidar_type = lidar_type;\n p_imu->imu_en = imu_en;\n p_imu->Gril_Calib_done = false;\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_R_LI_cov(V3D(VEC_FROM_ARRAY(Rot_LI_cov)));\n p_imu->set_T_LI_cov(V3D(VEC_FROM_ARRAY(Trans_LI_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 G.setZero();\n H_T_H.setZero();\n I_STATE.setIdentity();\n \n /*** Ground Segmentation ***/\n cout << \"Operating patchwork++...\" << endl;\n PatchworkppGroundSeg.reset(new PatchWorkpp(&nh));\n\n // LI Init Related\n MatrixXd Jaco_rot(30000, 3);\n Jaco_rot.setZero();\n\n /*** debug record ***/\n ofstream fout_out;\n fout_out.open(DEBUG_FILE_DIR(\"state_out.txt\"), ios::out);\n fout_result.open(RESULT_FILE_DIR(\"GRIL_Calib_result.txt\"), ios::out);\n if (fout_out)\n cout << \"~~~~\" << ROOT_DIR << \" file opened\" << endl;\n else\n cout << \"~~~~\" << ROOT_DIR << \" doesn't exist\" << endl;\n\n\n /*** ROS subscribe initialization ***/\n ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \\\n nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \\\n nh.subscribe(lid_topic, 200000, standard_pcl_cbk);\n\n ros::Publisher pubIMU_sync = nh.advertise\n (\"/livox/imu/async\", 100000);\n\n ros::Subscriber sub_imu = nh.subscribe\n (imu_topic, 200000, boost::bind(&imu_cbk, _1, pubIMU_sync));\n\n\n ros::Publisher pubLaserCloudFullRes = nh.advertise\n (\"/cloud_registered\", 100000);\n ros::Publisher pubLaserCloudFullRes_body = nh.advertise\n (\"/cloud_registered_body\", 100000);\n ros::Publisher pubLaserCloudEffect = nh.advertise\n (\"/cloud_effected\", 100000);\n ros::Publisher pubLaserCloudMap = nh.advertise\n (\"/Laser_map\", 100000);\n ros::Publisher pubOdomAftMapped = nh.advertise\n (\"/aft_mapped_to_init\", 100000);\n ros::Publisher pubPath = nh.advertise\n (\"/path\", 100000);\n\n pub_cloud = nh.advertise(\"/patchworkpp/cloud\", 100, true);\n pub_ground = nh.advertise(\"/patchworkpp/ground\", 100, true);\n pub_non_ground = nh.advertise(\"/patchworkpp/nonground\", 100, true);\n\n //** For AHRS algirithm **//\n\n // Define calibration (replace with actual calibration data)\n const FusionMatrix gyroscopeMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};\n const FusionVector gyroscopeSensitivity = {1.0f, 1.0f, 1.0f};\n const FusionVector gyroscopeOffset = {0.0f, 0.0f, 0.0f};\n\n const FusionMatrix accelerometerMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};\n const FusionVector accelerometerSensitivity = {1.0f, 1.0f, 1.0f};\n const FusionVector accelerometerOffset = {0.0f, 0.0f, 0.0f};\n\n // IMU AHRS algorithm initialization\n FusionOffset offset;\n FusionAhrs ahrs;\n\n FusionOffsetInitialise(&offset, IMU_Hz);\n FusionAhrsInitialise(&ahrs);\n\n // Set AHRS algorithm settings\n const FusionAhrsSettings settings = {\n .gain = 0.5f,\n .accelerationRejection = 10.0f,\n .magneticRejection = 0.0f,\n .rejectionTimeout = 5 * IMU_Hz, // TODO : Change IMU hz\n };\n\n FusionAhrsSetSettings(&ahrs, &settings);\n FusionEuler euler_imu = {0.0f, 0.0f, 0.0f}; // roll, pitch, yaw\n V3D euler_lidar = V3D(0, 0, 0);\n\n FusionQuaternion imu_q = {1.0f, 0.0f, 0.0f, 0.0f}; // qw, qx, qy, qz (from earth frame to IMU frame) \n Eigen::Quaterniond imu_q_eigen = Eigen::Quaterniond(1.0f, 0.0f, 0.0f, 0.0f); // qw, qx, qy, qz (from earth frame to IMU frame)\n Eigen::Quaterniond lidar_q = Eigen::Quaterniond(1, 0, 0, 0); // qw, qx, qy, qz (from earth frame to lidar frame)\n V3D normal_lidar = V3D(0, 0, 1); // normal vector of lidar frame\n double lidar_estimate_height = 0.0;\n\n//------------------------------------------------------------------------------------------------------\n signal(SIGINT, SigHandle);\n ros::Rate rate(5000);\n bool status = ros::ok();\n\n while (status) {\n if (flg_exit) break;\n ros::spinOnce();\n if (sync_packages(Measures)) {\n if (flg_reset) {\n ROS_WARN(\"reset when rosbag play back.\");\n p_imu->Reset();\n flg_reset = false;\n continue;\n }\n \n kdtree_search_time = 0.0;\n \n\n /** IMU pre-processing **/\n p_imu->Process(Measures, state, feats_undistort);\n state_propagat = state;\n\n if (feats_undistort->empty() || (feats_undistort == NULL)) {\n first_lidar_time = Measures.lidar_beg_time;\n p_imu->first_lidar_time = first_lidar_time;\n ROS_WARN(\"FAST-LIO not ready, no points stored.\");\n online_calib_starts_time = first_lidar_time;\n continue;\n }\n\n /** AHRS algorithm and estimate Rotation matrix (from earth to IMU & from earth to LiDAR plane) **/\n auto v_imu = Measures.imu; \n auto imu_timestamp = Measures.imu.front()->header.stamp; // first imu ROS timestamp\n estimate_timestamp_ = imu_timestamp;\n\n for (auto imu_msg : v_imu) {\n const sensor_msgs::Imu thisImu = *imu_msg; \n float delta_time = (thisImu.header.stamp - estimate_timestamp_).toSec(); // unit : s\n\n auto gyr = thisImu.angular_velocity;\n \n // gyr : rad/s -> deg/s\n FusionVector gyroscope = {static_cast(rad2deg(gyr.x)), \n static_cast(rad2deg(gyr.y)), \n static_cast(rad2deg(gyr.z))}; \n\n // TODO : IMU gyro calibration\n gyroscope = FusionCalibrationInertial(gyroscope, gyroscopeMisalignment, gyroscopeSensitivity, gyroscopeOffset);\n \n // Update gyroscope offset correction algorithm \n gyroscope = FusionOffsetUpdate(&offset, gyroscope);\n\n // ref : http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html\n auto acc = thisImu.linear_acceleration;\n // acc : m/s^2 -> g (SI unit to g unit)\n // ref : https://github.com/Livox-SDK/livox_ros_driver/issues/63\n FusionVector accelerometer = {static_cast(SI2g(acc.x)), \n static_cast(SI2g(acc.y)), \n static_cast(SI2g(acc.z))};\n \n // TODO : IMU acc calibration\n accelerometer = FusionCalibrationInertial(accelerometer, accelerometerMisalignment, accelerometerSensitivity, accelerometerOffset);\n\n FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, delta_time);\n\n FusionAhrsFlags ahrsFlag = FusionAhrsGetFlags(&ahrs);\n bool ahrsInitializing = ahrsFlag.initialising; \n \n FusionAhrsInternalStates ahrsState = FusionAhrsGetInternalStates(&ahrs);\n \n // Initalization is finished\n if(!ahrsInitializing){\n // from ground frame to IMU frame\n euler_imu = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); \n imu_q = FusionAhrsGetQuaternion(&ahrs);\n imu_q_eigen = Eigen::Quaterniond(imu_q.element.w, imu_q.element.x, imu_q.element.y, imu_q.element.z);\n\n // from ground frame to LiDAR frame\n get_quat_LiDAR_plane_to_gravity(lidar_q, normal_lidar,lidar_estimate_height); \n }\n \n estimate_timestamp_ = thisImu.header.stamp;\n }\n \n /*** Segment the map in lidar FOV ***/\n lasermap_fov_segment();\n\n /*** downsample the feature points in a scan ***/\n downSizeFilterSurf.setInputCloud(feats_undistort);\n downSizeFilterSurf.filter(*feats_down_body);\n feats_down_size = feats_down_body->points.size();\n \n /*** initialize the map kdtree ***/\n if (ikdtree.Root_Node == nullptr) {\n if (feats_down_size > 5) {\n ikdtree.set_downsample_param(filter_size_map_min);\n feats_down_world->resize(feats_down_size);\n for (int i = 0; i < feats_down_size; i++) {\n pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));\n }\n ikdtree.Build(feats_down_world->points);\n }\n continue;\n }\n \n /*** ICP and iterated Kalman filter update ***/\n normvec->resize(feats_down_size);\n feats_down_world->resize(feats_down_size);\n euler_cur = RotMtoEuler(state.rot_end);\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 /*** iterated state estimation ***/\n std::vector body_var;\n std::vector crossmat_list;\n body_var.reserve(feats_down_size);\n crossmat_list.reserve(feats_down_size);\n\n double t_update_start = omp_get_wtime();\n\n for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) {\n laserCloudOri->clear();\n corr_normvect->clear();\n total_residual = 0.0;\n\n /** closest surface search and residual computation **/\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 PointType &point_body = feats_down_body->points[i];\n PointType &point_world = feats_down_world->points[i];\n V3D p_body(point_body.x, point_body.y, point_body.z);\n /// transform to world frame\n pointBodyToWorld(&point_body, &point_world);\n vector pointSearchSqDis(NUM_MATCH_POINTS);\n auto &points_near = Nearest_Points[i];\n uint8_t search_flag = 0;\n double search_start = omp_get_wtime();\n if (nearest_search_en) {\n /** Find the closest surfaces in the map **/\n ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 5);\n if (points_near.size() < NUM_MATCH_POINTS)\n point_selected_surf[i] = false;\n else\n point_selected_surf[i] = !(pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5);\n\n search_time_rec[i] = omp_get_wtime() - search_start;\n }\n\n res_last[i] = -1000.0f;\n\n if (!point_selected_surf[i] || points_near.size() < NUM_MATCH_POINTS) {\n point_selected_surf[i] = false;\n continue;\n }\n\n point_selected_surf[i] = false;\n VD(4) pabcd;\n pabcd.setZero();\n if (esti_plane(pabcd, points_near, 0.1)) //(planeValid)\n {\n float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z +\n pabcd(3);\n float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());\n\n if (s > 0.9) {\n point_selected_surf[i] = true;\n normvec->points[i].x = pabcd(0);\n normvec->points[i].y = pabcd(1);\n normvec->points[i].z = pabcd(2);\n normvec->points[i].intensity = pd2;\n res_last[i] = abs(pd2);\n }\n }\n }\n effect_feat_num = 0;\n for (int i = 0; i < feats_down_size; i++) {\n if (point_selected_surf[i]) {\n laserCloudOri->points[effect_feat_num] = feats_down_body->points[i];\n corr_normvect->points[effect_feat_num] = normvec->points[i];\n effect_feat_num++;\n }\n }\n\n res_mean_last = total_residual / effect_feat_num;\n \n\n /*** Computation of Measurement Jacobian matrix H and measurents vector ***/\n\n int residual_dim = effect_feat_num + 3;\n\n // Ground plane residual\n MatrixXd Hsub(residual_dim, 12);\n MatrixXd Hsub_T_R_inv(12, residual_dim);\n VectorXd R_inv(residual_dim);\n VectorXd meas_vec(residual_dim);\n\n Hsub.setZero();\n Hsub_T_R_inv.setZero();\n meas_vec.setZero();\n\n /*** Measurement: point-to-plane ***/\n for (int i = 0; i < effect_feat_num; i++) {\n const PointType &laser_p = laserCloudOri->points[i];\n V3D point_this_L(laser_p.x, laser_p.y, laser_p.z);\n\n V3D point_this = state.offset_R_L_I * point_this_L + state.offset_T_L_I;\n M3D var;\n calcBodyVar(point_this, 0.02, 0.05, var);\n var = state.rot_end * var * state.rot_end.transpose();\n M3D point_crossmat;\n point_crossmat << SKEW_SYM_MATRX(point_this);\n\n /*** get the normal vector of closest surface/corner ***/\n const PointType &norm_p = corr_normvect->points[i];\n V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);\n\n R_inv(i) = 1000;\n laserCloudOri->points[i].intensity = sqrt(R_inv(i));\n\n /*** calculate the Measurement Jacobian matrix H ***/\n if (imu_en) {\n M3D point_this_L_cross;\n point_this_L_cross << SKEW_SYM_MATRX(point_this_L);\n V3D H_R_LI = point_this_L_cross * state.offset_R_L_I.transpose() * state.rot_end.transpose() *\n norm_vec;\n V3D H_T_LI = state.rot_end.transpose() * norm_vec;\n V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);\n Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(\n H_R_LI), VEC_FROM_ARRAY(H_T_LI);\n } else {\n V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);\n Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, 0, 0, 0, 0, 0, 0;\n }\n\n Hsub_T_R_inv.col(i) = Hsub.row(i).transpose() * 1000;\n /*** Measurement: distance to the closest surface/corner ***/\n meas_vec(i) = -norm_p.intensity;\n }\n\n /*** Ground plane residual : Calculate the Measurement Jacobian matrix H ***/\n M3D R_GL_ = lidar_q.toRotationMatrix();\n M3D R_LG_ = R_GL_.transpose();\n\n V3D e1(1.0, 0.0, 0.0);\n V3D e2(0.0, 1.0, 0.0);\n V3D e3(0.0, 0.0, 1.0);\n\n M3D skew_sym_ground_norm;\n skew_sym_ground_norm << SKEW_SYM_MATRX(normal_lidar);\n\n V3D Jac_gp_first = skew_sym_ground_norm * R_LG_ * state.rot_end * e1;\n V3D Jac_gp_second = skew_sym_ground_norm * R_LG_ * state.rot_end * e2;\n V3D Jac_gp_third = R_LG_ * e3;\n\n Hsub.row(effect_feat_num) << VEC_FROM_ARRAY(Jac_gp_first), 0, 0, 0, 0, 0, 0, 0, 0, 0;\n Hsub_T_R_inv.col(effect_feat_num) = Hsub.row(effect_feat_num).transpose() * ground_cov;\n R_inv(effect_feat_num) = ground_cov;\n\n Hsub.row(effect_feat_num+1) << VEC_FROM_ARRAY(Jac_gp_second), 0, 0, 0, 0, 0, 0, 0, 0, 0;\n Hsub_T_R_inv.col(effect_feat_num+1) = Hsub.row(effect_feat_num+1).transpose() * ground_cov;\n R_inv(effect_feat_num+1) = ground_cov;\n\n Hsub.row(effect_feat_num+2) << 0, 0, 0, VEC_FROM_ARRAY(Jac_gp_third), 0, 0, 0, 0, 0, 0;\n Hsub_T_R_inv.col(effect_feat_num+2) = Hsub.row(effect_feat_num+2).transpose() * ground_cov;\n R_inv(effect_feat_num+2) = ground_cov;\n\n /* Ground plane residual */\n V3D meas_gp_ = R_LG_ * state.rot_end * normal_lidar;\n V3D meas_gp_last = R_LG_ * state.pos_end;\n\n double meas_gp_first_result = e1.transpose() * meas_gp_;\n double meas_gp_second_result = e2.transpose() * meas_gp_;\n double meas_gp_last_result = e3.transpose() * meas_gp_last;\n\n meas_vec(effect_feat_num) = meas_gp_first_result;\n meas_vec(effect_feat_num+1) = meas_gp_second_result;\n meas_vec(effect_feat_num+2) = meas_gp_last_result - 1.0;\n\n /* Kalman Gain */\n MatrixXd K(DIM_STATE, residual_dim);\n\n EKF_stop_flg = false;\n flg_EKF_converged = false;\n\n /*** Iterative Kalman Filter Update ***/\n H_T_H.block<12, 12>(0, 0) = Hsub_T_R_inv * Hsub;\n MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + state.cov.inverse()).inverse();\n K = K_1.block(0, 0) * Hsub_T_R_inv;\n auto vec = state_propagat - state;\n solution = K * meas_vec + vec - K * Hsub * vec.block<12, 1>(0, 0);\n\n //state update\n state += solution;\n\n rot_add = solution.block<3, 1>(0, 0);\n T_add = solution.block<3, 1>(3, 0);\n\n\n if ((rot_add.norm() * 57.3 < 0.01) && (T_add.norm() * 100 < 0.015))\n flg_EKF_converged = true;\n\n deltaR = rot_add.norm() * 57.3;\n deltaT = T_add.norm() * 100;\n\n euler_cur = RotMtoEuler(state.rot_end);\n\n /*** Rematch Judgement ***/\n nearest_search_en = false;\n if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (NUM_MAX_ITERATIONS - 2)))) {\n nearest_search_en = true;\n rematch_num++;\n }\n\n /*** Convergence Judgements and Covariance Update ***/\n if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == NUM_MAX_ITERATIONS - 1))) {\n if (flg_EKF_inited) {\n /*** Covariance Update ***/\n G.setZero();\n G.block(0, 0) = K * Hsub;\n state.cov = (I_STATE - G) * state.cov;\n total_distance += (state.pos_end - position_last).norm();\n position_last = state.pos_end;\n if (!imu_en) {\n geoQuat = tf::createQuaternionMsgFromRollPitchYaw\n (euler_cur(0), euler_cur(1), euler_cur(2));\n } else {\n //Publish LiDAR's pose, instead of IMU's pose\n M3D rot_cur_lidar = state.rot_end * state.offset_R_L_I;\n V3D euler_cur_lidar = RotMtoEuler(rot_cur_lidar);\n geoQuat = tf::createQuaternionMsgFromRollPitchYaw\n (euler_cur_lidar(0), euler_cur_lidar(1), euler_cur_lidar(2));\n }\n VD(DIM_STATE) K_sum = K.rowwise().sum();\n VD(DIM_STATE) P_diag = state.cov.diagonal();\n }\n EKF_stop_flg = true;\n }\n\n if (EKF_stop_flg) break;\n }\n\n\n for (int i = 0; i < feats_down_size; i++) kdtree_search_time += search_time_rec[i];\n \n \n /******* Publish odometry *******/\n publish_odometry(pubOdomAftMapped);\n\n /*** add the feature points to map kdtree ***/\n map_incremental();\n kdtree_size_end = ikdtree.size();\n\n /***** Device starts to move, data accmulation begins. ****/\n if (!imu_en && !data_accum_start && state.pos_end.norm() > 0.05) {\n printf(BOLDCYAN \"[Initialization] Movement detected, data accumulation starts.\\n\\n\\n\\n\\n\" RESET);\n data_accum_start = true;\n move_start_time = lidar_end_time;\n }\n\n /******* Publish points *******/\n if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes);\n if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body);\n last_odom = state.pos_end;\n last_rot = state.rot_end;\n publish_effect_world(pubLaserCloudEffect);\n if (path_en) publish_path(pubPath);\n \n frame_num++;\n V3D ext_euler = RotMtoEuler(state.offset_R_L_I);\n // for mat_out.txt\n fout_out << euler_cur.transpose() * 57.3 << \" \" << state.pos_end.transpose() << \" \"\n << ext_euler.transpose() * 57.3 << \" \" \\\n << state.offset_T_L_I.transpose() << \" \" << state.vel_end.transpose() << \" \" \\\n << \" \" << state.bias_g.transpose() << \" \" << state.bias_a.transpose() * 0.9822 / 9.81 << \" \"\n << state.gravity.transpose() << \" \" << total_distance << endl;\n\n if (!imu_en && !data_accum_finished && data_accum_start) {\n // Push Lidar's Angular velocity and linear velocity \n Calib_LI->push_Lidar_CalibState(state.rot_end, state.pos_end, state.bias_g, state.vel_end, lidar_end_time);\n Calib_LI->push_Plane_Constraint(lidar_q, imu_q_eigen, normal_lidar, lidar_estimate_height);\n \n //Data Accumulation Sufficience Appraisal\n data_accum_finished = Calib_LI->data_sufficiency_assess(Jaco_rot, frame_num, state.bias_g,\n orig_odom_freq, cut_frame_num, \n lidar_q, imu_q_eigen, lidar_estimate_height);\n\n if (data_accum_finished) {\n Calib_LI->LI_Calibration(orig_odom_freq, cut_frame_num, timediff_imu_wrt_lidar, move_start_time);\n\n online_calib_starts_time = lidar_end_time;\n\n // Transfer to state vector\n state.offset_R_L_I = Calib_LI->get_R_LI();\n state.offset_T_L_I = Calib_LI->get_T_LI();\n state.pos_end = -state.rot_end * state.offset_R_L_I.transpose() * state.offset_T_L_I +\n state.pos_end; // Body frame is IMU frame in FAST-LIO mode\n state.rot_end = state.rot_end * state.offset_R_L_I.transpose();\n state.gravity = Calib_LI->get_Grav_L0();\n state.bias_g = Calib_LI->get_gyro_bias();\n state.bias_a = Calib_LI->get_acc_bias();\n\n time_result = Calib_LI->get_time_result();\n\n // Output calibration result\n fout_result << \"LiDAR-IMU calibration result:\" << endl;\n fileout_calib_result();\n\n // Save LiDAR trajectory\n if(traj_save_en){\n saveTrajectory(traj_save_path);\n std::cout << \"save LiDAR trajectory !!\" << std::endl; \n }\n\n // ros shutdown\n ros::shutdown();\n }\n }\n }\n status = ros::ok();\n rate.sleep();\n }\n\n return 0;\n}" }, { "path": "src/preprocess.cpp", "content": "#include \"preprocess.h\"\n\n#define RETURN0 0x00\n#define RETURN0AND1 0x10\n\nconst bool time_list_cut_frame(PointType &x, PointType &y) {\n return (x.curvature < y.curvature);\n}\n\nPreprocess::Preprocess()\n : feature_enabled(0), lidar_type(AVIA), blind(1.0), point_filter_num(1) {\n inf_bound = 10;\n N_SCANS = 6;\n group_size = 8;\n disA = 0.01;\n disA = 0.1; // B?\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;\n smallp_intersect = 172.5;\n smallp_ratio = 1.2;\n given_offset_time = false;\n\n jump_up_limit = cos(jump_up_limit / 180 * M_PI);\n jump_down_limit = cos(jump_down_limit / 180 * M_PI);\n cos160 = cos(cos160 / 180 * M_PI);\n smallp_intersect = cos(smallp_intersect / 180 * M_PI);\n}\n\nPreprocess::~Preprocess() {}\n\nvoid Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) {\n feature_enabled = feat_en;\n lidar_type = lid_type;\n blind = bld;\n point_filter_num = pfilt_num;\n}\n\nvoid Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) {\n avia_handler(msg);\n *pcl_out = pl_surf;\n}\n\nvoid Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg,\n deque &pcl_out, deque &time_lidar,\n const int required_frame_num, int scan_count) {\n int plsize = msg->point_num;\n pl_surf.clear();\n pl_surf.reserve(plsize);\n pl_full.clear();\n pl_full.resize(plsize);\n int valid_point_num = 0;\n\n for (uint i = 1; i < plsize; i++) {\n if ((msg->points[i].line < N_SCANS) &&\n ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))\n {\n valid_point_num++;\n if (valid_point_num % point_filter_num == 0) {\n pl_full[i].x = msg->points[i].x;\n pl_full[i].y = msg->points[i].y;\n pl_full[i].z = msg->points[i].z;\n pl_full[i].intensity = msg->points[i].reflectivity;\n //use curvature as time of each laser points，unit: ms\n pl_full[i].curvature = msg->points[i].offset_time / float(1000000);\n\n double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;\n if (dist < blind) continue;\n\n if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)\n || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)\n || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) {\n pl_surf.push_back(pl_full[i]);\n }\n }\n }\n }\n sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);\n //end time of last frame，单位ms\n double last_frame_end_time = msg->header.stamp.toSec() * 1000;\n uint valid_num = 0;\n uint cut_num = 0;\n uint valid_pcl_size = pl_surf.points.size();\n\n int required_cut_num = required_frame_num;\n if (scan_count < 5)\n required_cut_num = 1;\n\n PointCloudXYZI pcl_cut;\n for (uint i = 1; i < valid_pcl_size; i++) {\n valid_num++;\n //Compute new opffset time of each point：ms\n pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time;\n pcl_cut.push_back(pl_surf[i]);\n if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {\n cut_num++;\n time_lidar.push_back(last_frame_end_time);\n PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI()); //Initialize shared_ptr\n *pcl_temp = pcl_cut;\n pcl_out.push_back(pcl_temp);\n //Update frame head\n last_frame_end_time += pl_surf[i].curvature;\n pcl_cut.clear();\n pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num);\n }\n }\n}\n#define MAX_LINE_NUM 128\nvoid\nPreprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque &pcl_out,\n deque &time_lidar, const int required_frame_num, int scan_count) {\n pl_surf.clear();\n pl_corn.clear();\n pl_full.clear();\n\n if (lidar_type == VELO) {\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 (deg/ms)\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 // given_offset_time = true;\n // } else {\n // cout << \"Compute offset time using constant rotation model.\" << endl;\n // given_offset_time = false;\n // memset(is_first, true, sizeof(is_first));\n // }\n\n if (pl_orig.points[plsize - 1].time > 0 && pl_orig.points[0].time > 0) {\n cout << \"Use given offset time.\" << endl;\n given_offset_time = true;\n } else {\n // cout << \"Compute offset time using constant rotation model.\" << endl;\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 if (pl_orig.points[i].ring == layer_first) {\n yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;\n break;\n }\n }\n }\n\n for (int i = 0; i < plsize; i++) {\n PointType added_pt;\n added_pt.normal_x = 0;\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\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; //ms\n\n double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;\n if ( dist < blind * blind || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))\n continue;\n\n if (!given_offset_time) {\n int layer = pl_orig.points[i].ring;\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;\n\n if (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 // compute offset time\n if (yaw_angle <= yaw_fp[layer]) {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;\n } else {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;\n }\n if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;\n\n yaw_last[layer] = yaw_angle;\n time_last[layer] = added_pt.curvature;\n }\n\n\n if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {\n pl_surf.points.push_back(added_pt);\n }\n }\n } \n\n else if (lidar_type == VELO_without_Time) {\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 (deg/ms)\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 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 if (pl_orig.points[i].ring == layer_first) {\n yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;\n break;\n }\n }\n\n for (int i = 0; i < plsize; i++) {\n PointType added_pt;\n added_pt.normal_x = 0;\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\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\n int layer = pl_orig.points[i].ring;\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;\n\n if (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 // compute offset time\n if (yaw_angle <= yaw_fp[layer]) {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;\n } else {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;\n }\n if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;\n\n yaw_last[layer] = yaw_angle;\n time_last[layer] = added_pt.curvature;\n\n if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {\n if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind)\n pl_surf.points.push_back(added_pt);\n }\n }\n }\n \n else if (lidar_type == OUSTER) {\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 double time_stamp = msg->header.stamp.toSec();\n\n for (int i = 0; i < pl_orig.points.size(); i++)\n {\n\n if (i % point_filter_num != 0) 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) 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;\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 } \n else {\n ROS_ERROR(\"Lidar type not supported!\");\n }\n\n sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);\n\n //ms\n double last_frame_end_time = msg->header.stamp.toSec() * 1000;\n uint valid_num = 0;\n uint cut_num = 0;\n uint valid_pcl_size = pl_surf.points.size();\n\n int required_cut_num = required_frame_num;\n\n if (scan_count < 20)\n required_cut_num = 1;\n\n\n PointCloudXYZI pcl_cut;\n for (uint i = 1; i < valid_pcl_size; i++) {\n valid_num++;\n pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time;\n pcl_cut.push_back(pl_surf[i]);\n\n if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {\n cut_num++;\n time_lidar.push_back(last_frame_end_time);\n PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI());\n *pcl_temp = pcl_cut;\n pcl_out.push_back(pcl_temp);\n last_frame_end_time += pl_surf[i].curvature;\n pcl_cut.clear();\n pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num);\n }\n }\n}\n\nvoid Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) {\n switch (lidar_type) {\n case OUSTER:\n oust_handler(msg);\n break;\n case VELO:\n velodyne_handler(msg);\n break;\n case L515:\n l515_handler(msg);\n break;\n case VELO_NCLT:\n velodyne_handler_nclt(msg);\n break;\n\n default:\n printf(\"Error LiDAR Type\");\n break;\n }\n *pcl_out = pl_surf;\n}\n\nvoid Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) {\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\n pl_corn.reserve(plsize);\n pl_surf.reserve(plsize);\n pl_full.resize(plsize);\n\n for (int i = 0; i < N_SCANS; i++) {\n pl_buff[i].clear();\n pl_buff[i].reserve(plsize);\n }\n uint valid_num = 0;\n\n if (feature_enabled) {\n for (uint i = 1; i < plsize; i++) {\n if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10) ||\n ((msg->points[i].tag & 0x30) == 0x00)) {\n pl_full[i].x = msg->points[i].x;\n pl_full[i].y = msg->points[i].y;\n pl_full[i].z = msg->points[i].z;\n pl_full[i].intensity = msg->points[i].reflectivity;\n pl_full[i].curvature =\n msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points\n double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;\n if (dist < blind) continue;\n\n if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)\n || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)\n || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) {\n pl_buff[msg->points[i].line].push_back(pl_full[i]);\n }\n }\n }\n static int count = 0;\n static double time = 0.0;\n count++;\n double t0 = omp_get_wtime();\n for (int j = 0; j < N_SCANS; j++) {\n if (pl_buff[j].size() <= 5) 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 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 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 } else {\n for (uint i = 1; i < plsize; i++) {\n if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 ||\n (msg->points[i].tag & 0x30) == 0x00)) {\n valid_num++;\n if (valid_num % point_filter_num == 0) {\n pl_full[i].x = msg->points[i].x;\n pl_full[i].y = msg->points[i].y;\n pl_full[i].z = msg->points[i].z;\n pl_full[i].intensity = msg->points[i].reflectivity;\n pl_full[i].curvature =\n msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points\n\n double dist =\n pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;\n if (dist < blind) continue;\n\n if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)\n || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)\n || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) {\n pl_surf.push_back(pl_full[i]);\n }\n }\n }\n }\n }\n}\n\nvoid Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {\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\n for (int i = 0; i < pl_orig.points.size(); i++) {\n\n if (i % point_filter_num != 0) 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 +\n pl_orig.points[i].z * pl_orig.points[i].z;\n\n if (range < blind) 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.normal_x = pl_orig.points[i].r;\n added_pt.normal_y = pl_orig.points[i].g;\n added_pt.normal_z = pl_orig.points[i].b;\n added_pt.curvature = 0.0;\n pl_surf.points.push_back(added_pt);\n }\n}\n\nvoid Preprocess::oust_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {\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 for (int i = 0; i < N_SCANS; i++) {\n pl_buff[i].clear();\n pl_buff[i].reserve(plsize);\n }\n\n for (uint i = 0; i < plsize; i++) {\n double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y +\n pl_orig.points[i].z * pl_orig.points[i].z;\n if (range < blind) 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 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 pl_buff[pl_orig.points[i].ring].push_back(added_pt);\n }\n }\n\n for (int j = 0; j < N_SCANS; j++) {\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 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 } else {\n for (int i = 0; i < pl_orig.points.size(); i++) {\n if (i % point_filter_num != 0) 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 +\n pl_orig.points[i].z * pl_orig.points[i].z;\n\n if (range < blind) 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;\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\n added_pt.curvature = pl_orig.points[i].t / 1e6; //ms\n if (pl_orig.points[i].ring < N_SCANS)\n pl_surf.points.push_back(added_pt);\n }\n }\n}\n\n// #define MAX_LINE_NUM 64\n\nvoid Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {\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 (deg/ms)\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 && pl_orig.points[0].time > 0) {\n cout << \"Use given offset time.\" << endl;\n given_offset_time = true;\n } else {\n cout << \"Compute offset time using constant rotation model.\" << endl;\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 if (pl_orig.points[i].ring == layer_first) {\n yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;\n break;\n }\n }\n }\n\n if (feature_enabled) {\n for (int i = 0; i < N_SCANS; i++) {\n pl_buff[i].clear();\n pl_buff[i].reserve(plsize);\n }\n\n for (int i = 0; i < plsize; i++) {\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) 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; // unit: ms\n\n if (!given_offset_time) {\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;\n if (is_first[layer]) {\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 added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;\n } else {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;\n }\n\n if (added_pt.curvature < time_last[layer]) 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 PointCloudXYZI &pl = pl_buff[j];\n int linesize = pl.size();\n if (linesize < 2) continue;\n vector &types = typess[j];\n types.clear();\n types.resize(linesize);\n linesize--;\n for (uint i = 0; i < linesize; i++) {\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 } else {\n for (int i = 0; i < plsize; i++) {\n PointType added_pt;\n\n added_pt.normal_x = 0;\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\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; //ms\n\n if (!given_offset_time) {\n int layer = pl_orig.points[i].ring;\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;\n\n if (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 // compute offset time\n if (yaw_angle <= yaw_fp[layer]) {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;\n } else {\n added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;\n }\n\n if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;\n\n yaw_last[layer] = yaw_angle;\n time_last[layer] = added_pt.curvature;\n }\n\n if (i % point_filter_num == 0) {\n if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind) {\n pl_surf.points.push_back(added_pt);\n }\n }\n }\n }\n}\n\nvoid Preprocess::velodyne_handler_nclt(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 if (plsize == 0) return;\n pl_surf.reserve(plsize);\n\n /*** These variables only works when no point timestamps given ***/\n double omega_l = 0.361 * SCAN_RATE; // scan angular velocity\n std::vector is_first(N_SCANS,true);\n std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point\n std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point\n std::vector time_last(N_SCANS, 0.0); // last offset time\n /*****************************************************************/\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 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;\n break;\n }\n }\n }\n\n for (int i = 0; i < plsize; i++)\n {\n PointType added_pt;\n \n added_pt.normal_x = 0;\n added_pt.normal_y = 0;\n added_pt.normal_z = 0;\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 * 1.e3f; // curvature unit: sec\n\n \n int layer = pl_orig.points[i].ring;\n double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;\n\n if (is_first[layer])\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 // compute offset time\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]) added_pt.curvature+=360.0/omega_l;\n\n yaw_last[layer] = yaw_angle;\n time_last[layer]=added_pt.curvature;\n \n\n if (i % point_filter_num == 0)\n {\n if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > (blind * blind))\n {\n pl_surf.points.push_back(added_pt);\n }\n }\n }\n}\n\nvoid Preprocess::velodyne_handler_kitti(const sensor_msgs::PointCloud2::ConstPtr &msg) {\n pl_surf.clear();\n pl_full.clear();\n pcl::fromROSMsg(*msg, pl_full);\n for (int i = 0; i < pl_full.size(); i++) {\n if (i % point_filter_num == 0) {\n double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;\n if (dist > blind)\n pl_surf.points.push_back(pl_full[i]);\n }\n }\n}\n\nvoid Preprocess::give_feature(pcl::PointCloud &pl, vector &types) {\n int plsize = pl.size();\n int plsize2;\n if (plsize == 0) {\n printf(\"something wrong\\n\");\n return;\n }\n uint head = 0;\n\n while (types[head].range < blind) {\n head++;\n }\n\n // Surf\n plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;\n\n Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());\n Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());\n\n uint i_nex = 0, i2;\n uint last_i = 0;\n uint last_i_nex = 0;\n int last_state = 0;\n int plane_type;\n\n for (uint i = head; i < plsize2; i++) {\n if (types[i].range < blind) {\n continue;\n }\n\n i2 = i;\n\n plane_type = plane_judge(pl, types, i, i_nex, curr_direct);\n\n if (plane_type == 1) {\n for (uint j = i; j <= i_nex; j++) {\n if (j != i && j != i_nex) {\n types[j].ftype = Real_Plane;\n } else {\n types[j].ftype = Poss_Plane;\n }\n }\n\n // if(last_state==1 && fabs(last_direct.sum())>0.5)\n if (last_state == 1 && last_direct.norm() > 0.1) {\n double mod = last_direct.transpose() * curr_direct;\n if (mod > -0.707 && mod < 0.707) {\n types[i].ftype = Edge_Plane;\n } else {\n types[i].ftype = Real_Plane;\n }\n }\n\n i = i_nex - 1;\n last_state = 1;\n } else // if(plane_type == 2)\n {\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;\n last_i_nex = i_nex;\n last_direct = curr_direct;\n }\n\n plsize2 = plsize > 3 ? plsize - 3 : 0;\n for (uint i = head + 3; i < plsize2; i++) {\n if (types[i].range < blind || types[i].ftype >= Real_Plane) {\n continue;\n }\n\n if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) {\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 int m = -1;\n if (j == 1) {\n m = 1;\n }\n\n if (types[i + m].range < blind) {\n if (types[i].range > inf_bound) {\n types[i].edj[j] = Nr_inf;\n } else {\n types[i].edj[j] = 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\n types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();\n if (types[i].angle[j] < jump_up_limit) {\n types[i].edj[j] = Nr_180;\n } else if (types[i].angle[j] > jump_down_limit) {\n types[i].edj[j] = Nr_zero;\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 &&\n types[i].dista > 4 * types[i - 1].dista) {\n if (types[i].intersect > cos160) {\n if (edge_jump_judge(pl, types, i, Prev)) {\n types[i].ftype = Edge_Jump;\n }\n }\n } else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 &&\n types[i - 1].dista > 4 * types[i].dista) {\n if (types[i].intersect > cos160) {\n if (edge_jump_judge(pl, types, i, Next)) {\n types[i].ftype = Edge_Jump;\n }\n }\n } else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) {\n if (edge_jump_judge(pl, types, i, Prev)) {\n types[i].ftype = Edge_Jump;\n }\n } else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) {\n if (edge_jump_judge(pl, types, i, Next)) {\n types[i].ftype = Edge_Jump;\n }\n\n } else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) {\n if (types[i].ftype == Nor) {\n types[i].ftype = Wire;\n }\n }\n }\n\n plsize2 = plsize - 1;\n double ratio;\n for (uint i = head + 1; i < plsize2; i++) {\n if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind) {\n continue;\n }\n\n if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) {\n continue;\n }\n\n if (types[i].ftype == Nor) {\n if (types[i - 1].dista > types[i].dista) {\n ratio = types[i - 1].dista / types[i].dista;\n } else {\n ratio = types[i].dista / types[i - 1].dista;\n }\n\n if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) {\n if (types[i - 1].ftype == Nor) {\n types[i - 1].ftype = Real_Plane;\n }\n if (types[i + 1].ftype == Nor) {\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 if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) {\n if (last_surface == -1) {\n last_surface = j;\n }\n\n if (j == uint(last_surface + point_filter_num - 1)) {\n PointType ap;\n ap.x = pl[j].x;\n ap.y = pl[j].y;\n ap.z = pl[j].z;\n ap.curvature = pl[j].curvature;\n pl_surf.push_back(ap);\n\n last_surface = -1;\n }\n } else {\n if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) {\n pl_corn.push_back(pl[j]);\n }\n if (last_surface != -1) {\n PointType ap;\n for (uint k = last_surface; k < j; k++) {\n ap.x += pl[k].x;\n ap.y += pl[k].y;\n ap.z += pl[k].z;\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.curvature /= (j - last_surface);\n pl_surf.push_back(ap);\n }\n last_surface = -1;\n }\n }\n}\n\n// void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct) {\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\nint Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex,\n Eigen::Vector3d &curr_direct) {\n double group_dis = disA * types[i_cur].range + disB;\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 for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) {\n if (types[i_nex].range < blind) {\n curr_direct.setZero();\n return 2;\n }\n disarr.push_back(types[i_nex].dista);\n }\n\n for (;;) {\n if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break;\n\n if (types[i_nex].range < blind) {\n curr_direct.setZero();\n return 2;\n }\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) {\n break;\n }\n disarr.push_back(types[i_nex].dista);\n 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 if ((j >= pl.size()) || (i_cur >= pl.size())) break;\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\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\n double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2];\n if (lw > leng_wid) {\n leng_wid = lw;\n }\n }\n\n\n if ((two_dis * two_dis / leng_wid) < p2l_ratio) {\n curr_direct.setZero();\n return 0;\n }\n\n uint disarrsize = disarr.size();\n for (uint j = 0; j < disarrsize - 1; j++) {\n for (uint k = j + 1; k < disarrsize; k++) {\n if (disarr[j] < disarr[k]) {\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 curr_direct.setZero();\n return 0;\n }\n\n if (lidar_type == AVIA) {\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 curr_direct.setZero();\n return 0;\n }\n } else {\n double dismax_min = disarr[0] / disarr[disarrsize - 2];\n if (dismax_min >= limit_maxmin) {\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 if (nor_dir == 0) {\n if (types[i - 1].range < blind || types[i - 2].range < blind) {\n return false;\n }\n } else if (nor_dir == 1) {\n if (types[i + 1].range < blind || types[i + 2].range < blind) {\n return false;\n }\n }\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 d = d1;\n d1 = d2;\n d2 = d;\n }\n\n d1 = sqrt(d1);\n d2 = sqrt(d2);\n\n\n if (d1 > edgea * d2 || (d1 - d2) > edgeb) {\n return false;\n }\n\n return true;\n}" } ]