Repository: Taeyoung96/GRIL-Calib Branch: master Commit: 819d86dea09b Files: 64 Total size: 574.8 KB Directory structure: gitextract_kkt8sabp/ ├── CMakeLists.txt ├── LI-Init-LICENSE ├── README.md ├── config/ │ ├── hilti/ │ │ ├── Basement_3.yaml │ │ └── Basement_4.yaml │ ├── m2dgr/ │ │ ├── velodyne_m2dgr_gate01.yaml │ │ ├── velodyne_m2dgr_hall04.yaml │ │ ├── velodyne_m2dgr_lift04.yaml │ │ ├── velodyne_m2dgr_rotation02.yaml │ │ └── velodyne_m2dgr_street08.yaml │ ├── ouster64.yaml │ ├── s3e/ │ │ ├── velodyne_s3e_bob_lab01.yaml │ │ └── velodyne_s3e_bob_lab02.yaml │ └── velodyne16.yaml ├── docker/ │ ├── Dockerfile │ ├── container_run.sh │ └── ros_entrypoint.sh ├── include/ │ ├── Fusion/ │ │ ├── CMakeLists.txt │ │ ├── Fusion.h │ │ ├── FusionAhrs.c │ │ ├── FusionAhrs.h │ │ ├── FusionAxes.h │ │ ├── FusionCalibration.h │ │ ├── FusionCompass.c │ │ ├── FusionCompass.h │ │ ├── FusionMath.h │ │ ├── FusionOffset.c │ │ └── FusionOffset.h │ ├── Gril_Calib/ │ │ ├── Gril_Calib.cpp │ │ └── Gril_Calib.h │ ├── GroundSegmentation/ │ │ ├── patchworkpp.hpp │ │ └── utils.hpp │ ├── IMU_Processing.hpp │ ├── color.h │ ├── common_lib.h │ ├── ikd-Tree/ │ │ ├── README.md │ │ ├── ikd_Tree.cpp │ │ └── ikd_Tree.h │ ├── matplotlibcpp.h │ ├── preprocess.h │ ├── scope_timer.hpp │ └── so3_math.h ├── launch/ │ ├── hilti/ │ │ ├── hilti_basement_3.launch │ │ └── hilti_basement_4.launch │ ├── m2dgr/ │ │ ├── m2dgr_gate01.launch │ │ ├── m2dgr_hall04.launch │ │ ├── m2dgr_lift04.launch │ │ ├── m2dgr_rotation02.launch │ │ └── m2dgr_street08.launch │ ├── ouster.launch │ ├── s3e/ │ │ ├── s3e_bob_lab01.launch │ │ └── s3e_bob_lab02.launch │ └── velodyne.launch ├── msg/ │ ├── Pose6D.msg │ └── States.msg ├── package.xml ├── result/ │ └── traj.txt ├── rviz_cfg/ │ ├── .gitignore │ ├── hilti.rviz │ ├── m2dgr.rviz │ ├── s3e.rviz │ └── spinning.rviz └── src/ ├── laserMapping.cpp └── preprocess.cpp ================================================ FILE CONTENTS ================================================ ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(gril_calib) SET(CMAKE_BUILD_TYPE "Release") ADD_COMPILE_OPTIONS(-std=c++14 ) # ADD_COMPILE_OPTIONS(-std=c++14 ) set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") message("Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) include(ProcessorCount) ProcessorCount(N) message("Processer number: ${N}") if(N GREATER 4) add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM=3) message("core for MP: 3") elseif(N GREATER 3) add_definitions(-DMP_EN) add_definitions(-DMP_PROC_NUM=2) message("core for MP: 2") else() add_definitions(-DMP_PROC_NUM=1) endif() else() add_definitions(-DMP_PROC_NUM=1) endif() find_package(OpenMP QUIET) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") find_package(PythonLibs REQUIRED) find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs sensor_msgs roscpp rospy std_msgs pcl_ros tf livox_ros_driver message_generation eigen_conversions ) find_package(Eigen3 REQUIRED) find_package(PCL 1.8 REQUIRED) find_package(Ceres REQUIRED) message(Eigen: ${EIGEN3_INCLUDE_DIR}) include_directories( ../../devel/include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} include) add_message_files( FILES Pose6D.msg States.msg ) generate_messages( DEPENDENCIES geometry_msgs ) catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime DEPENDS EIGEN3 PCL INCLUDE_DIRS ) add_executable(gril_calib src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp include/Gril_Calib/Gril_Calib.cpp src/preprocess.cpp include/Fusion/FusionAhrs.c include/Fusion/FusionCompass.c include/Fusion/FusionOffset.c) target_link_libraries(gril_calib ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${CERES_LIBRARIES}) target_include_directories(gril_calib PRIVATE ${PYTHON_INCLUDE_DIRS}) ================================================ FILE: LI-Init-LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 2, June 1991 Copyright (C) 1989, 1991 Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change free software--to make sure the software is free for all its users. This General Public License applies to most of the Free Software Foundation's software and to any other program whose authors commit to using it. (Some other Free Software Foundation software is covered by the GNU Lesser General Public License instead.) You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for this service if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs; and that you know you can do these things. To protect your rights, we need to make restrictions that forbid anyone to deny you these rights or to ask you to surrender the rights. These restrictions translate to certain responsibilities for you if you distribute copies of the software, or if you modify it. For example, if you distribute copies of such a program, whether gratis or for a fee, you must give the recipients all the rights that you have. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. We protect your rights with two steps: (1) copyright the software, and (2) offer you this license which gives you legal permission to copy, distribute and/or modify the software. Also, for each author's protection and ours, we want to make certain that everyone understands that there is no warranty for this free software. If the software is modified by someone else and passed on, we want its recipients to know that what they have is not the original, so that any problems introduced by others will not reflect on the original authors' reputations. Finally, any free program is threatened constantly by software patents. We wish to avoid the danger that redistributors of a free program will individually obtain patent licenses, in effect making the program proprietary. To prevent this, we have made it clear that any patent must be licensed for everyone's free use or not licensed at all. The precise terms and conditions for copying, distribution and modification follow. GNU GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 0. This License applies to any program or other work which contains a notice placed by the copyright holder saying it may be distributed under the terms of this General Public License. The "Program", below, refers to any such program or work, and a "work based on the Program" means either the Program or any derivative work under copyright law: that is to say, a work containing the Program or a portion of it, either verbatim or with modifications and/or translated into another language. (Hereinafter, translation is included without limitation in the term "modification".) Each licensee is addressed as "you". Activities other than copying, distribution and modification are not covered by this License; they are outside its scope. The act of running the Program is not restricted, and the output from the Program is covered only if its contents constitute a work based on the Program (independent of having been made by running the Program). Whether that is true depends on what the Program does. 1. You may copy and distribute verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice and disclaimer of warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and give any other recipients of the Program a copy of this License along with the Program. You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. 2. You may modify your copy or copies of the Program or any portion of it, thus forming a work based on the Program, and copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of these conditions: a) You must cause the modified files to carry prominent notices stating that you changed the files and the date of any change. b) You must cause any work that you distribute or publish, that in whole or in part contains or is derived from the Program or any part thereof, to be licensed as a whole at no charge to all third parties under the terms of this License. c) If the modified program normally reads commands interactively when run, you must cause it, when started running for such interactive use in the most ordinary way, to print or display an announcement including an appropriate copyright notice and a notice that there is no warranty (or else, saying that you provide a warranty) and that users may redistribute the program under these conditions, and telling the user how to view a copy of this License. (Exception: if the Program itself is interactive but does not normally print such an announcement, your work based on the Program is not required to print an announcement.) These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the Program, and can be reasonably considered independent and separate works in themselves, then this License, and its terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same sections as part of a whole which is a work based on the Program, the distribution of the whole must be on the terms of this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part regardless of who wrote it. Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you; rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the Program. In addition, mere aggregation of another work not based on the Program with the Program (or with a work based on the Program) on a volume of a storage or distribution medium does not bring the other work under the scope of this License. 3. You may copy and distribute the Program (or a work based on it, under Section 2) in object code or executable form under the terms of Sections 1 and 2 above provided that you also do one of the following: a) Accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, b) Accompany it with a written offer, valid for at least three years, to give any third party, for a charge no more than your cost of physically performing source distribution, a complete machine-readable copy of the corresponding source code, to be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange; or, c) Accompany it with the information you received as to the offer to distribute corresponding source code. (This alternative is allowed only for noncommercial distribution and only if you received the program in object code or executable form with such an offer, in accord with Subsection b above.) The source code for a work means the preferred form of the work for making modifications to it. For an executable work, complete source code means all the source code for all modules it contains, plus any associated interface definition files, plus the scripts used to control compilation and installation of the executable. However, as a special exception, the source code distributed need not include anything that is normally distributed (in either source or binary form) with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless that component itself accompanies the executable. If distribution of executable or object code is made by offering access to copy from a designated place, then offering equivalent access to copy the source code from the same place counts as distribution of the source code, even though third parties are not compelled to copy the source along with the object code. 4. You may not copy, modify, sublicense, or distribute the Program except as expressly provided under this License. Any attempt otherwise to copy, modify, sublicense or distribute the Program is void, and will automatically terminate your rights under this License. However, parties who have received copies, or rights, from you under this License will not have their licenses terminated so long as such parties remain in full compliance. 5. You are not required to accept this License, since you have not signed it. However, nothing else grants you permission to modify or distribute the Program or its derivative works. These actions are prohibited by law if you do not accept this License. Therefore, by modifying or distributing the Program (or any work based on the Program), you indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or modifying the Program or works based on it. 6. Each time you redistribute the Program (or any work based on the Program), the recipient automatically receives a license from the original licensor to copy, distribute or modify the Program subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties to this License. 7. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot distribute so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not distribute the Program at all. For example, if a patent license would not permit royalty-free redistribution of the Program by all those who receive copies directly or indirectly through you, then the only way you could satisfy both it and this License would be to refrain entirely from distribution of the Program. If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the section is intended to apply and the section as a whole is intended to apply in other circumstances. It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest validity of any such claims; this section has the sole purpose of protecting the integrity of the free software distribution system, which is implemented by public license practices. Many people have made generous contributions to the wide range of software distributed through that system in reliance on consistent application of that system; it is up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee cannot impose that choice. This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License. 8. If the distribution and/or use of the Program is restricted in certain countries either by patents or by copyrighted interfaces, the original copyright holder who places the Program under this License may add an explicit geographical distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus excluded. In such case, this License incorporates the limitation as if written in the body of this License. 9. The Free Software Foundation may publish revised and/or new versions of the General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies a version number of this License which applies to it and "any later version", you have the option of following the terms and conditions either of that version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of this License, you may choose any version ever published by the Free Software Foundation. 10. If you wish to incorporate parts of the Program into other free programs whose distribution conditions are different, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation, write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. NO WARRANTY 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively convey the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. Also add information on how to contact you by electronic and paper mail. If the program is interactive, make it output a short notice like this when it starts in an interactive mode: Gnomovision version 69, Copyright (C) year name of author Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, the commands you use may be called something other than `show w' and `show c'; they could even be mouse-clicks or menu items--whatever suits your program. You should also get your employer (if you work as a programmer) or your school, if any, to sign a "copyright disclaimer" for the program, if necessary. Here is a sample; alter the names: Yoyodyne, Inc., hereby disclaims all copyright interest in the program `Gnomovision' (which makes passes at compilers) written by James Hacker. , 1 April 1989 Ty Coon, President of Vice This General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. ================================================ FILE: README.md ================================================ # GRIL-Calib [ROS1 / ROS2] Official implementation of our paper **"GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrinsic Calibration Method using Ground Plane Motion Constraints"**. - ArXiv : [https://arxiv.org/abs/2312.14035](https://arxiv.org/abs/2312.14035) - IEEE : [https://ieeexplore.ieee.org/document/10506583](https://ieeexplore.ieee.org/document/10506583) ## About GRIL-Calib

- **GRIL-Calib** is the LiDAR-IMU calibration method for ground robots. - Using only **planar motion**, the 6-DOF calibration parameter could be estimated. ## 🚀 ROS2 Support If you want to use ROS2 version, check out `humble` branch. ## Prerequisites (ROS1 version) - Ubuntu 18.04 - ROS Melodic - PCL >= 1.8 - Eigen >= 3.3.4 - [livox_ros_driver](https://github.com/Livox-SDK/livox_ros_driver) - [ceres-solver-2.0.0](http://ceres-solver.org/installation.html#linux) ### Set up your environment easily with Docker! 🐳 **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.** **1. Enter the `/docker` folder and make a docker image.** ``` git clone https://github.com/Taeyoung96/GRIL-Calib.git ``` ``` cd GRIL-Calib/docker ``` ``` docker build -t gril-calib . ``` When you have finished it, use the command `docker images` and you can see the output below. ``` REPOSITORY TAG IMAGE ID CREATED SIZE gril-calib latest 9f90339349a0 5 months ago 3.78GB ``` **2. Make docker container (same path as above)** In `/docker`, ``` sudo chmod -R 777 container_run.sh ``` ``` ./container_run.sh ``` **:warning: You should change {container_name}, {docker image} to suit your environment.** ``` ./container_run.sh gril-calib-container gril-calib:latest ``` If you have successfully created the docker container, the terminal output will be similar to the below. ``` ================Gril-Calib Docker Env Ready================ root@taeyoung-cilab:/root/catkin_ws# ``` **3. Build and run GRIL-Calib** Inside the docker container, build and run the package. ``` catkin_make ``` ``` source devel/setup.bash ``` ## Run with a public dataset The 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. - For M2DGR, ``` roslaunch gril_calib m2dgr_xxxx.launch ``` - For HILTI, ``` roslaunch gril_calib hilti_xxxx.launch ``` - For S3E, ``` roslaunch gril_calib s3e_xxxx.launch ``` After running the launch file, you simply run the bag file for each sequence. ## Run with your custom data **:warning: This version only supports Spinning LiDAR (Velodyne, Ouster), not Solid-state LiDAR.** The 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. If we could achieve ground segmentation, we could theoretically do it for a Solid-state LiDAR like Livox Avia. - Make sure to acquire your data on an area with flat ground. - It would be helpful to collect data as the ground robot draws an "8". - Please make sure the unit of your input angular velocity is rad/s. ### Important parameters Similar to [LI-Init](https://github.com/hku-mars/LiDAR_IMU_Init), edit `config/xxx.yaml` to set the below parameters: - `lid_topic`: Topic name of LiDAR point cloud. - `imu_topic`: Topic name of IMU measurements. - `imu_sensor_height`: Height from ground to IMU sensor (meter) - `data_accum_length`: A threshold to assess if the data is enough for calibration. - `x_accumulate`: Parameter that determines how much the x-axis rotates (Assuming the x-axis is front) - `y_accumulate`: Parameter that determines how much the y-axis rotates (Assuming the y-axis is left) - `z_accumulate`: Parameter that determines how much the z-axis rotates (Assuming the z-axis is up) - `gyro_factor`, `acc_factor`, `ground_factor`: Weight for each residual - `set_boundary`: When performing nonlinear optimization, set the bound based on the initial value. (only translation vector) - `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. ## Acknowledgments Thanks to [hku-mars/LiDAR_IMU_Init](https://github.com/hku-mars/LiDAR_IMU_Init) for sharing their awesome work! We also thanks to [url-kaist/patchwork-plusplus-ros](https://github.com/url-kaist/patchwork-plusplus-ros) for sharing LiDAR ground segmentation algorithm. ## Citation If you find our paper useful in your research, please cite us using the following entry: ```BibTeX @ARTICLE{10506583, author={Kim, TaeYoung and Pak, Gyuhyeon and Kim, Euntai}, journal={IEEE Robotics and Automation Letters}, title={GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrinsic Calibration Method Using Ground Plane Motion Constraints}, year={2024}, volume={9}, number={6}, pages={5409-5416}, keywords={Calibration;Laser radar;Robot sensing systems;Robots;Optimization;Odometry;Vectors;Calibration and identification;sensor fusion}, doi={10.1109/LRA.2024.3392081}} ``` ================================================ FILE: config/hilti/Basement_3.yaml ================================================ # rosbag play -s 85 -u 230 Basement_3.bag common: lid_topic: "/os_cloud_node/points" imu_topic: "/alphasense/imu" preprocess: lidar_type: 3 # Ouster LiDAR scan_line: 64 blind: 2 feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 1.69 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 350 # To determine how much data is used x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.90 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : 0.1 trans_IL_y : 0.2 trans_IL_z : 0.4 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 0.5 filter_size_map: 0.5 gyr_cov: 0.5 acc_cov: 0.5 det_range: 100.0 ground_cov : 100.0 publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: false traj_file_path: "/root/catkin_ws/src/result/traj.txt" # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 1.60 # Height from ground to LiDAR sensor (meter) mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 5 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 5.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/hilti/Basement_4.yaml ================================================ # rosbag play -s 75 -u 150 Basement_4.bag common: lid_topic: "/os_cloud_node/points" imu_topic: "/alphasense/imu" preprocess: lidar_type: 3 # Ouster LiDAR scan_line: 64 blind: 0.5 feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 1.69 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 200 # To determine how much data is used 220 210 x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.90 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : 0.1 trans_IL_y : 0.2 trans_IL_z : 0.4 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 0.5 filter_size_map: 0.5 gyr_cov: 0.5 acc_cov: 0.5 det_range: 100.0 ground_cov : 200.0 publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: false traj_file_path: "/root/catkin_ws/src/result/traj.txt" # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 1.60 # Height from ground to LiDAR sensor (meter) mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 5 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 5.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/m2dgr/velodyne_m2dgr_gate01.yaml ================================================ # rosbag play -u 140 gate_01.bag common: lid_topic: "/velodyne_points" imu_topic: "/handsfree/imu" preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 32 # LiDAR scan line number blind: 1 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 270 # To determine how much data is used x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : -0.6 trans_IL_y : -0.45 trans_IL_z : -0.6 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 0.5 filter_size_map: 0.5 gyr_cov: 0.5 acc_cov: 0.5 det_range: 200.0 # LiDAR range ground_cov : 500.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: true traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/m2dgr/velodyne_m2dgr_hall04.yaml ================================================ # rosbag play -u 160 hall_04.bag common: lid_topic: "/velodyne_points" imu_topic: "/handsfree/imu" preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 32 # LiDAR scan line number blind: 0.5 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 400 # To determine how much data is used x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : -0.6 trans_IL_y : -0.45 trans_IL_z : -0.6 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 0.5 filter_size_map: 0.5 gyr_cov: 0.5 acc_cov: 0.5 det_range: 200.0 # LiDAR range ground_cov : 220.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: false traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/m2dgr/velodyne_m2dgr_lift04.yaml ================================================ # rosbag play -u 80 lift_04.bag common: lid_topic: "/velodyne_points" imu_topic: "/handsfree/imu" preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 32 # LiDAR scan line number blind: 0.5 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 170 # To determine how much data is used x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.76 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : -0.6 trans_IL_y : -0.45 trans_IL_z : -0.6 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 0.5 # 1.0 # 0.5 filter_size_map: 0.5 # 1.0 # 0.5 gyr_cov: 0.5 # 0.5 # 50 acc_cov: 0.5 # 1.0 # 2 det_range: 200.0 # LiDAR range ground_cov : 10.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: true traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/m2dgr/velodyne_m2dgr_rotation02.yaml ================================================ # rosbag play -u 120 rotation_02.bag common: lid_topic: "/velodyne_points" imu_topic: "/handsfree/imu" preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 32 # LiDAR scan line number blind: 1 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.73 # absolute height from IMU sensor origin to ground (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 1300 # To determine how much data is used x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : -0.6 trans_IL_y : -0.45 trans_IL_z : -0.6 set_boundary : false bound_th : 0.1 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 1.0 # 1.0 # 0.5 filter_size_map: 1.0 # 1.0 # 0.5 gyr_cov: 0.5 # 0.5 # 50 acc_cov: 0.5 # 1.0 # 2 det_range: 200.0 # LiDAR range ground_cov : 2500.0 # 2500.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: false traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/m2dgr/velodyne_m2dgr_street08.yaml ================================================ # rosbag play -s 20 -u 110 street_08.bag common: lid_topic: "/velodyne_points" imu_topic: "/handsfree/imu" preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 32 # LiDAR scan line number blind: 1 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.73 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 400 # To determine how much data is used x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.58 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : -0.6 trans_IL_y : -0.45 trans_IL_z : -0.6 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 1.0 filter_size_map: 1.0 gyr_cov: 0.5 acc_cov: 0.5 det_range: 200.0 # LiDAR range ground_cov : 1000.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: true traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.85 # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/ouster64.yaml ================================================ common: lid_topic: "/os1_cloud_node/points" imu_topic: "/os1_cloud_node/imu" preprocess: lidar_type: 3 # Ouster LiDAR scan_line: 64 blind: 2 feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 1.69 # TODO: Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 350 # To determine how much data is used x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : 0.0 trans_IL_y : 0.0 trans_IL_z : 0.0 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 13.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 0.5 filter_size_map: 0.5 gyr_cov: 0.5 acc_cov: 0.5 det_range: 100.0 ground_cov : 100.0 publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # Taeyoung 추가 trajectory_save: traj_save_en: false traj_file_path: "/root/catkin_ws/src/result/traj.txt" # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 1.60 # TODO: Height from ground to LiDAR sensor (meter) mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 5 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 5.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/s3e/velodyne_s3e_bob_lab01.yaml ================================================ # rosbag play -s 65 -u 145 S3E_Laboratory_1.bag common: lid_topic: "/Bob/velodyne_points" imu_topic: "/Bob/imu/data" # handsfree imu preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 16 # LiDAR scan line number blind: 0.5 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.7 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 200 # To determine how much data is used x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.1 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : 0.1 trans_IL_y : 0.2 trans_IL_z : 0.5 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 15.0 acc_factor : 1.3 ground_factor : 3.0 mapping: filter_size_surf: 0.5 filter_size_map: 0.5 gyr_cov: 0.5 acc_cov: 0.5 det_range: 200.0 # LiDAR range ground_cov : 10.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: false traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.8 # For S3E dataset : https://github.com/PengYu-Team/S3E mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/s3e/velodyne_s3e_bob_lab02.yaml ================================================ # rosbag play -s 220 -u 120 S3E_Laboratory_2.bag common: lid_topic: "/Bob/velodyne_points" imu_topic: "/Bob/imu/data" # handsfree imu preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 16 # LiDAR scan line number blind: 0.2 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 2 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.7 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 200 # To determine how much data is used x_accumulate : 0.001 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.001 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.17 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : 0.1 trans_IL_y : 0.2 trans_IL_z : 0.5 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 20.0 acc_factor : 0.5 ground_factor : 10.0 mapping: filter_size_surf: 0.1 filter_size_map: 0.1 gyr_cov: 0.5 acc_cov: 0.5 det_range: 200.0 # LiDAR range ground_cov : 100.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: false traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.8 # For S3E dataset : https://github.com/PengYu-Team/S3E mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: config/velodyne16.yaml ================================================ common: lid_topic: "/velodyne_points" imu_topic: "/imu/data" preprocess: lidar_type: 2 # Velodyne LiDAR scan_line: 32 # LiDAR scan line number blind: 0.5 # blind area in the beginning of each scan line feature_extract_en: false calibration: cut_frame: true # false: do not cut input pointcloud cut_frame_num: 3 # must be positive integer orig_odom_freq: 10 # original Lidar frequency (for velodyne, it is 10Hz) mean_acc_norm: 9.81 # 1: for livox built-in IMU imu_sensor_height : 0.1 # Height from ground to IMU sensor (meter) verbose : true # For debug, Rotation matrix related ground, lidar sensor height # For Data Accumulation data_accum_length: 300 # To determine how much data is used x_accumulate : 0.01 # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999) y_accumulate : 0.01 # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999) z_accumulate : 0.99 # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999) # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m trans_IL_x : 0.0 trans_IL_y : 0.0 trans_IL_z : 0.0 set_boundary : false bound_th : 0.3 # Weight for residual gyro_factor : 10.0 acc_factor : 1.0 ground_factor : 5.0 mapping: filter_size_surf: 0.5 filter_size_map: 0.5 gyr_cov: 0.5 acc_cov: 0.5 det_range: 200.0 # LiDAR range ground_cov : 100.0 publish: path_en: true # true: output the path of the robot scan_publish_en: true # false: close all the point cloud output dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame # For LiDAR Map Saving pcd_save: pcd_save_en: false interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. # For LiDAR odometry Saving trajectory_save: traj_save_en: true traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path. # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus patchworkpp: sensor_height: 0.85 # TODO: change this value to your sensor height mode: "czm" verbose: false # To check effect of uprightness/elevation/flatness visualize: true # Ground Likelihood Estimation is visualized # Ground Plane Fitting parameters num_iter: 3 # Number of iterations for ground plane estimation using PCA. num_lpr: 20 # Maximum number of points to be selected as lowest points representative. num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. th_dist: 0.125 # threshold for thickenss of ground. th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. th_dist_v: 0.1 # threshold for thickenss of vertical structure. max_r: 4.0 # max_range of ground estimation area min_r: 1.0 # min_range of ground estimation area uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered czm: num_zones: 4 num_sectors_each_zone: [16, 32, 54, 32] mum_rings_each_zone: [2, 4, 4, 4] elevation_thresholds: [0.1, 0.2, 0.4, 0.6] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. enable_RNR : true enable_RVPF : true enable_TGR : true RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. ================================================ FILE: docker/Dockerfile ================================================ # Author: Tae Young Kim # email: tyoung96@yonsei.ac.kr FROM ros:melodic # Install PCL & Eigen & essential libraries RUN apt-get update && apt-get install -y cmake libeigen3-dev libpcl-dev \ ros-melodic-rviz ros-melodic-pcl-ros ros-melodic-eigen-conversions \ libatlas-base-dev libgoogle-glog-dev libsuitesparse-dev libglew-dev wget ros-melodic-image-transport-plugins # Install jsk-visualization RUN apt-get update && apt-get install -y ros-melodic-jsk-recognition ros-melodic-jsk-common-msgs ros-melodic-jsk-rviz-plugins # Install matplotlib RUN apt-get update && apt-get install -y python-pip python-tk && pip install matplotlib # Install ceres-solver WORKDIR /root/thirdParty RUN wget https://github.com/ceres-solver/ceres-solver/archive/refs/tags/2.0.0.tar.gz RUN tar zxf 2.0.0.tar.gz RUN cd ceres-solver-2.0.0 RUN mkdir build && cd build RUN ls RUN cmake -DCMAKE_BUILD_TYPE=Release ./ceres-solver-2.0.0 && make -j2 && make install # Install livox driver WORKDIR /root/livox_ws/src RUN wget https://github.com/Livox-SDK/livox_ros_driver/archive/refs/tags/v2.6.0.tar.gz RUN tar zxf v2.6.0.tar.gz && rm -rf v2.6.0.tar.gz RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' WORKDIR /root/livox_ws RUN /bin/bash -c 'source /opt/ros/melodic/setup.bash && catkin_make' RUN /bin/bash -c 'source /root/livox_ws/devel/setup.bash' WORKDIR /root/catkin_ws/ # Load ROS environment at each run COPY ./ros_entrypoint.sh / RUN chmod 755 /ros_entrypoint.sh ENTRYPOINT ["/ros_entrypoint.sh"] CMD ["bash"] ================================================ FILE: docker/container_run.sh ================================================ #!/bin/bash # Author : Taeyoung Kim (https://github.com/Taeyoung96) # Set the project directory (PROJECT_DIR) as the parent directory of the current working directory PROJECT_DIR=$(dirname "$PWD") # Move to the parent folder of the project directory cd "$PROJECT_DIR" # Check if arguments are provided for the image name and tag if [ "$#" -ne 2 ]; then echo "[Error] Usage: $0 " exit 1 fi # Print the current working directory to verify the change echo "Current working directory: $PROJECT_DIR" xhost +local:docker # Assign the arguments to variables for clarity CONTAINER_NAME="$1" IMAGE_NAME="$2" # Launch the nvidia-docker container with the provided image name and tag docker run --privileged -it \ --gpus all \ -e NVIDIA_DRIVER_CAPABILITIES=all \ -e NVIDIA_VISIBLE_DEVICES=all \ --volume="$PROJECT_DIR:/root/catkin_ws/src" \ --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw \ --net=host \ --ipc=host \ --shm-size=2gb \ --name="$CONTAINER_NAME" \ --env="DISPLAY=$DISPLAY" \ "$IMAGE_NAME" /bin/bash ================================================ FILE: docker/ros_entrypoint.sh ================================================ #!/bin/bash set -e # Ros build source "/opt/ros/melodic/setup.bash" source "/root/livox_ws/devel/setup.bash" # Libray install if you want echo "================Gril-Calib Docker Env Ready================" cd /root/catkin_ws exec "$@" ================================================ FILE: include/Fusion/CMakeLists.txt ================================================ file(GLOB_RECURSE files "*.c") add_library(Fusion ${files}) if(UNIX AND NOT APPLE) target_link_libraries(Fusion m) # link math library for Linux endif() ================================================ FILE: include/Fusion/Fusion.h ================================================ /** * @file Fusion.h * @author Seb Madgwick * @brief Main header file for the Fusion library. This is the only file that * needs to be included when using the library. */ #ifndef FUSION_H #define FUSION_H //------------------------------------------------------------------------------ // Includes #ifdef __cplusplus extern "C" { #endif #include "FusionAhrs.h" #include "FusionAxes.h" #include "FusionCalibration.h" #include "FusionCompass.h" #include "FusionMath.h" #include "FusionOffset.h" #ifdef __cplusplus } #endif #endif //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionAhrs.c ================================================ /** * @file FusionAhrs.c * @author Seb Madgwick * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer * measurements into a single measurement of orientation relative to the Earth. */ //------------------------------------------------------------------------------ // Includes #include // FLT_MAX #include "FusionAhrs.h" #include "FusionCompass.h" #include // atan2f, cosf, powf, sinf //------------------------------------------------------------------------------ // Definitions /** * @brief Initial gain used during the initialisation. */ #define INITIAL_GAIN (10.0f) /** * @brief Initialisation period in seconds. */ #define INITIALISATION_PERIOD (3.0f) //------------------------------------------------------------------------------ // Functions /** * @brief Initialises the AHRS algorithm structure. * @param ahrs AHRS algorithm structure. */ void FusionAhrsInitialise(FusionAhrs *const ahrs) { const FusionAhrsSettings settings = { .gain = 0.5f, .accelerationRejection = 90.0f, .magneticRejection = 90.0f, .rejectionTimeout = 0, }; FusionAhrsSetSettings(ahrs, &settings); FusionAhrsReset(ahrs); } /** * @brief Resets the AHRS algorithm. This is equivalent to reinitialising the * algorithm while maintaining the current settings. * @param ahrs AHRS algorithm structure. */ void FusionAhrsReset(FusionAhrs *const ahrs) { ahrs->quaternion = FUSION_IDENTITY_QUATERNION; ahrs->accelerometer = FUSION_VECTOR_ZERO; ahrs->initialising = true; ahrs->rampedGain = INITIAL_GAIN; ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; ahrs->accelerometerIgnored = false; ahrs->accelerationRejectionTimer = 0; ahrs->accelerationRejectionTimeout = false; ahrs->magnetometerIgnored = false; ahrs->magneticRejectionTimer = 0; ahrs->magneticRejectionTimeout = false; } /** * @brief Sets the AHRS algorithm settings. * @param ahrs AHRS algorithm structure. * @param settings Settings. */ void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) { ahrs->settings.gain = settings->gain; if ((settings->accelerationRejection == 0.0f) || (settings->rejectionTimeout == 0)) { ahrs->settings.accelerationRejection = FLT_MAX; } else { ahrs->settings.accelerationRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); } if ((settings->magneticRejection == 0.0f) || (settings->rejectionTimeout == 0)) { ahrs->settings.magneticRejection = FLT_MAX; } else { ahrs->settings.magneticRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); } ahrs->settings.rejectionTimeout = settings->rejectionTimeout; if (ahrs->initialising == false) { ahrs->rampedGain = ahrs->settings.gain; } ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD; } /** * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and * magnetometer measurements. * @param ahrs AHRS algorithm structure. * @param gyroscope Gyroscope measurement in degrees per second. * @param accelerometer Accelerometer measurement in g. * @param magnetometer Magnetometer measurement in arbitrary units. * @param deltaTime Delta time in seconds. */ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) { #define Q ahrs->quaternion.element // Store accelerometer ahrs->accelerometer = accelerometer; // Ramp down gain during initialisation if (ahrs->initialising == true) { ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime; if (ahrs->rampedGain < ahrs->settings.gain) { ahrs->rampedGain = ahrs->settings.gain; ahrs->initialising = false; ahrs->accelerationRejectionTimeout = false; } } // Calculate direction of gravity indicated by algorithm const FusionVector halfGravity = { .axis.x = Q.x * Q.z - Q.w * Q.y, .axis.y = Q.y * Q.z + Q.w * Q.x, .axis.z = Q.w * Q.w - 0.5f + Q.z * Q.z, }; // third column of transposed rotation matrix scaled by 0.5 // Calculate accelerometer feedback FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; ahrs->accelerometerIgnored = true; if (FusionVectorIsZero(accelerometer) == false) { // Enter acceleration recovery state if acceleration rejection times out if (ahrs->accelerationRejectionTimer > ahrs->settings.rejectionTimeout) { const FusionQuaternion quaternion = ahrs->quaternion; FusionAhrsReset(ahrs); ahrs->quaternion = quaternion; ahrs->accelerationRejectionTimer = 0; ahrs->accelerationRejectionTimeout = true; } // Calculate accelerometer feedback scaled by 0.5 ahrs->halfAccelerometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(accelerometer), halfGravity); // Ignore accelerometer if acceleration distortion detected if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection)) { halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; ahrs->accelerometerIgnored = false; ahrs->accelerationRejectionTimer -= ahrs->accelerationRejectionTimer >= 10 ? 10 : 0; } else { ahrs->accelerationRejectionTimer++; } } // Calculate magnetometer feedback FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; ahrs->magnetometerIgnored = true; if (FusionVectorIsZero(magnetometer) == false) { // Set to compass heading if magnetic rejection times out ahrs->magneticRejectionTimeout = false; if (ahrs->magneticRejectionTimer > ahrs->settings.rejectionTimeout) { FusionAhrsSetHeading(ahrs, FusionCompassCalculateHeading(halfGravity, magnetometer)); ahrs->magneticRejectionTimer = 0; ahrs->magneticRejectionTimeout = true; } // Compute direction of west indicated by algorithm const FusionVector halfWest = { .axis.x = Q.x * Q.y + Q.w * Q.z, .axis.y = Q.w * Q.w - 0.5f + Q.y * Q.y, .axis.z = Q.y * Q.z - Q.w * Q.x }; // second column of transposed rotation matrix scaled by 0.5 // Calculate magnetometer feedback scaled by 0.5 ahrs->halfMagnetometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfWest); // Ignore magnetometer if magnetic distortion detected if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection)) { halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; ahrs->magnetometerIgnored = false; ahrs->magneticRejectionTimer -= ahrs->magneticRejectionTimer >= 10 ? 10 : 0; } else { ahrs->magneticRejectionTimer++; } } // Convert gyroscope to radians per second scaled by 0.5 const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f)); // Apply feedback to gyroscope const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain)); // Integrate rate of change of quaternion ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime))); // Normalise quaternion ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion); #undef Q } /** * @brief Updates the AHRS algorithm using the gyroscope and accelerometer * measurements only. * @param ahrs AHRS algorithm structure. * @param gyroscope Gyroscope measurement in degrees per second. * @param accelerometer Accelerometer measurement in g. * @param deltaTime Delta time in seconds. */ void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) { // Update AHRS algorithm FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); // Zero heading during initialisation if ((ahrs->initialising == true) && (ahrs->accelerationRejectionTimeout == false)) { FusionAhrsSetHeading(ahrs, 0.0f); } } /** * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and * heading measurements. * @param ahrs AHRS algorithm structure. * @param gyroscope Gyroscope measurement in degrees per second. * @param accelerometer Accelerometer measurement in g. * @param heading Heading measurement in degrees. * @param deltaTime Delta time in seconds. */ void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime) { #define Q ahrs->quaternion.element // Calculate roll const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x); // Calculate magnetometer const float headingRadians = FusionDegreesToRadians(heading); const float sinHeadingRadians = sinf(headingRadians); const FusionVector magnetometer = { .axis.x = cosf(headingRadians), .axis.y = -1.0f * cosf(roll) * sinHeadingRadians, .axis.z = sinHeadingRadians * sinf(roll), }; // Update AHRS algorithm FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime); #undef Q } /** * @brief Returns the quaternion describing the sensor relative to the Earth. * @param ahrs AHRS algorithm structure. * @return Quaternion describing the sensor relative to the Earth. */ FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { return ahrs->quaternion; } /** * @brief Returns the linear acceleration measurement equal to the accelerometer * measurement with the 1 g of gravity removed. * @param ahrs AHRS algorithm structure. * @return Linear acceleration measurement in g. */ FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element const FusionVector gravity = { .axis.x = 2.0f * (Q.x * Q.z - Q.w * Q.y), .axis.y = 2.0f * (Q.y * Q.z + Q.w * Q.x), .axis.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), }; // third column of transposed rotation matrix const FusionVector linearAcceleration = FusionVectorSubtract(ahrs->accelerometer, gravity); return linearAcceleration; #undef Q } /** * @brief Returns the Earth acceleration measurement equal to accelerometer * measurement in the Earth coordinate frame with the 1 g of gravity removed. * @param ahrs AHRS algorithm structure. * @return Earth acceleration measurement in g. */ FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element #define A ahrs->accelerometer.axis const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations const float qwqx = Q.w * Q.x; const float qwqy = Q.w * Q.y; const float qwqz = Q.w * Q.z; const float qxqy = Q.x * Q.y; const float qxqz = Q.x * Q.z; const float qyqz = Q.y * Q.z; const FusionVector earthAcceleration = { .axis.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z), .axis.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z), .axis.z = (2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z)) - 1.0f, }; // rotation matrix multiplied with the accelerometer, with 1 g subtracted return earthAcceleration; #undef Q #undef A } /** * @brief Returns the AHRS algorithm internal states. * @param ahrs AHRS algorithm structure. * @return AHRS algorithm internal states. */ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) { const FusionAhrsInternalStates internalStates = { .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), .accelerometerIgnored = ahrs->accelerometerIgnored, .accelerationRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->accelerationRejectionTimer / (float) ahrs->settings.rejectionTimeout, .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), .magnetometerIgnored = ahrs->magnetometerIgnored, .magneticRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->magneticRejectionTimer / (float) ahrs->settings.rejectionTimeout, }; return internalStates; } /** * @brief Returns the AHRS algorithm flags. * @param ahrs AHRS algorithm structure. * @return AHRS algorithm flags. */ FusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs) { const unsigned int warningTimeout = ahrs->settings.rejectionTimeout / 4; const FusionAhrsFlags flags = { .initialising = ahrs->initialising, .accelerationRejectionWarning = ahrs->accelerationRejectionTimer > warningTimeout, .accelerationRejectionTimeout = ahrs->accelerationRejectionTimeout, .magneticRejectionWarning = ahrs->magneticRejectionTimer > warningTimeout, .magneticRejectionTimeout = ahrs->magneticRejectionTimeout, }; return flags; } /** * @brief Sets the heading of the orientation measurement provided by the AHRS * algorithm. This function can be used to reset drift in heading when the AHRS * algorithm is being used without a magnetometer. * @param ahrs AHRS algorithm structure. * @param heading Heading angle in degrees. */ void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) { #define Q ahrs->quaternion.element const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z); const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading)); const FusionQuaternion rotation = { .element.w = cosf(halfYawMinusHeading), .element.x = 0.0f, .element.y = 0.0f, .element.z = -1.0f * sinf(halfYawMinusHeading), }; ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion); #undef Q } //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionAhrs.h ================================================ /** * @file FusionAhrs.h * @author Seb Madgwick * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer * measurements into a single measurement of orientation relative to the Earth. */ #ifndef FUSION_AHRS_H #define FUSION_AHRS_H //------------------------------------------------------------------------------ // Includes #include "FusionMath.h" #include //------------------------------------------------------------------------------ // Definitions /** * @brief AHRS algorithm settings. */ typedef struct { float gain; float accelerationRejection; float magneticRejection; unsigned int rejectionTimeout; } FusionAhrsSettings; /** * @brief AHRS algorithm structure. Structure members are used internally and * must not be accessed by the application. */ typedef struct { FusionAhrsSettings settings; FusionQuaternion quaternion; FusionVector accelerometer; bool initialising; float rampedGain; float rampedGainStep; FusionVector halfAccelerometerFeedback; FusionVector halfMagnetometerFeedback; bool accelerometerIgnored; unsigned int accelerationRejectionTimer; bool accelerationRejectionTimeout; bool magnetometerIgnored; unsigned int magneticRejectionTimer; bool magneticRejectionTimeout; } FusionAhrs; /** * @brief AHRS algorithm internal states. */ typedef struct { float accelerationError; bool accelerometerIgnored; float accelerationRejectionTimer; float magneticError; bool magnetometerIgnored; float magneticRejectionTimer; } FusionAhrsInternalStates; /** * @brief AHRS algorithm flags. */ typedef struct { bool initialising; bool accelerationRejectionWarning; bool accelerationRejectionTimeout; bool magneticRejectionWarning; bool magneticRejectionTimeout; } FusionAhrsFlags; //------------------------------------------------------------------------------ // Function declarations void FusionAhrsInitialise(FusionAhrs *const ahrs); void FusionAhrsReset(FusionAhrs *const ahrs); void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime); void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime); void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime); FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs); FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs); FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs); FusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs); void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading); #endif //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionAxes.h ================================================ /** * @file FusionAxes.h * @author Seb Madgwick * @brief Swaps sensor axes for alignment with the body axes. */ #ifndef FUSION_AXES_H #define FUSION_AXES_H //------------------------------------------------------------------------------ // Includes #include "FusionMath.h" //------------------------------------------------------------------------------ // Definitions /** * @brief Axes alignment describing the sensor axes relative to the body axes. * For example, if the body X axis is aligned with the sensor Y axis and the * body Y axis is aligned with sensor X axis but pointing the opposite direction * then alignment is +Y-X+Z. */ typedef enum { FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ } FusionAxesAlignment; //------------------------------------------------------------------------------ // Inline functions /** * @brief Swaps sensor axes for alignment with the body axes. * @param sensor Sensor axes. * @param alignment Axes alignment. * @return Sensor axes aligned with the body axes. */ static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) { FusionVector result; switch (alignment) { case FusionAxesAlignmentPXPYPZ: break; case FusionAxesAlignmentPXNZPY: result.axis.x = +sensor.axis.x; result.axis.y = -sensor.axis.z; result.axis.z = +sensor.axis.y; return result; case FusionAxesAlignmentPXNYNZ: result.axis.x = +sensor.axis.x; result.axis.y = -sensor.axis.y; result.axis.z = -sensor.axis.z; return result; case FusionAxesAlignmentPXPZNY: result.axis.x = +sensor.axis.x; result.axis.y = +sensor.axis.z; result.axis.z = -sensor.axis.y; return result; case FusionAxesAlignmentNXPYNZ: result.axis.x = -sensor.axis.x; result.axis.y = +sensor.axis.y; result.axis.z = -sensor.axis.z; return result; case FusionAxesAlignmentNXPZPY: result.axis.x = -sensor.axis.x; result.axis.y = +sensor.axis.z; result.axis.z = +sensor.axis.y; return result; case FusionAxesAlignmentNXNYPZ: result.axis.x = -sensor.axis.x; result.axis.y = -sensor.axis.y; result.axis.z = +sensor.axis.z; return result; case FusionAxesAlignmentNXNZNY: result.axis.x = -sensor.axis.x; result.axis.y = -sensor.axis.z; result.axis.z = -sensor.axis.y; return result; case FusionAxesAlignmentPYNXPZ: result.axis.x = +sensor.axis.y; result.axis.y = -sensor.axis.x; result.axis.z = +sensor.axis.z; return result; case FusionAxesAlignmentPYNZNX: result.axis.x = +sensor.axis.y; result.axis.y = -sensor.axis.z; result.axis.z = -sensor.axis.x; return result; case FusionAxesAlignmentPYPXNZ: result.axis.x = +sensor.axis.y; result.axis.y = +sensor.axis.x; result.axis.z = -sensor.axis.z; return result; case FusionAxesAlignmentPYPZPX: result.axis.x = +sensor.axis.y; result.axis.y = +sensor.axis.z; result.axis.z = +sensor.axis.x; return result; case FusionAxesAlignmentNYPXPZ: result.axis.x = -sensor.axis.y; result.axis.y = +sensor.axis.x; result.axis.z = +sensor.axis.z; return result; case FusionAxesAlignmentNYNZPX: result.axis.x = -sensor.axis.y; result.axis.y = -sensor.axis.z; result.axis.z = +sensor.axis.x; return result; case FusionAxesAlignmentNYNXNZ: result.axis.x = -sensor.axis.y; result.axis.y = -sensor.axis.x; result.axis.z = -sensor.axis.z; return result; case FusionAxesAlignmentNYPZNX: result.axis.x = -sensor.axis.y; result.axis.y = +sensor.axis.z; result.axis.z = -sensor.axis.x; return result; case FusionAxesAlignmentPZPYNX: result.axis.x = +sensor.axis.z; result.axis.y = +sensor.axis.y; result.axis.z = -sensor.axis.x; return result; case FusionAxesAlignmentPZPXPY: result.axis.x = +sensor.axis.z; result.axis.y = +sensor.axis.x; result.axis.z = +sensor.axis.y; return result; case FusionAxesAlignmentPZNYPX: result.axis.x = +sensor.axis.z; result.axis.y = -sensor.axis.y; result.axis.z = +sensor.axis.x; return result; case FusionAxesAlignmentPZNXNY: result.axis.x = +sensor.axis.z; result.axis.y = -sensor.axis.x; result.axis.z = -sensor.axis.y; return result; case FusionAxesAlignmentNZPYPX: result.axis.x = -sensor.axis.z; result.axis.y = +sensor.axis.y; result.axis.z = +sensor.axis.x; return result; case FusionAxesAlignmentNZNXPY: result.axis.x = -sensor.axis.z; result.axis.y = -sensor.axis.x; result.axis.z = +sensor.axis.y; return result; case FusionAxesAlignmentNZNYNX: result.axis.x = -sensor.axis.z; result.axis.y = -sensor.axis.y; result.axis.z = -sensor.axis.x; return result; case FusionAxesAlignmentNZPXNY: result.axis.x = -sensor.axis.z; result.axis.y = +sensor.axis.x; result.axis.z = -sensor.axis.y; return result; } return sensor; } #endif //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionCalibration.h ================================================ /** * @file FusionCalibration.h * @author Seb Madgwick * @brief Gyroscope, accelerometer, and magnetometer calibration models. */ #ifndef FUSION_CALIBRATION_H #define FUSION_CALIBRATION_H //------------------------------------------------------------------------------ // Includes #include "FusionMath.h" //------------------------------------------------------------------------------ // Inline functions /** * @brief Gyroscope and accelerometer calibration model. * @param uncalibrated Uncalibrated measurement. * @param misalignment Misalignment matrix. * @param sensitivity Sensitivity. * @param offset Offset. * @return Calibrated measurement. */ static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) { return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); } /** * @brief Magnetometer calibration model. * @param uncalibrated Uncalibrated measurement. * @param softIronMatrix Soft-iron matrix. * @param hardIronOffset Hard-iron offset. * @return Calibrated measurement. */ static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) { return FusionVectorSubtract(FusionMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronOffset); } #endif //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionCompass.c ================================================ /** * @file FusionCompass.c * @author Seb Madgwick * @brief Tilt-compensated compass to calculate an heading relative to magnetic * north using accelerometer and magnetometer measurements. */ //------------------------------------------------------------------------------ // Includes #include "FusionCompass.h" #include // atan2f //------------------------------------------------------------------------------ // Functions /** * @brief Calculates the heading relative to magnetic north. * @param accelerometer Accelerometer measurement in any calibrated units. * @param magnetometer Magnetometer measurement in any calibrated units. * @return Heading angle in degrees. */ float FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer) { // Compute direction of magnetic west (Earth's y axis) const FusionVector magneticWest = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); // Compute direction of magnetic north (Earth's x axis) const FusionVector magneticNorth = FusionVectorNormalise(FusionVectorCrossProduct(magneticWest, accelerometer)); // Calculate angular heading relative to magnetic north return FusionRadiansToDegrees(atan2f(magneticWest.axis.x, magneticNorth.axis.x)); } //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionCompass.h ================================================ /** * @file FusionCompass.h * @author Seb Madgwick * @brief Tilt-compensated compass to calculate an heading relative to magnetic * north using accelerometer and magnetometer measurements. */ #ifndef FUSION_COMPASS_H #define FUSION_COMPASS_H //------------------------------------------------------------------------------ // Includes #include "FusionMath.h" //------------------------------------------------------------------------------ // Function declarations float FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer); #endif //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionMath.h ================================================ /** * @file FusionMath.h * @author Seb Madgwick * @brief Math library. */ #ifndef FUSION_MATH_H #define FUSION_MATH_H //------------------------------------------------------------------------------ // Includes #include // M_PI, sqrtf, atan2f, asinf #include #include //------------------------------------------------------------------------------ // Definitions /** * @brief 3D vector. */ typedef union { float array[3]; struct { float x; float y; float z; } axis; } FusionVector; /** * @brief Quaternion. */ typedef union { float array[4]; struct { float w; float x; float y; float z; } element; } FusionQuaternion; /** * @brief 3x3 matrix in row-major order. * See http://en.wikipedia.org/wiki/Row-major_order */ typedef union { float array[3][3]; struct { float xx; float xy; float xz; float yx; float yy; float yz; float zx; float zy; float zz; } element; } FusionMatrix; /** * @brief Euler angles. Roll, pitch, and yaw correspond to rotations around * X, Y, and Z respectively. */ typedef union { float array[3]; struct { float roll; float pitch; float yaw; } angle; } FusionEuler; /** * @brief Vector of zeros. */ #define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} }) /** * @brief Vector of ones. */ #define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} }) /** * @brief Identity quaternion. */ #define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} }) /** * @brief Identity matrix. */ #define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} }) /** * @brief Euler angles of zero. */ #define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} }) /** * @brief Pi. May not be defined in math.h. */ #ifndef M_PI #define M_PI (3.14159265358979323846) #endif /** * @brief Include this definition or add as a preprocessor definition to use * normal square root operations. */ //#define FUSION_USE_NORMAL_SQRT //------------------------------------------------------------------------------ // Inline functions - Degrees and radians conversion /** * @brief Converts degrees to radians. * @param degrees Degrees. * @return Radians. */ static inline float FusionDegreesToRadians(const float degrees) { return degrees * ((float) M_PI / 180.0f); } /** * @brief Converts radians to degrees. * @param radians Radians. * @return Degrees. */ static inline float FusionRadiansToDegrees(const float radians) { return radians * (180.0f / (float) M_PI); } //------------------------------------------------------------------------------ // Inline functions - Arc sine /** * @brief Returns the arc sine of the value. * @param value Value. * @return Arc sine of the value. */ static inline float FusionAsin(const float value) { if (value <= -1.0f) { return (float) M_PI / -2.0f; } if (value >= 1.0f) { return (float) M_PI / 2.0f; } return asinf(value); } //------------------------------------------------------------------------------ // Inline functions - Fast inverse square root #ifndef FUSION_USE_NORMAL_SQRT /** * @brief Calculates the reciprocal of the square root. * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/ * @param x Operand. * @return Reciprocal of the square root of x. */ static inline float FusionFastInverseSqrt(const float x) { typedef union { float f; int32_t i; } Union32; Union32 union32 = {.f = x}; union32.i = 0x5F1F1412 - (union32.i >> 1); return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); } #endif //------------------------------------------------------------------------------ // Inline functions - Vector operations /** * @brief Returns true if the vector is zero. * @param vector Vector. * @return True if the vector is zero. */ static inline bool FusionVectorIsZero(const FusionVector vector) { return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); } /** * @brief Returns the sum of two vectors. * @param vectorA Vector A. * @param vectorB Vector B. * @return Sum of two vectors. */ static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) { FusionVector result; result.axis.x = vectorA.axis.x + vectorB.axis.x; result.axis.y = vectorA.axis.y + vectorB.axis.y; result.axis.z = vectorA.axis.z + vectorB.axis.z; return result; } /** * @brief Returns vector B subtracted from vector A. * @param vectorA Vector A. * @param vectorB Vector B. * @return Vector B subtracted from vector A. */ static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) { FusionVector result; result.axis.x = vectorA.axis.x - vectorB.axis.x; result.axis.y = vectorA.axis.y - vectorB.axis.y; result.axis.z = vectorA.axis.z - vectorB.axis.z; return result; } /** * @brief Returns the sum of the elements. * @param vector Vector. * @return Sum of the elements. */ static inline float FusionVectorSum(const FusionVector vector) { return vector.axis.x + vector.axis.y + vector.axis.z; } /** * @brief Returns the multiplication of a vector by a scalar. * @param vector Vector. * @param scalar Scalar. * @return Multiplication of a vector by a scalar. */ static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) { FusionVector result; result.axis.x = vector.axis.x * scalar; result.axis.y = vector.axis.y * scalar; result.axis.z = vector.axis.z * scalar; return result; } /** * @brief Calculates the Hadamard product (element-wise multiplication). * @param vectorA Vector A. * @param vectorB Vector B. * @return Hadamard product. */ static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) { FusionVector result; result.axis.x = vectorA.axis.x * vectorB.axis.x; result.axis.y = vectorA.axis.y * vectorB.axis.y; result.axis.z = vectorA.axis.z * vectorB.axis.z; return result; } /** * @brief Returns the cross product. * @param vectorA Vector A. * @param vectorB Vector B. * @return Cross product. */ static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) { #define A vectorA.axis #define B vectorB.axis FusionVector result; result.axis.x = A.y * B.z - A.z * B.y; result.axis.y = A.z * B.x - A.x * B.z; result.axis.z = A.x * B.y - A.y * B.x; return result; #undef A #undef B } /** * @brief Returns the vector magnitude squared. * @param vector Vector. * @return Vector magnitude squared. */ static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); } /** * @brief Returns the vector magnitude. * @param vector Vector. * @return Vector magnitude. */ static inline float FusionVectorMagnitude(const FusionVector vector) { return sqrtf(FusionVectorMagnitudeSquared(vector)); } /** * @brief Returns the normalised vector. * @param vector Vector. * @return Normalised vector. */ static inline FusionVector FusionVectorNormalise(const FusionVector vector) { #ifdef FUSION_USE_NORMAL_SQRT const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); #else const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); #endif return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); } //------------------------------------------------------------------------------ // Inline functions - Quaternion operations /** * @brief Returns the sum of two quaternions. * @param quaternionA Quaternion A. * @param quaternionB Quaternion B. * @return Sum of two quaternions. */ static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { FusionQuaternion result; result.element.w = quaternionA.element.w + quaternionB.element.w; result.element.x = quaternionA.element.x + quaternionB.element.x; result.element.y = quaternionA.element.y + quaternionB.element.y; result.element.z = quaternionA.element.z + quaternionB.element.z; return result; } /** * @brief Returns the multiplication of two quaternions. * @param quaternionA Quaternion A (to be post-multiplied). * @param quaternionB Quaternion B (to be pre-multiplied). * @return Multiplication of two quaternions. */ static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { #define A quaternionA.element #define B quaternionB.element FusionQuaternion result; result.element.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z; result.element.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y; result.element.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x; result.element.z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w; return result; #undef A #undef B } /** * @brief Returns the multiplication of a quaternion with a vector. This is a * normal quaternion multiplication where the vector is treated a * quaternion with a W element value of zero. The quaternion is post- * multiplied by the vector. * @param quaternion Quaternion. * @param vector Vector. * @return Multiplication of a quaternion with a vector. */ static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) { #define Q quaternion.element #define V vector.axis FusionQuaternion result; result.element.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z; result.element.x = Q.w * V.x + Q.y * V.z - Q.z * V.y; result.element.y = Q.w * V.y - Q.x * V.z + Q.z * V.x; result.element.z = Q.w * V.z + Q.x * V.y - Q.y * V.x; return result; #undef Q #undef V } /** * @brief Returns the normalised quaternion. * @param quaternion Quaternion. * @return Normalised quaternion. */ static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) { #define Q quaternion.element #ifdef FUSION_USE_NORMAL_SQRT const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); #else const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); #endif FusionQuaternion normalisedQuaternion; normalisedQuaternion.element.w = Q.w * magnitudeReciprocal; normalisedQuaternion.element.x = Q.x * magnitudeReciprocal; normalisedQuaternion.element.y = Q.y * magnitudeReciprocal; normalisedQuaternion.element.z = Q.z * magnitudeReciprocal; return normalisedQuaternion; #undef Q } //------------------------------------------------------------------------------ // Inline functions - Matrix operations /** * @brief Returns the multiplication of a matrix with a vector. * @param matrix Matrix. * @param vector Vector. * @return Multiplication of a matrix with a vector. */ static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) { #define R matrix.element FusionVector result; result.axis.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z; result.axis.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z; result.axis.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z; return result; #undef R } //------------------------------------------------------------------------------ // Inline functions - Conversion operations /** * @brief Converts a quaternion to a rotation matrix. * @param quaternion Quaternion. * @return Rotation matrix. */ static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) { #define Q quaternion.element const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations const float qwqx = Q.w * Q.x; const float qwqy = Q.w * Q.y; const float qwqz = Q.w * Q.z; const float qxqy = Q.x * Q.y; const float qxqz = Q.x * Q.z; const float qyqz = Q.y * Q.z; FusionMatrix matrix; matrix.element.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x); matrix.element.xy = 2.0f * (qxqy - qwqz); matrix.element.xz = 2.0f * (qxqz + qwqy); matrix.element.yx = 2.0f * (qxqy + qwqz); matrix.element.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y); matrix.element.yz = 2.0f * (qyqz - qwqx); matrix.element.zx = 2.0f * (qxqz - qwqy); matrix.element.zy = 2.0f * (qyqz + qwqx); matrix.element.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z); return matrix; #undef Q } /** * @brief Converts a quaternion to ZYX Euler angles in degrees. * @param quaternion Quaternion. * @return Euler angles in degrees. */ static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) { #define Q quaternion.element const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations FusionEuler euler; euler.angle.roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)); euler.angle.pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))); euler.angle.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)); return euler; #undef Q } #endif //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionOffset.c ================================================ /** * @file FusionOffset.c * @author Seb Madgwick * @brief Gyroscope offset correction algorithm for run-time calibration of the * gyroscope offset. */ //------------------------------------------------------------------------------ // Includes #include "FusionOffset.h" #include // fabs //------------------------------------------------------------------------------ // Definitions /** * @brief Cutoff frequency in Hz. */ #define CUTOFF_FREQUENCY (0.02f) /** * @brief Timeout in seconds. */ #define TIMEOUT (5) /** * @brief Threshold in degrees per second. */ #define THRESHOLD (3.0f) //------------------------------------------------------------------------------ // Functions /** * @brief Initialises the gyroscope offset algorithm. * @param offset Gyroscope offset algorithm structure. * @param sampleRate Sample rate in Hz. */ void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) { offset->filterCoefficient = 2.0f * (float) M_PI * CUTOFF_FREQUENCY * (1.0f / (float) sampleRate); offset->timeout = TIMEOUT * sampleRate; offset->timer = 0; offset->gyroscopeOffset = FUSION_VECTOR_ZERO; } /** * @brief Updates the gyroscope offset algorithm and returns the corrected * gyroscope measurement. * @param offset Gyroscope offset algorithm structure. * @param gyroscope Gyroscope measurement in degrees per second. * @return Corrected gyroscope measurement in degrees per second. */ FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) { // Subtract offset from gyroscope measurement gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset); // Reset timer if gyroscope not stationary if ((fabs(gyroscope.axis.x) > THRESHOLD) || (fabs(gyroscope.axis.y) > THRESHOLD) || (fabs(gyroscope.axis.z) > THRESHOLD)) { offset->timer = 0; return gyroscope; } // Increment timer while gyroscope stationary if (offset->timer < offset->timeout) { offset->timer++; return gyroscope; } // Adjust offset if timer has elapsed offset->gyroscopeOffset = FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient)); return gyroscope; } //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Fusion/FusionOffset.h ================================================ /** * @file FusionOffset.h * @author Seb Madgwick * @brief Gyroscope offset correction algorithm for run-time calibration of the * gyroscope offset. */ #ifndef FUSION_OFFSET_H #define FUSION_OFFSET_H //------------------------------------------------------------------------------ // Includes #include "FusionMath.h" //------------------------------------------------------------------------------ // Definitions /** * @brief Gyroscope offset algorithm structure. Structure members are used * internally and must not be accessed by the application. */ typedef struct { float filterCoefficient; unsigned int timeout; unsigned int timer; FusionVector gyroscopeOffset; } FusionOffset; //------------------------------------------------------------------------------ // Function declarations void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate); FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope); #endif //------------------------------------------------------------------------------ // End of file ================================================ FILE: include/Gril_Calib/Gril_Calib.cpp ================================================ #include "Gril_Calib.h" /* Description: Gril-Calib (Heavily adapted from LI-Init by Fangcheng Zhu) Modifier : Taeyoung Kim (https://github.com/Taeyoung96) */ Gril_Calib::Gril_Calib() : time_delay_IMU_wtr_Lidar(0.0), time_lag_1(0.0), time_lag_2(0.0), lag_IMU_wtr_Lidar(0) { fout_LiDAR_meas.open(FILE_DIR("LiDAR_meas.txt"), ios::out); fout_IMU_meas.open(FILE_DIR("IMU_meas.txt"), ios::out); fout_before_filt_IMU.open(FILE_DIR("IMU_before_filter.txt"), ios::out); fout_before_filt_Lidar.open(FILE_DIR("Lidar_before_filter.txt"), ios::out); fout_acc_cost.open(FILE_DIR("acc_cost.txt"), ios::out); fout_after_rot.open(FILE_DIR("Lidar_omg_after_rot.txt"), ios::out); fout_LiDAR_ang_vel.open(FILE_DIR("Lidar_ang_vel.txt"), ios::out); fout_IMU_ang_vel.open(FILE_DIR("IMU_ang_vel.txt"), ios::out); fout_Jacob_trans.open(FILE_DIR("Jacob_trans.txt"), ios::out); fout_LiDAR_meas_after.open(FILE_DIR("LiDAR_meas_after.txt"), ios::out); data_accum_length = 300; trans_IL_x = 0.0; trans_IL_y = 0.0; trans_IL_z = 0.0; bound_th = 0.1; set_boundary = false; Rot_Grav_wrt_Init_Lidar = Eye3d; Trans_Lidar_wrt_IMU = Zero3d; Rot_Lidar_wrt_IMU = Eye3d; gyro_bias = Zero3d; acc_bias = Zero3d; } Gril_Calib::~Gril_Calib() = default; void Gril_Calib::set_IMU_state(const deque &IMU_states) { IMU_state_group.assign(IMU_states.begin(), IMU_states.end() - 1); } void Gril_Calib::set_Lidar_state(const deque &Lidar_states) { Lidar_state_group.assign(Lidar_states.begin(), Lidar_states.end() - 1); } void Gril_Calib::set_states_2nd_filter(const deque &IMU_states, const deque &Lidar_states) { for (int i = 0; i < IMU_state_group.size(); i++) { IMU_state_group[i].ang_acc = IMU_states[i].ang_acc; Lidar_state_group[i].ang_acc = Lidar_states[i].ang_acc; Lidar_state_group[i].linear_acc = Lidar_states[i].linear_acc; } } void Gril_Calib::fout_before_filter() { for (auto it_IMU = IMU_state_group.begin(); it_IMU != IMU_state_group.end() - 1; it_IMU++) { fout_before_filt_IMU << setprecision(15) << it_IMU->ang_vel.transpose() << " " << it_IMU->ang_vel.norm() << " " << it_IMU->linear_acc.transpose() << " " << it_IMU->timeStamp << endl; } for (auto it = Lidar_state_group.begin(); it != Lidar_state_group.end() - 1; it++) { fout_before_filt_Lidar << setprecision(15) << it->ang_vel.transpose() << " " << it->ang_vel.norm() << " " << it->timeStamp << endl; } } void Gril_Calib::fout_check_lidar() { auto it_Lidar_state = Lidar_state_group.begin() + 1; for (; it_Lidar_state != Lidar_state_group.end() - 2; it_Lidar_state++) { fout_LiDAR_meas_after << setprecision(12) << it_Lidar_state->ang_vel.transpose() << " " << it_Lidar_state->ang_vel.norm() << " " << it_Lidar_state->linear_acc.transpose() << " " << it_Lidar_state->ang_acc.transpose() << " " << it_Lidar_state->timeStamp << endl; } } void Gril_Calib::push_ALL_IMU_CalibState(const sensor_msgs::Imu::ConstPtr &msg, const double &mean_acc_norm) { CalibState IMUstate; double invert = -1.0; IMUstate.ang_vel = V3D(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); IMUstate.linear_acc = V3D(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z) / mean_acc_norm * G_m_s2; IMUstate.timeStamp = msg->header.stamp.toSec(); IMU_state_group_ALL.push_back(IMUstate); } void Gril_Calib::push_IMU_CalibState(const V3D &omg, const V3D &acc, const double ×tamp) { CalibState IMUstate; IMUstate.ang_vel = omg; IMUstate.linear_acc = acc; IMUstate.timeStamp = timestamp; IMU_state_group.push_back(IMUstate); } void Gril_Calib::push_Lidar_CalibState(const M3D &rot, const V3D &pos, const V3D &omg, const V3D &linear_vel, const double ×tamp) { CalibState Lidarstate; Lidarstate.rot_end = rot; Lidarstate.pos_end = pos; Lidarstate.ang_vel = omg; Lidarstate.linear_vel = linear_vel; Lidarstate.timeStamp = timestamp; Lidar_state_group.push_back(Lidarstate); } void Gril_Calib::push_Plane_Constraint(const Eigen::Quaterniond &q_lidar, const Eigen::Quaterniond &q_imu, const V3D &normal_lidar, const double &distance_lidar) { Lidar_wrt_ground_group.push_back(q_lidar); IMU_wrt_ground_group.push_back(q_imu); normal_vector_wrt_lidar_group.push_back(normal_lidar); distance_Lidar_wrt_ground_group.push_back(distance_lidar); } void Gril_Calib::downsample_interpolate_IMU(const double &move_start_time) { while (IMU_state_group_ALL.front().timeStamp < move_start_time - 3.0) IMU_state_group_ALL.pop_front(); while (Lidar_state_group.front().timeStamp < move_start_time - 3.0) Lidar_state_group.pop_front(); // Original IMU measurements deque IMU_states_all_origin; IMU_states_all_origin.assign(IMU_state_group_ALL.begin(), IMU_state_group_ALL.end() - 1); // Mean filter to attenuate noise int mean_filt_size = 3; for (int i = mean_filt_size; i < IMU_state_group_ALL.size() - mean_filt_size; i++) { V3D acc_real = Zero3d; for (int k = -mean_filt_size; k < mean_filt_size + 1; k++) acc_real += (IMU_states_all_origin[i + k].linear_acc - acc_real) / (k + mean_filt_size + 1); IMU_state_group_ALL[i].linear_acc = acc_real; } // Down-sample and interpolation，Fig.4 in the paper for (int i = 0; i < Lidar_state_group.size(); i++) { for (int j = 1; j < IMU_state_group_ALL.size(); j++) { if (IMU_state_group_ALL[j - 1].timeStamp <= Lidar_state_group[i].timeStamp && IMU_state_group_ALL[j].timeStamp > Lidar_state_group[i].timeStamp) { CalibState IMU_state_interpolation; double delta_t = IMU_state_group_ALL[j].timeStamp - IMU_state_group_ALL[j - 1].timeStamp; double delta_t_right = IMU_state_group_ALL[j].timeStamp - Lidar_state_group[i].timeStamp; double s = delta_t_right / delta_t; IMU_state_interpolation.ang_vel = s * IMU_state_group_ALL[j - 1].ang_vel + (1 - s) * IMU_state_group_ALL[j].ang_vel; IMU_state_interpolation.linear_acc = s * IMU_state_group_ALL[j - 1].linear_acc + (1 - s) * IMU_state_group_ALL[j].linear_acc; push_IMU_CalibState(IMU_state_interpolation.ang_vel, IMU_state_interpolation.linear_acc, Lidar_state_group[i].timeStamp); break; } } } } // Calculates the angular acceleration of the IMU, LiDAR, and linear acceleration. void Gril_Calib::central_diff() { auto it_IMU_state = IMU_state_group.begin() + 1; for (; it_IMU_state != IMU_state_group.end() - 2; it_IMU_state++) { auto last_imu = it_IMU_state - 1; auto next_imu = it_IMU_state + 1; double dt_imu = next_imu->timeStamp - last_imu->timeStamp; it_IMU_state->ang_acc = (next_imu->ang_vel - last_imu->ang_vel) / dt_imu; fout_IMU_meas << setprecision(12) << it_IMU_state->ang_vel.transpose() << " " << it_IMU_state->ang_vel.norm() << " " << it_IMU_state->linear_acc.transpose() << " " << it_IMU_state->ang_acc.transpose() << " " << it_IMU_state->timeStamp << endl; } auto it_Lidar_state = Lidar_state_group.begin() + 1; for (; it_Lidar_state != Lidar_state_group.end() - 2; it_Lidar_state++) { auto last_lidar = it_Lidar_state - 1; auto next_lidar = it_Lidar_state + 1; double dt_lidar = next_lidar->timeStamp - last_lidar->timeStamp; it_Lidar_state->ang_acc = (next_lidar->ang_vel - last_lidar->ang_vel) / dt_lidar; it_Lidar_state->linear_acc = (next_lidar->linear_vel - last_lidar->linear_vel) / dt_lidar; fout_LiDAR_meas << setprecision(12) << it_Lidar_state->ang_vel.transpose() << " " << it_Lidar_state->ang_vel.norm() << " " << (it_Lidar_state->linear_acc - STD_GRAV).transpose() << " " << it_Lidar_state->ang_acc.transpose() << " " << it_Lidar_state->timeStamp << endl; } } // Temporal calibration by Cross-Correlation : calculate time_lag_1 void Gril_Calib::xcorr_temporal_init(const double &odom_freq) { int N = IMU_state_group.size(); //Calculate mean value of IMU and LiDAR angular velocity double mean_IMU_ang_vel = 0, mean_LiDAR_ang_vel = 0; for (int i = 0; i < N; i++) { mean_IMU_ang_vel += (IMU_state_group[i].ang_vel.norm() - mean_IMU_ang_vel) / (i + 1); mean_LiDAR_ang_vel += (Lidar_state_group[i].ang_vel.norm() - mean_LiDAR_ang_vel) / (i + 1); } //Calculate zero-centered cross correlation double max_corr = -DBL_MAX; for (int lag = -N + 1; lag < N; lag++) { double corr = 0; int cnt = 0; for (int i = 0; i < N; i++) { int j = i + lag; if (j < 0 || j > N - 1) continue; else { cnt++; corr += (IMU_state_group[i].ang_vel.norm() - mean_IMU_ang_vel) * (Lidar_state_group[j].ang_vel.norm() - mean_LiDAR_ang_vel); // Zero-centered cross correlation } } if (corr > max_corr) { max_corr = corr; lag_IMU_wtr_Lidar = -lag; } } time_lag_1 = lag_IMU_wtr_Lidar / odom_freq; cout << "Max Cross-correlation: IMU lag wtr Lidar : " << -lag_IMU_wtr_Lidar << endl; cout << "Time lag 1: IMU lag wtr Lidar : " << time_lag_1 << endl; } void Gril_Calib::IMU_time_compensate(const double &lag_time, const bool &is_discard) { if (is_discard) { // Discard first 10 Lidar estimations and corresponding IMU measurements due to long time interval int i = 0; while (i < 10) { Lidar_state_group.pop_front(); IMU_state_group.pop_front(); i++; } } auto it_IMU_state = IMU_state_group.begin(); for (; it_IMU_state != IMU_state_group.end() - 1; it_IMU_state++) { it_IMU_state->timeStamp = it_IMU_state->timeStamp - lag_time; } while (Lidar_state_group.front().timeStamp < IMU_state_group.front().timeStamp) Lidar_state_group.pop_front(); while (Lidar_state_group.front().timeStamp > IMU_state_group[1].timeStamp) IMU_state_group.pop_front(); // align the size of two sequences while (IMU_state_group.size() > Lidar_state_group.size()) IMU_state_group.pop_back(); while (IMU_state_group.size() < Lidar_state_group.size()) Lidar_state_group.pop_back(); } void Gril_Calib::cut_sequence_tail() { for (int i = 0; i < 20; ++i) { Lidar_state_group.pop_back(); IMU_state_group.pop_back(); } while (Lidar_state_group.front().timeStamp < IMU_state_group.front().timeStamp) Lidar_state_group.pop_front(); while (Lidar_state_group.front().timeStamp > IMU_state_group[1].timeStamp) IMU_state_group.pop_front(); //Align the size of two sequences while (IMU_state_group.size() > Lidar_state_group.size()) IMU_state_group.pop_back(); while (IMU_state_group.size() < Lidar_state_group.size()) Lidar_state_group.pop_back(); } void Gril_Calib::acc_interpolate() { //Interpolation to get acc_I(t_L) for (int i = 1; i < Lidar_state_group.size() - 1; i++) { double deltaT = Lidar_state_group[i].timeStamp - IMU_state_group[i].timeStamp; if (deltaT > 0) { double DeltaT = IMU_state_group[i + 1].timeStamp - IMU_state_group[i].timeStamp; double s = deltaT / DeltaT; IMU_state_group[i].linear_acc = s * IMU_state_group[i + 1].linear_acc + (1 - s) * IMU_state_group[i].linear_acc; IMU_state_group[i].timeStamp += deltaT; } else { double DeltaT = IMU_state_group[i].timeStamp - IMU_state_group[i - 1].timeStamp; double s = -deltaT / DeltaT; IMU_state_group[i].linear_acc = s * IMU_state_group[i - 1].linear_acc + (1 - s) * IMU_state_group[i].linear_acc; IMU_state_group[i].timeStamp += deltaT; } } } // Butterworth filter (Low-pass filter) void Gril_Calib::Butter_filt(const deque &signal_in, deque &signal_out) { Gril_Calib::Butterworth butter; butter.extend_num = 10 * (butter.Coeff_size - 1); auto it_front = signal_in.begin() + butter.extend_num; auto it_back = signal_in.end() - 1 - butter.extend_num; deque extend_front; deque extend_back; for (int idx = 0; idx < butter.extend_num; idx++) { extend_front.push_back(*it_front); extend_back.push_front(*it_back); it_front--; it_back++; } deque sig_extended(signal_in); while (!extend_front.empty()) { sig_extended.push_front(extend_front.back()); extend_front.pop_back(); } while (!extend_back.empty()) { sig_extended.push_back(extend_back.front()); extend_back.pop_front(); } deque sig_out(sig_extended); // One-direction Butterworth filter Starts (all states) for (int i = butter.Coeff_size; i < sig_extended.size() - butter.extend_num; i++) { CalibState temp_state; for (int j = 0; j < butter.Coeff_size; j++) { auto it_sig_ext = *(sig_extended.begin() + i - j); temp_state += it_sig_ext * butter.Coeff_b[j]; } for (int jj = 1; jj < butter.Coeff_size; jj++) { auto it_sig_out = *(sig_out.begin() + i - jj); temp_state -= it_sig_out * butter.Coeff_a[jj]; } sig_out[i] = temp_state; } for (auto it = sig_out.begin() + butter.extend_num; it != sig_out.end() - butter.extend_num; it++) { signal_out.push_back(*it); } } // zero phase low-pass filter // void Gril_Calib::zero_phase_filt(const deque &signal_in, deque &signal_out) { deque sig_out1; Butter_filt(signal_in, sig_out1); // signal_in에 대해 Butterworth filter를 적용한 결과를 sig_out1에 저장 deque sig_rev(sig_out1); reverse(sig_rev.begin(), sig_rev.end()); //Reverse the elements Butter_filt(sig_rev, signal_out); reverse(signal_out.begin(), signal_out.end()); //Reverse the elements } // To obtain a rough initial value of rotation matrix // void Gril_Calib::solve_Rotation_only() { double R_LI_quat[4]; R_LI_quat[0] = 1; R_LI_quat[1] = 0; R_LI_quat[2] = 0; R_LI_quat[3] = 0; ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization(); ceres::Problem problem_rot; problem_rot.AddParameterBlock(R_LI_quat, 4, quatParam); for (int i = 0; i < IMU_state_group.size(); i++) { M3D Lidar_angvel_skew; Lidar_angvel_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_vel); problem_rot.AddResidualBlock(Angular_Vel_Cost_only_Rot::Create(IMU_state_group[i].ang_vel, Lidar_state_group[i].ang_vel), nullptr, R_LI_quat); } ceres::Solver::Options options_quat; ceres::Solver::Summary summary_quat; ceres::Solve(options_quat, &problem_rot, &summary_quat); Eigen::Quaterniond q_LI(R_LI_quat[0], R_LI_quat[1], R_LI_quat[2], R_LI_quat[3]); Rot_Lidar_wrt_IMU = q_LI.matrix(); // LiDAR angulr velocity in IMU frame (from LiDAR to IMU) } // Proposed algorithm (Rotation + Translation) void Gril_Calib::solve_Rot_Trans_calib(double &timediff_imu_wrt_lidar, const double &imu_sensor_height) { M3D R_IL_init = Rot_Lidar_wrt_IMU.transpose(); // Initial value of Rotation of IL (from IMU frame to LiDAR frame) Eigen::Quaterniond quat(R_IL_init); double R_IL_quat[4]; R_IL_quat[0] = quat.w(); R_IL_quat[1] = quat.x(); R_IL_quat[2] = quat.y(); R_IL_quat[3] = quat.z(); double Trans_IL[3]; // Initial value of Translation of IL (IMU with respect to Lidar) - ceres solver input Trans_IL[0] = trans_IL_x; Trans_IL[1] = trans_IL_y; Trans_IL[2] = trans_IL_z; double bias_g[3]; // Initial value of gyro bias bias_g[0] = 0; bias_g[1] = 0; bias_g[2] = 0; double bias_aL[3]; // Initial value of acc bias bias_aL[0] = 0; bias_aL[1] = 0; bias_aL[2] = 0; double time_lag2 = 0; // Second time lag (IMU wtr Lidar) // Define problem ceres::Problem problem_Ex_calib; ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization(); // Define Loss function ceres::LossFunction *loss_function_angular = new ceres::CauchyLoss(0.5); ceres::ScaledLoss *loss_function_angular_scaled = new ceres::ScaledLoss(loss_function_angular, 0.5, ceres::TAKE_OWNERSHIP); ceres::LossFunction *loss_function_acc = new ceres::CauchyLoss(0.5); ceres::ScaledLoss *scaled_loss_acc = new ceres::ScaledLoss(loss_function_acc, 0.2, ceres::TAKE_OWNERSHIP); ceres::LossFunction *loss_function_plain_motion = new ceres::HuberLoss(0.5); ceres::ScaledLoss *loss_function_plain_motion_scaled = new ceres::ScaledLoss(loss_function_plain_motion, 0.3, ceres::TAKE_OWNERSHIP); // Add Parameter Block problem_Ex_calib.AddParameterBlock(R_IL_quat, 4, quatParam); problem_Ex_calib.AddParameterBlock(Trans_IL, 3); problem_Ex_calib.AddParameterBlock(bias_g, 3); problem_Ex_calib.AddParameterBlock(bias_aL, 3); //Jacobian of acc_bias, gravity, Translation int Jaco_size = 3 * Lidar_state_group.size(); MatrixXd Jacobian(Jaco_size, 9); Jacobian.setZero(); // Jacobian of Translation MatrixXd Jaco_Trans(Jaco_size, 3); Jaco_Trans.setZero(); // Add Residual Block for (int i = 0; i < IMU_state_group.size(); i++) { double deltaT = Lidar_state_group[i].timeStamp - IMU_state_group[i].timeStamp; problem_Ex_calib.AddResidualBlock(Ground_Plane_Cost_IL::Create(Lidar_wrt_ground_group[i], IMU_wrt_ground_group[i], distance_Lidar_wrt_ground_group[i], imu_sensor_height), loss_function_plain_motion_scaled, R_IL_quat, Trans_IL); problem_Ex_calib.AddResidualBlock(Angular_Vel_IL_Cost::Create(IMU_state_group[i].ang_vel, IMU_state_group[i].ang_acc, Lidar_state_group[i].ang_vel, deltaT), loss_function_angular_scaled, R_IL_quat, bias_g, &time_lag2); problem_Ex_calib.AddResidualBlock(Linear_acc_Rot_Cost_without_Gravity::Create(Lidar_state_group[i], IMU_state_group[i].linear_acc, Lidar_wrt_ground_group[i]), scaled_loss_acc, R_IL_quat, bias_aL, Trans_IL); Jacobian.block<3, 3>(3 * i, 0) = -Lidar_state_group[i].rot_end; Jacobian.block<3, 3>(3 * i, 3) << SKEW_SYM_MATRX(STD_GRAV); M3D omg_skew, angacc_skew; omg_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_vel); angacc_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_acc); M3D Jaco_trans_i = -omg_skew * omg_skew - angacc_skew; Jaco_Trans.block<3, 3>(3 * i, 0) = Jaco_trans_i; Jacobian.block<3, 3>(3 * i, 6) = Jaco_trans_i; } // Set boundary for (int index = 0; index < 3; ++index) { problem_Ex_calib.SetParameterUpperBound(bias_aL, index, 0.01); problem_Ex_calib.SetParameterLowerBound(bias_aL, index, -0.01); problem_Ex_calib.SetParameterUpperBound(bias_g, index, 0.01); problem_Ex_calib.SetParameterLowerBound(bias_g, index, -0.01); } if(set_boundary) { for (int index = 0; index < 3; ++index) { problem_Ex_calib.SetParameterUpperBound(Trans_IL, index, Trans_IL[index] + bound_th); problem_Ex_calib.SetParameterLowerBound(Trans_IL, index, Trans_IL[index] - bound_th); } } // Solver options ceres::Solver::Options options_Ex_calib; options_Ex_calib.num_threads = 1; options_Ex_calib.use_explicit_schur_complement = true; options_Ex_calib.linear_solver_type = ceres::ITERATIVE_SCHUR; options_Ex_calib.preconditioner_type = ceres::SCHUR_JACOBI; options_Ex_calib.minimizer_progress_to_stdout = false; // Solve ceres::Solver::Summary summary_Ex_calib; ceres::Solve(options_Ex_calib, &problem_Ex_calib, &summary_Ex_calib); // std::cout << summary_Ex_calib.FullReport() << "\n"; //** Update the result **// // Rotation matrix 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 Rot_Lidar_wrt_IMU = q_IL_final.matrix().transpose(); V3D euler_angle = RotMtoEuler(Rot_Lidar_wrt_IMU); // Translation vector V3D Trans_IL_vec(Trans_IL[0], Trans_IL[1], Trans_IL[2]); Trans_Lidar_wrt_IMU = -1.0 * Rot_Lidar_wrt_IMU * Trans_IL_vec; // gravity vector - not used M3D R_WLO = Lidar_wrt_ground_group[0].matrix(); Grav_L0 = R_WLO * STD_GRAV; // gravity in first lidar frame // bias acc V3D bias_a_Lidar(bias_aL[0], bias_aL[1], bias_aL[2]); acc_bias = Rot_Lidar_wrt_IMU * bias_a_Lidar; // bias gyro gyro_bias = V3D(bias_g[0], bias_g[1], bias_g[2]); // time offset time_lag_2 = time_lag2; time_delay_IMU_wtr_Lidar = time_lag_1 + time_lag_2; time_offset_result = time_delay_IMU_wtr_Lidar; //The second temporal compensation IMU_time_compensate(get_lag_time_2(), false); // For debug for (int i = 0; i < IMU_state_group.size(); i++) { M3D R_GL = Lidar_wrt_ground_group[i].matrix(); V3D Grav_L = R_GL * STD_GRAV; V3D acc_I = Lidar_state_group[i].rot_end * Rot_Lidar_wrt_IMU.transpose() * IMU_state_group[i].linear_acc - Lidar_state_group[i].rot_end * bias_a_Lidar; V3D acc_L = Lidar_state_group[i].linear_acc + Lidar_state_group[i].rot_end * Jaco_Trans.block<3, 3>(3 * i, 0) * Trans_IL_vec - Grav_L; fout_acc_cost << setprecision(10) << acc_I.transpose() << " " << acc_L.transpose() << " " << IMU_state_group[i].timeStamp << " " << Lidar_state_group[i].timeStamp << endl; } } void Gril_Calib::normalize_acc(deque &signal_in) { V3D mean_acc(0, 0, 0); for (int i = 1; i < 10; i++) { mean_acc += (signal_in[i].linear_acc - mean_acc) / i; } for (int i = 0; i < signal_in.size(); i++) { signal_in[i].linear_acc = signal_in[i].linear_acc / mean_acc.norm() * G_m_s2; } } // Align the size of various states void Gril_Calib::align_Group(const deque &IMU_states, deque &Lidar_wrt_ground_states, deque &IMU_wrt_ground_states, deque &normal_vector_wrt_lidar_group, deque &distance_Lidar_ground_states) { // Align the size of two sequences while(IMU_states.size() < Lidar_wrt_ground_states.size()) { Lidar_wrt_ground_states.pop_back(); IMU_wrt_ground_states.pop_back(); normal_vector_wrt_lidar_group.pop_back(); distance_Lidar_ground_states.pop_back(); } } bool Gril_Calib::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) { //Calculation of Rotation Jacobian M3D lidar_omg_skew; lidar_omg_skew << SKEW_SYM_MATRX(lidar_omg); Jacobian_rot.block<3, 3>(3 * frame_num, 0) = lidar_omg_skew; bool data_sufficient = false; //Give a Data Appraisal every second if (frame_num % orig_odom_freq * cut_frame_num == 0) { M3D Hessian_rot = Jacobian_rot.transpose() * Jacobian_rot; EigenSolver es(Hessian_rot); V3D EigenValue = es.eigenvalues().real(); M3D EigenVec_mat = es.eigenvectors().real(); M3D EigenMatCwise = EigenVec_mat.cwiseProduct(EigenVec_mat); std::vector EigenMat_1_col{EigenMatCwise(0, 0), EigenMatCwise(1, 0), EigenMatCwise(2, 0)}; std::vector EigenMat_2_col{EigenMatCwise(0, 1), EigenMatCwise(1, 1), EigenMatCwise(2, 1)}; std::vector EigenMat_3_col{EigenMatCwise(0, 2), EigenMatCwise(1, 2), EigenMatCwise(2, 2)}; // Find the maximum value of each column int maxPos[3] = {0}; maxPos[0] = max_element(EigenMat_1_col.begin(), EigenMat_1_col.end()) - EigenMat_1_col.begin(); maxPos[1] = max_element(EigenMat_2_col.begin(), EigenMat_2_col.end()) - EigenMat_2_col.begin(); maxPos[2] = max_element(EigenMat_3_col.begin(), EigenMat_3_col.end()) - EigenMat_3_col.begin(); V3D Scaled_Eigen = EigenValue / data_accum_length; // the larger data_accum_length is, the more data is needed V3D Rot_percent(Scaled_Eigen[1] * Scaled_Eigen[2], Scaled_Eigen[0] * Scaled_Eigen[2], Scaled_Eigen[0] * Scaled_Eigen[1]); int axis[3]; axis[2] = max_element(maxPos, maxPos + 3) - maxPos; axis[0] = min_element(maxPos, maxPos + 3) - maxPos; axis[1] = 3 - (axis[0] + axis[2]); double percentage_x = Rot_percent[axis[0]] < x_accumulate ? Rot_percent[axis[0]] : 1; double percentage_y = Rot_percent[axis[1]] < y_accumulate ? Rot_percent[axis[1]] : 1; double percentage_z = Rot_percent[axis[2]] < z_accumulate ? Rot_percent[axis[2]] : 1; clear(); //clear the screen printf("\033[3A\r"); printProgress(percentage_x, 88); printProgress(percentage_y, 89); printProgress(percentage_z, 90); if(verbose){ M3D R_GL = lidar_q.toRotationMatrix(); M3D R_GI = imu_q.toRotationMatrix(); printf(BOLDREDPURPLE "[Rotation matrix Ground to LiDAR (euler)] " RESET); cout << setprecision(4) << RotMtoEuler(R_GL).transpose() * 57.3 << " deg" << '\n'; printf(BOLDREDPURPLE "[Rotation matrix Ground to IMU (euler)] " RESET); cout << setprecision(4) << RotMtoEuler(R_GI).transpose() * 57.3 << " deg" << '\n'; printf(BOLDREDPURPLE "[Estimated LiDAR sensor height] : " RESET); cout << setprecision(4) << lidar_estimate_height << " m" << '\n'; } fflush(stdout); if (Rot_percent[axis[0]] > x_accumulate && Rot_percent[axis[1]] > y_accumulate && Rot_percent[axis[2]] > z_accumulate) { printf(BOLDCYAN "[calibration] Data accumulation finished, Lidar IMU calibration begins.\n\n" RESET); printf(BOLDBLUE"============================================================ \n\n" RESET); data_sufficient = true; } } if (data_sufficient) return true; else return false; } void Gril_Calib::printProgress(double percentage, int axis_ascii) { int val = (int) (percentage * 100); int lpad = (int) (percentage * PBWIDTH); int rpad = PBWIDTH - lpad; printf(BOLDCYAN "[Data accumulation] "); if (percentage < 1) { printf(BOLDYELLOW "Rotation around Lidar %c Axis: ", char(axis_ascii)); printf(YELLOW "%3d%% [%.*s%*s]\n", val, lpad, PBSTR, rpad, ""); cout << RESET; } else { printf(BOLDGREEN "Rotation around Lidar %c Axis complete! ", char(axis_ascii)); cout << RESET << "\n"; } } void Gril_Calib::clear() { // CSI[2J clears screen, CSI[H moves the cursor to top-left corner cout << "\x1B[2J\x1B[H"; } //** main function in LiDAR IMU calibration **// void Gril_Calib::LI_Calibration(int &orig_odom_freq, int &cut_frame_num, double &timediff_imu_wrt_lidar, const double &move_start_time) { TimeConsuming time("Batch optimization"); downsample_interpolate_IMU(move_start_time); fout_before_filter(); IMU_time_compensate(0.0, true); deque IMU_after_zero_phase; deque Lidar_after_zero_phase; zero_phase_filt(get_IMU_state(), IMU_after_zero_phase); // zero phase low-pass filter normalize_acc(IMU_after_zero_phase); zero_phase_filt(get_Lidar_state(), Lidar_after_zero_phase); set_IMU_state(IMU_after_zero_phase); set_Lidar_state(Lidar_after_zero_phase); cut_sequence_tail(); xcorr_temporal_init(orig_odom_freq * cut_frame_num); IMU_time_compensate(get_lag_time_1(), false); central_diff(); deque IMU_after_2nd_zero_phase; deque Lidar_after_2nd_zero_phase; zero_phase_filt(get_IMU_state(), IMU_after_2nd_zero_phase); zero_phase_filt(get_Lidar_state(), Lidar_after_2nd_zero_phase); set_states_2nd_filter(IMU_after_2nd_zero_phase, Lidar_after_2nd_zero_phase); fout_check_lidar(); // file output for visualizing lidar low pass filter solve_Rotation_only(); acc_interpolate(); align_Group(IMU_state_group, Lidar_wrt_ground_group, IMU_wrt_ground_group, normal_vector_wrt_lidar_group, distance_Lidar_wrt_ground_group); // Calibration at once solve_Rot_Trans_calib(timediff_imu_wrt_lidar, imu_sensor_height); printf(BOLDBLUE"============================================================ \n\n" RESET); double time_L_I = timediff_imu_wrt_lidar + time_delay_IMU_wtr_Lidar; print_calibration_result(time_L_I, Rot_Lidar_wrt_IMU, Trans_Lidar_wrt_IMU, gyro_bias, acc_bias, Grav_L0); printf(BOLDBLUE"============================================================ \n\n" RESET); printf(BOLDCYAN "Gril-Calib : Ground Robot IMU-LiDAR calibration done.\n"); printf("" RESET); // For debug // plot_result(); } void 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) { cout.setf(ios::fixed); printf(BOLDCYAN "[Calibration Result] " RESET); cout << setprecision(6) << "Rotation matrix from LiDAR frame to IMU frame = " << RotMtoEuler(R_L_I).transpose() * 57.3 << " deg" << endl; printf(BOLDCYAN "[Calibration Result] " RESET); cout << "Translation vector from LiDAR frame to IMU frame = " << p_L_I.transpose() << " m" << endl; printf(BOLDCYAN "[Calibration Result] " RESET); printf("Time Lag IMU to LiDAR = %.8lf s \n", time_L_I); printf(BOLDCYAN "[Calibration Result] " RESET); cout << "Bias of Gyroscope = " << bias_g.transpose() << " rad/s" << endl; printf(BOLDCYAN "[Calibration Result] " RESET); cout << "Bias of Accelerometer = " << bias_a.transpose() << " m/s^2" << endl; } void Gril_Calib::plot_result() { 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); for (auto it_IMU_state = IMU_state_group.begin(); it_IMU_state != IMU_state_group.end() - 1; it_IMU_state++) { for (int i = 0; i < 3; i++) { IMU_omg[i].push_back(it_IMU_state->ang_vel[i]); IMU_ang_acc[i].push_back(it_IMU_state->ang_acc[i]); IMU_acc[i].push_back(it_IMU_state->linear_acc[i]); } } for (auto it_Lidar_state = Lidar_state_group.begin(); it_Lidar_state != Lidar_state_group.end() - 1; it_Lidar_state++) { for (int i = 0; i < 3; i++) { Lidar_pos[i].push_back(it_Lidar_state->pos_end[i]); Lidar_omg[i].push_back(it_Lidar_state->ang_vel[i]); Lidar_acc[i].push_back(it_Lidar_state->linear_acc[i]); Lidar_ang_acc[i].push_back(it_Lidar_state->ang_acc[i]); Lidar_vel[i].push_back(it_Lidar_state->linear_vel[i]); } } plt::figure(1); plt::subplot(2, 3, 1); plt::title("IMU Angular Velocity"); plt::named_plot("IMU omg x", IMU_omg[0]); plt::named_plot("IMU omg y", IMU_omg[1]); plt::named_plot("IMU omg z", IMU_omg[2]); plt::legend(); plt::grid(true); plt::subplot(2, 3, 2); plt::title("IMU Linear Acceleration"); plt::named_plot("IMU acc x", IMU_acc[0]); plt::named_plot("IMU acc y", IMU_acc[1]); plt::named_plot("IMU acc z", IMU_acc[2]); plt::legend(); plt::grid(true); plt::subplot(2, 3, 3); plt::title("IMU Angular Acceleration"); plt::named_plot("IMU ang acc x", IMU_ang_acc[0]); plt::named_plot("IMU ang acc y", IMU_ang_acc[1]); plt::named_plot("IMU ang acc z", IMU_ang_acc[2]); plt::legend(); plt::grid(true); plt::subplot(2, 3, 4); plt::title("LiDAR Angular Velocity"); plt::named_plot("Lidar omg x", Lidar_omg[0]); plt::named_plot("Lidar omg y", Lidar_omg[1]); plt::named_plot("Lidar omg z", Lidar_omg[2]); plt::legend(); plt::grid(true); plt::subplot(2, 3, 5); plt::title("LiDAR Linear Acceleration"); plt::named_plot("Lidar acc x", Lidar_acc[0]); plt::named_plot("Lidar acc y", Lidar_acc[1]); plt::named_plot("Lidar acc z", Lidar_acc[2]); plt::legend(); plt::grid(true); plt::subplot(2, 3, 6); plt::title("LiDAR Angular Acceleration"); plt::named_plot("Lidar ang acc x", Lidar_ang_acc[0]); plt::named_plot("Lidar ang acc y", Lidar_ang_acc[1]); plt::named_plot("Lidar ang acc z", Lidar_ang_acc[2]); plt::legend(); plt::grid(true); plt::show(); plt::pause(0); plt::close(); } ================================================ FILE: include/Gril_Calib/Gril_Calib.h ================================================ #ifndef Gril_Calib_H #define Gril_Calib_H #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "matplotlibcpp.h" #include #define FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) namespace plt = matplotlibcpp; using namespace std; using namespace Eigen; typedef Vector3d V3D; typedef Matrix3d M3D; typedef Eigen::Quaterniond QD; const V3D STD_GRAV = V3D(0, 0, -G_m_s2); extern double GYRO_FACTOR_; extern double ACC_FACTOR_; extern double GROUND_FACTOR_; // Lidar IMU calibration // States needed by calibration struct CalibState { M3D rot_end; V3D pos_end; V3D ang_vel; V3D linear_vel; V3D ang_acc; V3D linear_acc; double timeStamp; CalibState() { rot_end = Eye3d; pos_end = Zero3d; ang_vel = Zero3d; linear_vel = Zero3d; ang_acc = Zero3d; linear_acc = Zero3d; timeStamp = 0.0; }; CalibState(const CalibState &b) { this->rot_end = b.rot_end; this->pos_end = b.pos_end; this->ang_vel = b.ang_vel; this->ang_acc = b.ang_acc; this->linear_vel = b.linear_vel; this->linear_acc = b.linear_acc; this->timeStamp = b.timeStamp; }; CalibState operator*(const double &coeff) { CalibState a; a.ang_vel = this->ang_vel * coeff; a.ang_acc = this->ang_acc * coeff; a.linear_vel = this->linear_vel * coeff; a.linear_acc = this->linear_acc * coeff; return a; }; CalibState &operator+=(const CalibState &b) { this->ang_vel += b.ang_vel; this->ang_acc += b.ang_acc; this->linear_vel += b.linear_vel; this->linear_acc += b.linear_acc; return *this; }; CalibState &operator-=(const CalibState &b) { this->ang_vel -= b.ang_vel; this->ang_acc -= b.ang_acc; this->linear_vel -= b.linear_vel; this->linear_acc -= b.linear_acc; return *this; }; CalibState &operator=(const CalibState &b) { this->ang_vel = b.ang_vel; this->ang_acc = b.ang_acc; this->linear_vel = b.linear_vel; this->linear_acc = b.linear_acc; return *this; }; }; struct Angular_Vel_Cost_only_Rot { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Angular_Vel_Cost_only_Rot(V3D IMU_ang_vel_, V3D Lidar_ang_vel_) : IMU_ang_vel(IMU_ang_vel_), Lidar_ang_vel(Lidar_ang_vel_) {} template bool operator()(const T *q, T *residual) const { Eigen::Matrix IMU_ang_vel_T = IMU_ang_vel.cast(); Eigen::Matrix Lidar_ang_vel_T = Lidar_ang_vel.cast(); Eigen::Quaternion q_LI{q[0], q[1], q[2], q[3]}; Eigen::Matrix R_LI = q_LI.toRotationMatrix(); //Rotation (from LiDAR to IMU) Eigen::Matrix resi = R_LI * Lidar_ang_vel_T - IMU_ang_vel_T; residual[0] = resi[0]; residual[1] = resi[1]; residual[2] = resi[2]; return true; } static ceres::CostFunction *Create(const V3D IMU_ang_vel_, const V3D Lidar_ang_vel_) { return (new ceres::AutoDiffCostFunction( new Angular_Vel_Cost_only_Rot(IMU_ang_vel_, Lidar_ang_vel_))); } V3D IMU_ang_vel; V3D Lidar_ang_vel; }; struct Angular_Vel_IL_Cost { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Angular_Vel_IL_Cost(V3D IMU_ang_vel_, V3D IMU_ang_acc_, V3D Lidar_ang_vel_, double deltaT_LI_) : IMU_ang_vel(IMU_ang_vel_), IMU_ang_acc(IMU_ang_acc_), Lidar_ang_vel(Lidar_ang_vel_), deltaT_LI(deltaT_LI_) {} template bool operator()(const T *q, const T *b_g, const T *t, T *residual) const { //Known parameters used for Residual Construction Eigen::Matrix IMU_ang_vel_T = IMU_ang_vel.cast(); Eigen::Matrix IMU_ang_acc_T = IMU_ang_acc.cast(); Eigen::Matrix Lidar_ang_vel_T = Lidar_ang_vel.cast(); T deltaT_LI_T{deltaT_LI}; //Unknown Parameters, needed to be estimated Eigen::Quaternion q_IL{q[0], q[1], q[2], q[3]}; Eigen::Matrix R_IL = q_IL.toRotationMatrix(); //Rotation Eigen::Matrix bias_g{b_g[0], b_g[1], b_g[2]}; //Bias of gyroscope T td{t[0]}; //Time lag (IMU wtr Lidar) // original Residual Eigen::Matrix resi = R_IL.transpose() * Lidar_ang_vel_T - IMU_ang_vel_T - (deltaT_LI_T + td) * IMU_ang_acc_T + bias_g; residual[0] = T(GYRO_FACTOR_) * resi[0]; residual[1] = T(GYRO_FACTOR_) * resi[1]; residual[2] = T(GYRO_FACTOR_) * resi[2]; return true; } static ceres::CostFunction * Create(const V3D IMU_ang_vel_, const V3D IMU_ang_acc_, const V3D Lidar_ang_vel_, const double deltaT_LI_) { // 3 residual, 4 parameter (q), 3 parameter (bias), 1 parameter (time lag) return (new ceres::AutoDiffCostFunction( new Angular_Vel_IL_Cost(IMU_ang_vel_, IMU_ang_acc_, Lidar_ang_vel_, deltaT_LI_))); } V3D IMU_ang_vel; V3D IMU_ang_acc; V3D Lidar_ang_vel; double deltaT_LI; }; struct Ground_Plane_Cost_IL { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Ground_Plane_Cost_IL(QD Lidar_wrt_ground_, QD IMU_wrt_ground_, const double distance_Lidar_wrt_ground_, const double imu_sensor_height_) : Lidar_wrt_ground(Lidar_wrt_ground_), IMU_wrt_ground(IMU_wrt_ground_), distance_Lidar_wrt_ground(distance_Lidar_wrt_ground_), imu_sensor_height(imu_sensor_height_) {} template bool operator()(const T *q, const T *trans, T *residual) const { // Known parameters used for Residual Construction Eigen::Quaternion Lidar_wrt_ground_T = Lidar_wrt_ground.cast(); // from ground frame to LiDAR plane frame Eigen::Matrix R_GL = Lidar_wrt_ground_T.toRotationMatrix(); // Rotation matrix (from Ground to LiDAR) Eigen::Quaternion IMU_wrt_ground_T = IMU_wrt_ground.cast(); // from ground frame (earth frame) to IMU frame Eigen::Matrix R_GI = IMU_wrt_ground_T.toRotationMatrix(); // Rotation matrix (from Ground to IMU) T distance_Lidar_wrt_ground_T = T(distance_Lidar_wrt_ground); // Distance of LiDAR plane from ground T imu_sensor_height_T = T(imu_sensor_height); // Height of IMU sensor from ground // Unknown Parameters, needed to be estimated Eigen::Quaternion q_IL{q[0], q[1], q[2], q[3]}; Eigen::Matrix R_IL = q_IL.toRotationMatrix(); // Rotation IMU frame to LiDAR frame Eigen::Matrix T_IL{trans[0], trans[1], trans[2]}; // Translation of I-L (IMU wtr Lidar) // Plane motion constraint Residual Eigen::Matrix R_plane = R_IL.transpose() * R_GI.transpose() * R_GL; Eigen::Matrix e3 = Eigen::Matrix::UnitZ(); // (0,0,1) Eigen::Matrix resi_plane = R_plane * e3; // Distance constraint Residual - TODO : Generalized Eigen::Matrix imu_height_vec = imu_sensor_height_T * e3; Eigen::Matrix lidar_height_vec = distance_Lidar_wrt_ground_T * e3; Eigen::Matrix tmp_distance = R_IL * R_GI * imu_height_vec - R_GL * lidar_height_vec; T resi_distance = T_IL[2] - tmp_distance[2]; // Residual residual[0] = T(GROUND_FACTOR_) * resi_plane[0]; residual[1] = T(GROUND_FACTOR_) * resi_plane[1]; residual[2] = T(GROUND_FACTOR_) * resi_distance; return true; } static ceres::CostFunction * Create(const QD Lidar_wrt_ground_, const QD IMU_wrt_ground_, const double distance_Lidar_wrt_ground, const double imu_sensor_height) { // 3 residual, 4 parameter (q), 3 paramter (translation) return (new ceres::AutoDiffCostFunction( new Ground_Plane_Cost_IL(Lidar_wrt_ground_, IMU_wrt_ground_, distance_Lidar_wrt_ground, imu_sensor_height))); } QD Lidar_wrt_ground; QD IMU_wrt_ground; double distance_Lidar_wrt_ground; double imu_sensor_height; }; struct Linear_acc_Rot_Cost_without_Gravity { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Linear_acc_Rot_Cost_without_Gravity(CalibState LidarState_, V3D IMU_linear_acc_, QD Lidar_wrt_ground_) : LidarState(LidarState_), IMU_linear_acc(IMU_linear_acc_), Lidar_wrt_ground(Lidar_wrt_ground_) {} template bool operator()(const T *q,const T *b_a, const T *trans, T *residual) const { // Known parameters used for Residual Construction Eigen::Matrix R_LL0_T = LidarState.rot_end.cast(); Eigen::Matrix IMU_linear_acc_T = IMU_linear_acc.cast(); // Linear acceleration of IMU Eigen::Matrix Lidar_linear_acc_T = LidarState.linear_acc.cast(); // lidar linear acceleration Eigen::Quaternion Lidar_wrt_ground_T = Lidar_wrt_ground.cast(); // from ground frame to LiDAR plane frame Eigen::Matrix R_GL = Lidar_wrt_ground_T.toRotationMatrix(); // Rotation matrix from Ground frame to LiDAR frame // Unknown Parameters, needed to be estimated Eigen::Matrix bias_aL{b_a[0], b_a[1], b_a[2]}; // Bias of Linear acceleration Eigen::Matrix T_IL{trans[0], trans[1], trans[2]}; // Translation of I-L (IMU wtr Lidar) Eigen::Quaternion q_IL{q[0], q[1], q[2], q[3]}; // Rotation from IMU frame to LiDAR frame Eigen::Matrix R_IL = q_IL.toRotationMatrix(); // Residual Construction M3D Lidar_omg_SKEW, Lidar_angacc_SKEW; Lidar_omg_SKEW << SKEW_SYM_MATRX(LidarState.ang_vel); Lidar_angacc_SKEW << SKEW_SYM_MATRX(LidarState.ang_acc); M3D Jacob_trans = Lidar_omg_SKEW * Lidar_omg_SKEW + Lidar_angacc_SKEW; Eigen::Matrix Jacob_trans_T = Jacob_trans.cast(); Eigen::Matrix resi = R_LL0_T * R_IL * IMU_linear_acc_T - R_LL0_T * bias_aL + R_GL * STD_GRAV - Lidar_linear_acc_T - R_LL0_T * Jacob_trans_T * T_IL; residual[0] = T(ACC_FACTOR_) * resi[0]; residual[1] = T(ACC_FACTOR_) * resi[1]; residual[2] = T(ACC_FACTOR_) * resi[2]; return true; } static ceres::CostFunction *Create(const CalibState LidarState_, const V3D IMU_linear_acc_, const QD Lidar_wrt_ground_) { // residual 3, quaternion 4, bias 3, translation 3 return (new ceres::AutoDiffCostFunction( new Linear_acc_Rot_Cost_without_Gravity(LidarState_, IMU_linear_acc_, Lidar_wrt_ground_))); } CalibState LidarState; V3D IMU_linear_acc; QD Lidar_wrt_ground; }; class Gril_Calib { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ofstream fout_LiDAR_meas, fout_IMU_meas, fout_before_filt_IMU, fout_before_filt_Lidar, fout_acc_cost, fout_after_rot; ofstream fout_LiDAR_ang_vel, fout_IMU_ang_vel, fout_Jacob_trans; ofstream fout_LiDAR_meas_after; double data_accum_length = 0.0; double x_accumulate = 0.0; double y_accumulate = 0.0; double z_accumulate = 0.0; double svd_threshold = 0.01; double imu_sensor_height = 0.0; double trans_IL_x, trans_IL_y, trans_IL_z; double bound_th; bool set_boundary = false; bool verbose = false; Gril_Calib(); ~Gril_Calib(); // original struct Butterworth { // Coefficients of 6 order butterworth low pass filter, omega = 0.15 double Coeff_b[7] = {0.0001,0.0005,0.0011,0.0015,0.0011,0.0005,0.0001}; double Coeff_a[7] = {1,-4.1824,7.4916,-7.3136,4.0893,-1.2385,0.1584}; int Coeff_size = 7; int extend_num = 0; }; void plot_result(); void push_ALL_IMU_CalibState(const sensor_msgs::Imu::ConstPtr &msg, const double &mean_acc_norm); void push_IMU_CalibState(const V3D &omg, const V3D &acc, const double ×tamp); void push_Lidar_CalibState(const M3D &rot, const V3D &pos, const V3D &omg, const V3D &linear_vel, const double ×tamp); void push_Plane_Constraint(const Eigen::Quaterniond &q_lidar, const Eigen::Quaterniond &q_imu, const V3D &normal_lidar, const double &distance_lidar); void downsample_interpolate_IMU(const double &move_start_time); void central_diff(); void xcorr_temporal_init(const double &odom_freq); void IMU_time_compensate(const double &lag_time, const bool &is_discard); void acc_interpolate(); void Butter_filt(const deque &signal_in, deque &signal_out); void zero_phase_filt(const deque &signal_in, deque &signal_out); void cut_sequence_tail(); void set_IMU_state(const deque &IMU_states); void set_Lidar_state(const deque &Lidar_states); void set_states_2nd_filter(const deque &IMU_states, const deque &Lidar_states); void solve_Rot_Trans_calib(double &timediff_imu_wrt_lidar, const double &imu_sensor_height); void normalize_acc(deque &signal_in); void align_Group(const deque &IMU_states, deque &Lidar_wrt_ground_states, deque &IMU_wrt_ground_states, deque &normal_vector_wrt_lidar_group, deque &distance_Lidar_ground_states); 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); void solve_Rotation_only(); void printProgress(double percentage, int axis); void clear(); void fout_before_filter(); void fout_check_lidar(); void LI_Calibration(int &orig_odom_freq, int &cut_frame_num, double &timediff_imu_wrt_lidar, const double &move_start_time); void print_calibration_result(double &time_L_I, M3D &R_L_I, V3D &p_L_I, V3D &bias_g, V3D &bias_a, V3D gravity); inline double get_lag_time_1() { return time_lag_1; } inline double get_lag_time_2() { return time_lag_2; } inline double get_total_time_lag() { return time_delay_IMU_wtr_Lidar; } inline double get_time_result() { return time_offset_result; } inline V3D get_Grav_L0() { return Grav_L0; } inline M3D get_R_LI() { return Rot_Lidar_wrt_IMU; } inline V3D get_T_LI() { return Trans_Lidar_wrt_IMU; } inline V3D get_gyro_bias() { return gyro_bias; } inline V3D get_acc_bias() { return acc_bias; } inline void IMU_buffer_clear() { IMU_state_group_ALL.clear(); } deque get_IMU_state() { return IMU_state_group; } deque get_Lidar_state() { return Lidar_state_group; } private: deque IMU_state_group; // LiDAR와 interpolation을 진행한 IMU data 결과를 가지고 있는 groups deque Lidar_state_group; // LiDAR state deque IMU_state_group_ALL; // 모든 IMU data (ROS topic)를 가지고 있는 groups deque Lidar_wrt_ground_group; // from ground frame (earth frame) to LiDAR plane frame deque IMU_wrt_ground_group; // from ground frame (earth frame) to IMU frame deque normal_vector_wrt_lidar_group; // normal vector of LiDAR ground segmentation deque distance_Lidar_wrt_ground_group; // distance from ground frame (earth frame) to LiDAR frame V3D Grav_L0; // Gravity vector in the initial Lidar frame L_0 /// Parameters needed to be calibrated M3D Rot_Grav_wrt_Init_Lidar; // Rotation from inertial frame G to initial Lidar frame L_0 M3D Rot_Lidar_wrt_IMU; // Rotation from Lidar frame L to IMU frame I V3D Trans_Lidar_wrt_IMU; // Translation from Lidar frame L to IMU frame I V3D gyro_bias; // gyro bias V3D acc_bias; // acc bias double time_delay_IMU_wtr_Lidar; //(Soft) time delay between IMU and Lidar = time_lag_1 + time_lag_2 double time_offset_result; // Time offset between IMU and Lidar (final result) double time_lag_1; // Time offset estimated by cross-correlation double time_lag_2; // Time offset estimated by unified optimization int lag_IMU_wtr_Lidar; // positive: timestamp of IMU is larger than that of LiDAR // Previous calibration results M3D R_IL_prev; // Rotation from Lidar frame L to IMU frame I V3D T_IL_prev; // Translation from Lidar frame L to IMU frame I V3D gyro_bias_prev; // gyro bias V3D acc_bias_prev; // acc bias double time_lag_2_prev; // Time offset estimated by unified optimization }; #endif ================================================ FILE: include/GroundSegmentation/patchworkpp.hpp ================================================ /** * @file patchworkpp.hpp * @author Seungjae Lee * @brief * @version 0.1 * @date 2022-07-20 * * @copyright Copyright (c) 2022 * */ #ifndef PATCHWORKPP_H #define PATCHWORKPP_H #include #include #include #include #include #include #include #include #define MARKER_Z_VALUE -2.2 #define UPRIGHT_ENOUGH 0.55 #define FLAT_ENOUGH 0.2 #define TOO_HIGH_ELEVATION 0.0 #define TOO_TILTED 1.0 #define NUM_HEURISTIC_MAX_PTS_IN_PATCH 3000 using Eigen::MatrixXf; using Eigen::JacobiSVD; using Eigen::VectorXf; using namespace std; /* * @brief: This function is for preprocessing of pointcloud data. (XYZI >> XYZ)) */ void xyzi2xyz(pcl::PointCloud::Ptr XYZI, pcl::PointCloud::Ptr XYZ) { (*XYZ).points.resize((*XYZI).size()); for (size_t i = 0; i < (*XYZI).points.size(); i++) { (*XYZ).points[i].x = (*XYZI).points[i].x; (*XYZ).points[i].y = (*XYZI).points[i].y; (*XYZ).points[i].z = (*XYZI).points[i].z; } } /* @brief PathWork ROS Node. */ template bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; } template struct RevertCandidate { int concentric_idx; int sector_idx; double ground_flatness; double line_variable; Eigen::Vector4f pc_mean; pcl::PointCloud regionwise_ground; RevertCandidate(int _c_idx, int _s_idx, double _flatness, double _line_var, Eigen::Vector4f _pc_mean, pcl::PointCloud _ground) : concentric_idx(_c_idx), sector_idx(_s_idx), ground_flatness(_flatness), line_variable(_line_var), pc_mean(_pc_mean), regionwise_ground(_ground) {} }; template class PatchWorkpp { public: typedef std::vector> Ring; typedef std::vector Zone; PatchWorkpp() {}; PatchWorkpp(ros::NodeHandle *nh) : node_handle_(*nh) { // Init ROS related ROS_INFO("Inititalizing PatchWork++..."); node_handle_.param("/patchworkpp/verbose", verbose_, false); node_handle_.param("/patchworkpp/sensor_height", sensor_height_, 1.723); node_handle_.param("/patchworkpp/num_iter", num_iter_, 3); node_handle_.param("/patchworkpp/num_lpr", num_lpr_, 20); node_handle_.param("/patchworkpp/num_min_pts", num_min_pts_, 10); node_handle_.param("/patchworkpp/th_seeds", th_seeds_, 0.4); node_handle_.param("/patchworkpp/th_dist", th_dist_, 0.3); node_handle_.param("/patchworkpp/th_seeds_v", th_seeds_v_, 0.4); node_handle_.param("/patchworkpp/th_dist_v", th_dist_v_, 0.3); node_handle_.param("/patchworkpp/max_r", max_range_, 80.0); node_handle_.param("/patchworkpp/min_r", min_range_, 2.7); node_handle_.param("/patchworkpp/uprightness_thr", uprightness_thr_, 0.5); node_handle_.param("/patchworkpp/adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); node_handle_.param("/patchworkpp/RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); node_handle_.param("/patchworkpp/RNR_intensity_thr", RNR_intensity_thr_, 0.2); node_handle_.param("/patchworkpp/max_flatness_storage", max_flatness_storage_, 1000); node_handle_.param("/patchworkpp/max_elevation_storage", max_elevation_storage_, 1000); node_handle_.param("/patchworkpp/enable_RNR", enable_RNR_, true); node_handle_.param("/patchworkpp/enable_RVPF", enable_RVPF_, true); node_handle_.param("/patchworkpp/enable_TGR", enable_TGR_, true); ROS_INFO("Sensor Height: %f", sensor_height_); ROS_INFO("Num of Iteration: %d", num_iter_); ROS_INFO("Num of LPR: %d", num_lpr_); ROS_INFO("Num of min. points: %d", num_min_pts_); ROS_INFO("Seeds Threshold: %f", th_seeds_); ROS_INFO("Distance Threshold: %f", th_dist_); ROS_INFO("Max. range:: %f", max_range_); ROS_INFO("Min. range:: %f", min_range_); ROS_INFO("Normal vector threshold: %f", uprightness_thr_); ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); // CZM denotes 'Concentric Zone Model'. Please refer to our paper node_handle_.getParam("/patchworkpp/czm/num_zones", num_zones_); node_handle_.getParam("/patchworkpp/czm/num_sectors_each_zone", num_sectors_each_zone_); node_handle_.getParam("/patchworkpp/czm/mum_rings_each_zone", num_rings_each_zone_); node_handle_.getParam("/patchworkpp/czm/elevation_thresholds", elevation_thr_); node_handle_.getParam("/patchworkpp/czm/flatness_thresholds", flatness_thr_); ROS_INFO("Num. zones: %d", num_zones_); if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) { throw invalid_argument("Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone"); } if (elevation_thr_.size() != flatness_thr_.size()) { throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); } cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % num_sectors_each_zone_[2] % num_sectors_each_zone_[3]).str() << endl; cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % num_rings_each_zone_[1] % num_rings_each_zone_[2] % num_rings_each_zone_[3]).str() << endl; cout << (boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % elevation_thr_[0] % elevation_thr_[1] % elevation_thr_[2] % elevation_thr_[3]).str() << endl; cout << (boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % flatness_thr_[0] % flatness_thr_[1] % flatness_thr_[2] % flatness_thr_[3]).str() << endl; num_rings_of_interest_ = elevation_thr_.size(); node_handle_.param("/patchworkpp/visualize", visualize_, true); int num_polygons = std::inner_product(num_rings_each_zone_.begin(), num_rings_each_zone_.end(), num_sectors_each_zone_.begin(), 0); poly_list_.header.frame_id = "map"; poly_list_.polygons.reserve(num_polygons); revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); PlaneViz = node_handle_.advertise("/patchworkpp/plane", 100, true); pub_revert_pc = node_handle_.advertise("/patchworkpp/revert_pc", 100, true); pub_reject_pc = node_handle_.advertise("/patchworkpp/reject_pc", 100, true); pub_normal = node_handle_.advertise("/patchworkpp/normals", 100, true); pub_noise = node_handle_.advertise("/patchworkpp/noise", 100, true); pub_vertical = node_handle_.advertise("/patchworkpp/vertical", 100, true); min_range_z2_ = (7 * min_range_ + max_range_) / 8.0; min_range_z3_ = (3 * min_range_ + max_range_) / 4.0; min_range_z4_ = (min_range_ + max_range_) / 2.0; min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), 2 * M_PI / num_sectors_each_zone_.at(2), 2 * M_PI / num_sectors_each_zone_.at(3)}; cout << "INITIALIZATION COMPLETE" << endl; for (int i = 0; i < num_zones_; i++) { Zone z; initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); ConcentricZoneModel_.push_back(z); } } void estimate_ground(pcl::PointCloud cloud_in, pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, double &time_taken); private: // Every private member variable is written with the undescore("_") in its end. ros::NodeHandle node_handle_; std::recursive_mutex mutex_; int num_iter_; int num_lpr_; int num_min_pts_; int num_zones_; int num_rings_of_interest_; double sensor_height_; double th_seeds_; double th_dist_; double th_seeds_v_; double th_dist_v_; double max_range_; double min_range_; double uprightness_thr_; double adaptive_seed_selection_margin_; double min_range_z2_; // 12.3625 double min_range_z3_; // 22.025 double min_range_z4_; // 41.35 double RNR_ver_angle_thr_; double RNR_intensity_thr_; bool verbose_; bool enable_RNR_; bool enable_RVPF_; bool enable_TGR_; int max_flatness_storage_, max_elevation_storage_; std::vector update_flatness_[4]; std::vector update_elevation_[4]; float d_; VectorXf normal_; MatrixXf pnormal_; VectorXf singular_values_; Eigen::Matrix3f cov_; Eigen::Vector4f pc_mean_; // For visualization bool visualize_; vector num_sectors_each_zone_; vector num_rings_each_zone_; vector sector_sizes_; vector ring_sizes_; vector min_ranges_; vector elevation_thr_; vector flatness_thr_; vector ConcentricZoneModel_; jsk_recognition_msgs::PolygonArray poly_list_; ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical; pcl::PointCloud revert_pc_, reject_pc_, noise_pc_, vertical_pc_; pcl::PointCloud ground_pc_; pcl::PointCloud normals_; pcl::PointCloud regionwise_ground_, regionwise_nonground_; void initialize_zone(Zone &z, int num_sectors, int num_rings); void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings); void flush_patches(std::vector &czm); void pc2czm(const pcl::PointCloud &src, std::vector &czm); void reflected_noise_removal(pcl::PointCloud &cloud, pcl::PointCloud &cloud_nonground); void temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, std::vector ring_flatness, std::vector> candidates, int concentric_idx); void calc_mean_stdev(std::vector vec, double &mean, double &stdev); void update_elevation_thr(); void update_flatness_thr(); double xy2theta(const double &x, const double &y); double xy2radius(const double &x, const double &y); void estimate_plane(const pcl::PointCloud &ground); void extract_piecewiseground( const int zone_idx, const pcl::PointCloud &src, pcl::PointCloud &dst, pcl::PointCloud &non_ground_dst); void extract_initial_seeds( const int zone_idx, const pcl::PointCloud &p_sorted, pcl::PointCloud &init_seeds); void extract_initial_seeds( const int zone_idx, const pcl::PointCloud &p_sorted, pcl::PointCloud &init_seeds, double th_seed); /*** * For visulization of Ground Likelihood Estimation */ geometry_msgs::PolygonStamped set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split); void set_ground_likelihood_estimation_status( const int zone_idx, const int ring_idx, const int concentric_idx, const double z_vec, const double z_elevation, const double ground_flatness); }; template inline void PatchWorkpp::initialize_zone(Zone &z, int num_sectors, int num_rings) { z.clear(); pcl::PointCloud cloud; cloud.reserve(1000); Ring ring; for (int i = 0; i < num_sectors; i++) { ring.emplace_back(cloud); } for (int j = 0; j < num_rings; j++) { z.emplace_back(ring); } } template inline void PatchWorkpp::flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings) { for (int i = 0; i < num_sectors; i++) { for (int j = 0; j < num_rings; j++) { if (!patches[j][i].points.empty()) patches[j][i].points.clear(); } } } template inline void PatchWorkpp::flush_patches(vector &czm) { for (int k = 0; k < num_zones_; k++) { for (int i = 0; i < num_rings_each_zone_[k]; i++) { for (int j = 0; j < num_sectors_each_zone_[k]; j++) { if (!czm[k][i][j].points.empty()) czm[k][i][j].points.clear(); } } } if( verbose_ ) cout << "Flushed patches" << endl; } template inline void PatchWorkpp::estimate_plane(const pcl::PointCloud &ground) { pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_); // Singular Value Decomposition: SVD Eigen::JacobiSVD svd(cov_, Eigen::DecompositionOptions::ComputeFullU); singular_values_ = svd.singularValues(); // use the least singular vector as normal normal_ = (svd.matrixU().col(2)); if (normal_(2) < 0) { for(int i=0; i<3; i++) normal_(i) *= -1; } // mean ground seeds value Eigen::Vector3f seeds_mean = pc_mean_.head<3>(); // according to normal.T*[x,y,z] = -d d_ = -(normal_.transpose() * seeds_mean)(0, 0); } template inline void PatchWorkpp::extract_initial_seeds( const int zone_idx, const pcl::PointCloud &p_sorted, pcl::PointCloud &init_seeds, double th_seed) { init_seeds.points.clear(); // LPR is the mean of low point representative double sum = 0; int cnt = 0; int init_idx = 0; if (zone_idx == 0) { for (int i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { ++init_idx; } else { break; } } } // Calculate the mean height value. for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ for (int i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lpr_height + th_seed) { init_seeds.points.push_back(p_sorted.points[i]); } } } template inline void PatchWorkpp::extract_initial_seeds( const int zone_idx, const pcl::PointCloud &p_sorted, pcl::PointCloud &init_seeds) { init_seeds.points.clear(); // LPR is the mean of low point representative double sum = 0; int cnt = 0; int init_idx = 0; if (zone_idx == 0) { for (int i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { ++init_idx; } else { break; } } } // Calculate the mean height value. for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ for (int i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lpr_height + th_seeds_) { init_seeds.points.push_back(p_sorted.points[i]); } } } template inline void PatchWorkpp::reflected_noise_removal(pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_nonground) { for (int i=0; i::min(); } } cloud_nonground += noise_pc_; if (verbose_) cout << "[ RNR ] Num of noises : " << noise_pc_.points.size() << endl; } /* @brief Velodyne pointcloud callback function. The main GPF pipeline is here. PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud ->error points removal -> extract ground seeds -> ground plane fit mainloop */ template inline void PatchWorkpp::estimate_ground( pcl::PointCloud cloud_in, pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, double &time_taken) { unique_lock lock(mutex_); poly_list_.header.stamp = ros::Time::now(); if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); static double start, t0, t1, t2, end; double pca_time_ = 0.0; double t_revert = 0.0; double t_total_ground = 0.0; double t_total_estimate = 0.0; start = ros::Time::now().toSec(); cloud_ground.clear(); cloud_nonground.clear(); // 1. Reflected Noise Removal (RNR) if (enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground); t1 = ros::Time::now().toSec(); // 2. Concentric Zone Model (CZM) flush_patches(ConcentricZoneModel_); pc2czm(cloud_in, ConcentricZoneModel_); t2 = ros::Time::now().toSec(); int concentric_idx = 0; double t_sort = 0; std::vector> candidates; std::vector ringwise_flatness; for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) { auto zone = ConcentricZoneModel_[zone_idx]; for (int ring_idx = 0; ring_idx < num_rings_each_zone_[zone_idx]; ++ring_idx) { for (int sector_idx = 0; sector_idx < num_sectors_each_zone_[zone_idx]; ++sector_idx) { if (zone[ring_idx][sector_idx].points.size() < num_min_pts_) { cloud_nonground += zone[ring_idx][sector_idx]; continue; } // --------- region-wise sorting (faster than global sorting method) ---------------- // double t_sort_0 = ros::Time::now().toSec(); sort(zone[ring_idx][sector_idx].points.begin(), zone[ring_idx][sector_idx].points.end(), point_z_cmp); double t_sort_1 = ros::Time::now().toSec(); t_sort += (t_sort_1 - t_sort_0); // ---------------------------------------------------------------------------------- // double t_tmp0 = ros::Time::now().toSec(); extract_piecewiseground(zone_idx, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_); double t_tmp1 = ros::Time::now().toSec(); t_total_ground += t_tmp1 - t_tmp0; pca_time_ += t_tmp1 - t_tmp0; // Status of each patch // used in checking uprightness, elevation, and flatness, respectively const double ground_uprightness = normal_(2); const double ground_elevation = pc_mean_(2, 0); const double ground_flatness = singular_values_.minCoeff(); const double line_variable = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits::max(); double heading = 0.0; for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i); if (visualize_) { auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3); polygons.header = poly_list_.header; poly_list_.polygons.push_back(polygons); set_ground_likelihood_estimation_status(zone_idx, ring_idx, concentric_idx, ground_uprightness, ground_elevation, ground_flatness); pcl::PointXYZINormal tmp_p; tmp_p.x = pc_mean_(0,0); tmp_p.y = pc_mean_(1,0); tmp_p.z = pc_mean_(2,0); tmp_p.normal_x = normal_(0); tmp_p.normal_y = normal_(1); tmp_p.normal_z = normal_(2); normals_.points.emplace_back(tmp_p); } double t_tmp2 = ros::Time::now().toSec(); /* About 'is_heading_outside' condidition, heading should be smaller than 0 theoretically. ( Imagine the geometric relationship between the surface normal vector on the ground plane and the vector connecting the sensor origin and the mean point of the ground plane ) However, when the patch is far awaw from the sensor origin, heading could be larger than 0 even if it's ground due to lack of amount of ground plane points. Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition ) */ bool is_upright = ground_uprightness > uprightness_thr_; bool is_not_elevated = ground_elevation < elevation_thr_[concentric_idx]; bool is_flat = ground_flatness < flatness_thr_[concentric_idx]; bool is_near_zone = concentric_idx < num_rings_of_interest_; bool is_heading_outside = heading < 0.0; /* Store the elevation & flatness variables for A-GLE (Adaptive Ground Likelihood Estimation) and TGR (Temporal Ground Revert). More information in the paper Patchwork++. */ if (is_upright && is_not_elevated && is_near_zone) { update_elevation_[concentric_idx].push_back(ground_elevation); update_flatness_[concentric_idx].push_back(ground_flatness); ringwise_flatness.push_back(ground_flatness); } // Ground estimation based on conditions if (!is_upright) { cloud_nonground += regionwise_ground_; } else if (!is_near_zone) { cloud_ground += regionwise_ground_; } else if (!is_heading_outside) { cloud_nonground += regionwise_ground_; } else if (is_not_elevated || is_flat) { cloud_ground += regionwise_ground_; } else { RevertCandidate candidate(concentric_idx, sector_idx, ground_flatness, line_variable, pc_mean_, regionwise_ground_); candidates.push_back(candidate); } // Every regionwise_nonground is considered nonground. cloud_nonground += regionwise_nonground_; double t_tmp3 = ros::Time::now().toSec(); t_total_estimate += t_tmp3 - t_tmp2; } double t_bef_revert = ros::Time::now().toSec(); if (!candidates.empty()) { if (enable_TGR_) { temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx); } else { for (size_t i=0; i inline void PatchWorkpp::update_elevation_thr(void) { for (int i=0; i 0) update_elevation_[i].erase(update_elevation_[i].begin(), update_elevation_[i].begin() + exceed_num); } if (verbose_) { cout << "sensor height: " << sensor_height_ << endl; cout << (boost::format("elevation_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") % elevation_thr_[0] % elevation_thr_[1] % elevation_thr_[2] % elevation_thr_[3]).str() << endl; } return; } template inline void PatchWorkpp::update_flatness_thr(void) { for (int i=0; i 0) update_flatness_[i].erase(update_flatness_[i].begin(), update_flatness_[i].begin() + exceed_num); } if (verbose_) { cout << (boost::format("flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") % flatness_thr_[0] % flatness_thr_[1] % flatness_thr_[2] % flatness_thr_[3]).str() << endl; } return; } template inline void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, std::vector ring_flatness, std::vector> candidates, int concentric_idx) { if (verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl; double mean_flatness = 0.0, stdev_flatness = 0.0; calc_mean_stdev(ring_flatness, mean_flatness, stdev_flatness); if (verbose_) { cout << "[" << candidates[0].concentric_idx << ", " << candidates[0].sector_idx << "]" << " mean_flatness: " << mean_flatness << ", stdev_flatness: " << stdev_flatness << std::endl; } for( size_t i=0; i candidate = candidates[i]; // Debug if(verbose_) { cout << "\033[1;33m" << candidate.sector_idx << "th flat_sector_candidate" << " / flatness: " << candidate.ground_flatness << " / line_variable: " << candidate.line_variable << " / ground_num : " << candidate.regionwise_ground.size() << "\033[0m" << endl; } double mu_flatness = mean_flatness + 1.5*stdev_flatness; double prob_flatness = 1/(1+exp( (candidate.ground_flatness-mu_flatness)/(mu_flatness/10) )); if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < th_dist_*th_dist_) prob_flatness = 1.0; double prob_line = 1.0; if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)// candidate.ground_elevation > elevation_thr_[concentric_idx]) { // if (verbose_) cout << "line_dir: " << candidate.line_dir << endl; prob_line = 0.0; } bool revert = prob_line*prob_flatness > 0.5; if ( concentric_idx < num_rings_of_interest_ ) { if (revert) { if (verbose_) { cout << "\033[1;32m" << "REVERT TRUE" << "\033[0m" << endl; } revert_pc_ += candidate.regionwise_ground; cloud_ground += candidate.regionwise_ground; } else { if (verbose_) { cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl; } reject_pc_ += candidate.regionwise_ground; cloud_nonground += candidate.regionwise_ground; } } } if (verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl; } // For adaptive template inline void PatchWorkpp::extract_piecewiseground( const int zone_idx, const pcl::PointCloud &src, pcl::PointCloud &dst, pcl::PointCloud &non_ground_dst) { // 0. Initialization if (!ground_pc_.empty()) ground_pc_.clear(); if (!dst.empty()) dst.clear(); if (!non_ground_dst.empty()) non_ground_dst.clear(); // 1. Region-wise Vertical Plane Fitting (R-VPF) // : removes potential vertical plane under the ground plane pcl::PointCloud src_wo_verticals; src_wo_verticals = src; if (enable_RVPF_) { for (int i = 0; i < num_iter_; i++) { extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_); estimate_plane(ground_pc_); if (zone_idx == 0 && normal_(2) < uprightness_thr_) { pcl::PointCloud src_tmp; src_tmp = src_wo_verticals; src_wo_verticals.clear(); Eigen::MatrixXf points(src_tmp.points.size(), 3); int j = 0; for (auto &p:src_tmp.points) { points.row(j++) << p.x, p.y, p.z; } // ground plane model Eigen::VectorXf result = points * normal_; for (int r = 0; r < result.rows(); r++) { if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) { non_ground_dst.points.push_back(src_tmp[r]); vertical_pc_.points.push_back(src_tmp[r]); } else { src_wo_verticals.points.push_back(src_tmp[r]); } } } else break; } } extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_); estimate_plane(ground_pc_); // 2. Region-wise Ground Plane Fitting (R-GPF) // : fits the ground plane //pointcloud to matrix Eigen::MatrixXf points(src_wo_verticals.points.size(), 3); int j = 0; for (auto &p:src_wo_verticals.points) { points.row(j++) << p.x, p.y, p.z; } for (int i = 0; i < num_iter_; i++) { ground_pc_.clear(); // ground plane model Eigen::VectorXf result = points * normal_; // threshold filter for (int r = 0; r < result.rows(); r++) { if (i < num_iter_ - 1) { if (result[r] < th_dist_ - d_ ) { ground_pc_.points.push_back(src_wo_verticals[r]); } } else { // Final stage if (result[r] < th_dist_ - d_ ) { dst.points.push_back(src_wo_verticals[r]); } else { non_ground_dst.points.push_back(src_wo_verticals[r]); } } } if (i < num_iter_ -1) estimate_plane(ground_pc_); else estimate_plane(dst); } } template inline geometry_msgs::PolygonStamped PatchWorkpp::set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split) { geometry_msgs::PolygonStamped polygons; // Set point of polygon. Start from RL and ccw geometry_msgs::Point32 point; // RL double zone_min_range = min_ranges_[zone_idx]; double r_len = r_idx * ring_sizes_[zone_idx] + zone_min_range; double angle = theta_idx * sector_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); // RU r_len = r_len + ring_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); // RU -> LU for (int idx = 1; idx <= num_split; ++idx) { angle = angle + sector_sizes_[zone_idx] / num_split; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); } r_len = r_len - ring_sizes_[zone_idx]; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); for (int idx = 1; idx < num_split; ++idx) { angle = angle - sector_sizes_[zone_idx] / num_split; point.x = r_len * cos(angle); point.y = r_len * sin(angle); point.z = MARKER_Z_VALUE; polygons.polygon.points.push_back(point); } return polygons; } template inline void PatchWorkpp::set_ground_likelihood_estimation_status( const int zone_idx, const int ring_idx, const int concentric_idx, const double z_vec, const double z_elevation, const double ground_flatness) { if (z_vec > uprightness_thr_) { //orthogonal if (concentric_idx < num_rings_of_interest_) { if (z_elevation > elevation_thr_[concentric_idx]) { if (flatness_thr_[concentric_idx] > ground_flatness) { poly_list_.likelihood.push_back(FLAT_ENOUGH); } else { poly_list_.likelihood.push_back(TOO_HIGH_ELEVATION); } } else { poly_list_.likelihood.push_back(UPRIGHT_ENOUGH); } } else { poly_list_.likelihood.push_back(UPRIGHT_ENOUGH); } } else { // tilted poly_list_.likelihood.push_back(TOO_TILTED); } } template inline void PatchWorkpp::calc_mean_stdev(std::vector vec, double &mean, double &stdev) { if (vec.size() <= 1) return; mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size(); for (int i=0; i inline double PatchWorkpp::xy2theta(const double &x, const double &y) { // 0 ~ 2 * PI // if (y >= 0) { // return atan2(y, x); // 1, 2 quadrant // } else { // return 2 * M_PI + atan2(y, x);// 3, 4 quadrant // } double angle = atan2(y, x); return angle > 0 ? angle : 2*M_PI+angle; } template inline double PatchWorkpp::xy2radius(const double &x, const double &y) { return sqrt(pow(x, 2) + pow(y, 2)); } template inline void PatchWorkpp::pc2czm(const pcl::PointCloud &src, std::vector &czm) { for (auto const &pt : src.points) { // int ring_idx, sector_idx; if (pt.z == std::numeric_limits::min()) continue; double r = xy2radius(pt.x, pt.y); if ((r <= max_range_) && (r > min_range_)) { double theta = xy2theta(pt.x, pt.y); int zone_idx = 0; if ( r < min_ranges_[1] ) zone_idx = 0; else if ( r < min_ranges_[2] ) zone_idx = 1; else if ( r < min_ranges_[3] ) zone_idx = 2; else zone_idx = 3; int ring_idx = min(static_cast(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1); int sector_idx = min(static_cast((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1); czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt); } } if (verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl; } #endif ================================================ FILE: include/GroundSegmentation/utils.hpp ================================================ #ifndef COMMON_H #define COMMON_H #include "math.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include // CLASSES #define SENSOR_HEIGHT 1.73 #define UNLABELED 0 #define OUTLIER 1 #define NUM_ALL_CLASSES 34 #define ROAD 40 #define PARKING 44 #define SIDEWALKR 48 #define OTHER_GROUND 49 #define BUILDING 50 #define FENSE 51 #define LANE_MARKING 60 #define VEGETATION 70 #define TERRAIN 72 #define TRUEPOSITIVE 3 #define TRUENEGATIVE 2 #define FALSEPOSITIVE 1 #define FALSENEGATIVE 0 int NUM_ZEROS = 5; using namespace std; double VEGETATION_THR = - SENSOR_HEIGHT * 3 / 4; /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */ struct PointXYZILID { PCL_ADD_POINT4D; // quad-word XYZ float intensity; ///< laser intensity reading uint16_t label; ///< point label uint16_t id; EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment } EIGEN_ALIGN16; // Register custom point struct according to PCL POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (uint16_t, label, label) (uint16_t, id, id)) void PointXYZILID2XYZI(pcl::PointCloud& src, pcl::PointCloud::Ptr dst){ dst->points.clear(); for (const auto &pt: src.points){ pcl::PointXYZI pt_xyzi; pt_xyzi.x = pt.x; pt_xyzi.y = pt.y; pt_xyzi.z = pt.z; pt_xyzi.intensity = pt.intensity; dst->points.push_back(pt_xyzi); } } std::vector outlier_classes = {UNLABELED, OUTLIER}; std::vector ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, VEGETATION, TERRAIN}; std::vector ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING}; std::vector traversable_ground_classes = {ROAD, PARKING, LANE_MARKING, OTHER_GROUND}; int count_num_ground(const pcl::PointCloud& pc){ int num_ground = 0; std::vector::iterator iter; for (auto const& pt: pc.points){ iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes if (pt.label == VEGETATION){ if (pt.z < VEGETATION_THR){ num_ground ++; } }else num_ground ++; } } return num_ground; } int count_num_ground_without_vegetation(const pcl::PointCloud& pc){ int num_ground = 0; std::vector::iterator iter; std::vector classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, TERRAIN}; for (auto const& pt: pc.points){ iter = std::find(classes.begin(), classes.end(), pt.label); if (iter != classes.end()){ // corresponding class is in ground classes num_ground ++; } } return num_ground; } std::map set_initial_gt_counts(std::vector& gt_classes){ map gt_counts; for (int i = 0; i< gt_classes.size(); ++i){ gt_counts.insert(pair(gt_classes.at(i), 0)); } return gt_counts; } std::map count_num_each_class(const pcl::PointCloud& pc){ int num_ground = 0; auto gt_counts = set_initial_gt_counts(ground_classes); std::vector::iterator iter; for (auto const& pt: pc.points){ iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes if (pt.label == VEGETATION){ if (pt.z < VEGETATION_THR){ gt_counts.find(pt.label)->second++; } }else gt_counts.find(pt.label)->second++; } } return gt_counts; } int count_num_outliers(const pcl::PointCloud& pc){ int num_outliers = 0; std::vector::iterator iter; for (auto const& pt: pc.points){ iter = std::find(outlier_classes.begin(), outlier_classes.end(), pt.label); if (iter != outlier_classes.end()){ // corresponding class is in ground classes num_outliers ++; } } return num_outliers; } void discern_ground(const pcl::PointCloud& src, pcl::PointCloud& ground, pcl::PointCloud& non_ground){ ground.clear(); non_ground.clear(); std::vector::iterator iter; for (auto const& pt: src.points){ if (pt.label == UNLABELED || pt.label == OUTLIER) continue; iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes if (pt.label == VEGETATION){ if (pt.z < VEGETATION_THR){ ground.push_back(pt); }else non_ground.push_back(pt); }else ground.push_back(pt); }else{ non_ground.push_back(pt); } } } void discern_ground_without_vegetation(const pcl::PointCloud& src, pcl::PointCloud& ground, pcl::PointCloud& non_ground){ ground.clear(); non_ground.clear(); std::vector::iterator iter; for (auto const& pt: src.points){ if (pt.label == UNLABELED || pt.label == OUTLIER) continue; iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); if (iter != ground_classes.end()){ // corresponding class is in ground classes if (pt.label != VEGETATION) ground.push_back(pt); }else{ non_ground.push_back(pt); } } } void calculate_precision_recall(const pcl::PointCloud& pc_curr, pcl::PointCloud& ground_estimated, double & precision, double& recall, bool consider_outliers=true){ int num_ground_est = ground_estimated.points.size(); int num_ground_gt = count_num_ground(pc_curr); int num_TP = count_num_ground(ground_estimated); if (consider_outliers){ int num_outliers_est = count_num_outliers(ground_estimated); precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; recall = (double)(num_TP)/num_ground_gt * 100; }else{ precision = (double)(num_TP)/num_ground_est * 100; recall = (double)(num_TP)/num_ground_gt * 100; } } void calculate_precision_recall_without_vegetation(const pcl::PointCloud& pc_curr, pcl::PointCloud& ground_estimated, double & precision, double& recall, bool consider_outliers=true){ int num_veg = 0; for (auto const& pt: ground_estimated.points) { if (pt.label == VEGETATION) num_veg++; } int num_ground_est = ground_estimated.size() - num_veg; int num_ground_gt = count_num_ground_without_vegetation(pc_curr); int num_TP = count_num_ground_without_vegetation(ground_estimated); if (consider_outliers){ int num_outliers_est = count_num_outliers(ground_estimated); precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; recall = (double)(num_TP)/num_ground_gt * 100; }else{ precision = (double)(num_TP)/num_ground_est * 100; recall = (double)(num_TP)/num_ground_gt * 100; } } int save_all_labels(const pcl::PointCloud& pc, string ABS_DIR, string seq, int count){ std::string count_str = std::to_string(count); std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; std::string output_filename = ABS_DIR + "/" + seq + "/" + count_str_padded + ".csv"; ofstream sc_output(output_filename); vector labels(NUM_ALL_CLASSES, 0); for (auto const& pt: pc.points){ if (pt.label == 0) labels[0]++; else if (pt.label == 1) labels[1]++; else if (pt.label == 10) labels[2]++; else if (pt.label == 11) labels[3]++; else if (pt.label == 13) labels[4]++; else if (pt.label == 15) labels[5]++; else if (pt.label == 16) labels[6]++; else if (pt.label == 18) labels[7]++; else if (pt.label == 20) labels[8]++; else if (pt.label == 30) labels[9]++; else if (pt.label == 31) labels[10]++; else if (pt.label == 32) labels[11]++; else if (pt.label == 40) labels[12]++; else if (pt.label == 44) labels[13]++; else if (pt.label == 48) labels[14]++; else if (pt.label == 49) labels[15]++; else if (pt.label == 50) labels[16]++; else if (pt.label == 51) labels[17]++; else if (pt.label == 52) labels[18]++; else if (pt.label == 60) labels[19]++; else if (pt.label == 70) labels[20]++; else if (pt.label == 71) labels[21]++; else if (pt.label == 72) labels[22]++; else if (pt.label == 80) labels[23]++; else if (pt.label == 81) labels[24]++; else if (pt.label == 99) labels[25]++; else if (pt.label == 252) labels[26]++; else if (pt.label == 253) labels[27]++; else if (pt.label == 254) labels[28]++; else if (pt.label == 255) labels[29]++; else if (pt.label == 256) labels[30]++; else if (pt.label == 257) labels[31]++; else if (pt.label == 258) labels[32]++; else if (pt.label == 259) labels[33]++; } for (uint8_t i=0; i < NUM_ALL_CLASSES;++i){ if (i!=33){ sc_output<& pc_curr, pcl::PointCloud& ground_estimated, string acc_filename, double& accuracy, map&pc_curr_gt_counts, map&g_est_gt_counts){ // std::cout<<"debug: "<(num_TP + (num_False - num_FP)) / num_total_gt * 100.0; pc_curr_gt_counts = count_num_each_class(pc_curr); g_est_gt_counts = count_num_each_class(ground_estimated); // save output for (auto const& class_id: ground_classes){ sc_output2<second<<","<second<<","; } sc_output2<& TP, const pcl::PointCloud& FP, const pcl::PointCloud& FN, const pcl::PointCloud& TN, std::string pcd_filename){ pcl::PointCloud pc_out; for (auto const pt: TP.points){ pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = TRUEPOSITIVE; pc_out.points.push_back(pt_est); } for (auto const pt: FP.points){ pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = FALSEPOSITIVE; pc_out.points.push_back(pt_est); } for (auto const pt: FN.points){ pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = FALSENEGATIVE; pc_out.points.push_back(pt_est); } for (auto const pt: TN.points){ pcl::PointXYZI pt_est; pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; pt_est.intensity = TRUENEGATIVE; pc_out.points.push_back(pt_est); } pc_out.width = pc_out.points.size(); pc_out.height = 1; pcl::io::savePCDFileASCII(pcd_filename, pc_out); } #endif ================================================ FILE: include/IMU_Processing.hpp ================================================ #ifndef _IMU_PROCESSING_HPP #define _IMU_PROCESSING_HPP #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /// *************Preconfiguration #define MAX_INI_COUNT (200) const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; /// *************IMU Process and undistortion class ImuProcess { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImuProcess(); ~ImuProcess(); void Reset(); void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); void set_R_LI_cov(const V3D &R_LI_cov); void set_T_LI_cov(const V3D &T_LI_cov); void set_gyr_cov(const V3D &scaler); void set_acc_cov(const V3D &scaler); void set_mean_acc_norm(const double &mean_acc_norm); void set_gyr_bias_cov(const V3D &b_g); void set_acc_bias_cov(const V3D &b_a); void Process(const MeasureGroup &meas, StatesGroup &state, PointCloudXYZI::Ptr pcl_un_); // ros::NodeHandle nh; ofstream fout_imu; V3D cov_acc; V3D cov_gyr; V3D cov_R_LI; V3D cov_T_LI; V3D cov_acc_scale; V3D cov_gyr_scale; V3D cov_bias_gyr; V3D cov_bias_acc; double first_lidar_time; int lidar_type; bool imu_en; bool Gril_Calib_done = false; double IMU_mean_acc_norm; private: void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N); void propagation_and_undist(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_in_out); void Forward_propagation_without_imu(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); PointCloudXYZI::Ptr cur_pcl_un_; sensor_msgs::ImuConstPtr last_imu_; deque v_imu_; vector IMUpose; V3D mean_acc; V3D mean_gyr; V3D angvel_last; V3D acc_s_last; double last_lidar_end_time_; double time_last_scan; int init_iter_num = 1; bool b_first_frame_ = true; bool imu_need_init_ = true; }; ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true) { imu_en = true; init_iter_num = 1; cov_acc = V3D(0.1, 0.1, 0.1); cov_gyr = V3D(0.1, 0.1, 0.1); cov_R_LI = V3D(0.00001, 0.00001, 0.00001); cov_T_LI = V3D(0.0001, 0.0001, 0.0001); cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; last_imu_.reset(new sensor_msgs::Imu()); fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out); } ImuProcess::~ImuProcess() {} void ImuProcess::Reset() { ROS_WARN("Reset ImuProcess"); mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; imu_need_init_ = true; init_iter_num = 1; v_imu_.clear(); IMUpose.clear(); last_imu_.reset(new sensor_msgs::Imu()); cur_pcl_un_.reset(new PointCloudXYZI()); } void ImuProcess::set_gyr_cov(const V3D &scaler) { cov_gyr_scale = scaler; } void ImuProcess::set_acc_cov(const V3D &scaler) { cov_acc_scale = scaler; } void ImuProcess::set_R_LI_cov(const V3D &R_LI_cov) { cov_R_LI = R_LI_cov; } void ImuProcess::set_T_LI_cov(const V3D &T_LI_cov) { cov_T_LI = T_LI_cov; } void ImuProcess::set_mean_acc_norm(const double &mean_acc_norm){ IMU_mean_acc_norm = mean_acc_norm; } void ImuProcess::set_gyr_bias_cov(const V3D &b_g) { cov_bias_gyr = b_g; } void ImuProcess::set_acc_bias_cov(const V3D &b_a) { cov_bias_acc = b_a; } void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, int &N) { /** 1. initializing the gravity, gyro bias, acc and gyro covariance ** 2. normalize the acceleration measurements to unit gravity **/ ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); V3D cur_acc, cur_gyr; if (b_first_frame_) { Reset(); N = 1; b_first_frame_ = false; const auto &imu_acc = meas.imu.front()->linear_acceleration; const auto &gyr_acc = meas.imu.front()->angular_velocity; mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; first_lidar_time = meas.lidar_beg_time; } for (const auto &imu : meas.imu) { const auto &imu_acc = imu->linear_acceleration; const auto &gyr_acc = imu->angular_velocity; cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; mean_acc += (cur_acc - mean_acc) / N; mean_gyr += (cur_gyr - mean_gyr) / N; cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (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 ++; } state_inout.gravity = - mean_acc / mean_acc.norm() * G_m_s2; state_inout.rot_end = Eye3d; state_inout.bias_g.setZero(); last_imu_ = meas.imu.back(); } void ImuProcess::Forward_propagation_without_imu(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) { pcl_out = *(meas.lidar); /*** sort point clouds by offset time ***/ const double &pcl_beg_time = meas.lidar_beg_time; sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); const double &pcl_end_offset_time = pcl_out.points.back().curvature / double(1000); MD(DIM_STATE, DIM_STATE) F_x, cov_w; double dt = 0.0; if (b_first_frame_) { dt = 0.1; b_first_frame_ = false; } else { dt = pcl_beg_time - time_last_scan; time_last_scan = pcl_beg_time; } /* covariance propagation */ F_x.setIdentity(); cov_w.setZero(); /** In CV model, bias_g represents angular velocity **/ /** In CV model，bias_a represents linear acceleration **/ M3D Exp_f = Exp(state_inout.bias_g, dt); F_x.block<3, 3>(0, 0) = Exp(state_inout.bias_g, -dt); F_x.block<3, 3>(0, 15) = Eye3d * dt; F_x.block<3, 3>(3, 12) = Eye3d * dt; cov_w.block<3, 3>(15, 15).diagonal() = cov_gyr_scale * dt * dt; cov_w.block<3, 3>(12, 12).diagonal() = cov_acc_scale * dt * dt; /** Forward propagation of covariance**/ state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; /** Forward propagation of attitude **/ state_inout.rot_end = state_inout.rot_end * Exp_f; /** Position Propagation **/ state_inout.pos_end += state_inout.vel_end * dt; /**CV model： un-distort pcl using linear interpolation **/ if(lidar_type != L515){ auto it_pcl = pcl_out.points.end() - 1; double dt_j = 0.0; for(; it_pcl != pcl_out.points.begin(); it_pcl --) { dt_j= pcl_end_offset_time - it_pcl->curvature/double(1000); M3D R_jk(Exp(state_inout.bias_g, - dt_j)); V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z); // Using rotation and translation to un-distort points V3D p_jk; p_jk = - state_inout.rot_end.transpose() * state_inout.vel_end * dt_j; V3D P_compensate = R_jk * P_j + p_jk; /// save Undistorted points and their rotation it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2); } } } void ImuProcess::propagation_and_undist(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) { /*** add the imu of the last frame-tail to the current frame-head ***/ pcl_out = *(meas.lidar); auto v_imu = meas.imu; v_imu.push_front(last_imu_); double imu_end_time = v_imu.back()->header.stamp.toSec(); double pcl_beg_time, pcl_end_time; if (lidar_type == L515) { pcl_beg_time = last_lidar_end_time_; pcl_end_time = meas.lidar_beg_time; } else { pcl_beg_time = meas.lidar_beg_time; /*** sort point clouds by offset time ***/ sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); } /*** Initialize IMU pose ***/ IMUpose.clear(); IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end)); /*** forward propagation at each imu point ***/ V3D acc_imu, angvel_avr, acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end); M3D R_imu(state_inout.rot_end); MD(DIM_STATE, DIM_STATE) F_x, cov_w; double dt = 0; for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++) { auto &&head = *(it_imu); auto &&tail = *(it_imu + 1); if (tail->header.stamp.toSec() < last_lidar_end_time_) continue; angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); V3D angvel_now(head->angular_velocity.x, head->angular_velocity.y, head->angular_velocity.z); V3D acc_now(head->linear_acceleration.x, head->linear_acceleration.y, head->linear_acceleration.z); fout_imu << setw(10) << head->header.stamp.toSec() << " " << angvel_now.transpose()<< " " << acc_now.transpose() << endl; angvel_avr -= state_inout.bias_g; acc_avr = acc_avr / IMU_mean_acc_norm * G_m_s2 - state_inout.bias_a; if(head->header.stamp.toSec() < last_lidar_end_time_) dt = tail->header.stamp.toSec() - last_lidar_end_time_; else dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); /* covariance propagation */ M3D acc_avr_skew; M3D Exp_f = Exp(angvel_avr, dt); acc_avr_skew<(0,0) = Exp(angvel_avr, -dt); F_x.block<3,3>(0,15) = - Eye3d * dt; F_x.block<3,3>(3,12) = Eye3d * dt; F_x.block<3,3>(12,0) = - R_imu * acc_avr_skew * dt; F_x.block<3,3>(12,18) = - R_imu * dt; F_x.block<3,3>(12,21) = Eye3d * dt; cov_w.block<3,3>(0,0).diagonal() = cov_gyr * dt * dt; cov_w.block<3,3>(6,6).diagonal() = cov_R_LI * dt * dt; cov_w.block<3,3>(9,9).diagonal() = cov_T_LI * dt * dt; cov_w.block<3,3>(12,12) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt; cov_w.block<3,3>(15,15).diagonal() = cov_bias_gyr * dt * dt; // bias gyro covariance cov_w.block<3,3>(18,18).diagonal() = cov_bias_acc * dt * dt; // bias acc covariance state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; /* propagation of IMU attitude (global frame)*/ R_imu = R_imu * Exp_f; /* Specific acceleration (global frame) of IMU */ acc_imu = R_imu * acc_avr + state_inout.gravity; /* propagation of IMU position (global frame)*/ pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; /* velocity of IMU (global frame)*/ vel_imu = vel_imu + acc_imu * dt; /* save the poses at each IMU measurements (global frame)*/ angvel_last = angvel_avr; acc_s_last = acc_imu; double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu)); } /*** calculated the pos and attitude prediction at the frame-end ***/ double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; dt = note * (pcl_end_time - imu_end_time); state_inout.vel_end = vel_imu + note * acc_imu * dt; state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt); state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 * acc_imu * dt * dt; last_imu_ = meas.imu.back(); last_lidar_end_time_ = pcl_end_time; if (lidar_type != L515) { auto pos_liD_e = state_inout.pos_end + state_inout.rot_end * state_inout.offset_T_L_I; // auto R_liD_e = state_inout.rot_end * Lidar_R_to_IMU; #ifdef DEBUG_PRINT cout<<"[ IMU Process ]: vel "<rot); acc_imu<acc); vel_imu<vel); pos_imu<pos); angvel_avr<gyr); for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) { dt = it_pcl->curvature / double(1000) - head->offset_time; //dt = t_j - t_i > 0 /* Transform to the 'scan-end' IMU frame（I_k frame）using only rotation * Note: Compensation direction is INVERSE of Frame's moving direction * So if we want to compensate a point at timestamp-i to the frame-e * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ M3D R_i(R_imu * Exp(angvel_avr, dt)); 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); V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); V3D P_compensate = state_inout.rot_end.transpose() * (R_i * P_i + T_ei); /// save Undistorted points and their rotation it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2); if (it_pcl == pcl_out.points.begin()) break; } } } } void ImuProcess::Process(const MeasureGroup &meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_) { if (imu_en) { if(meas.imu.empty()) return; ROS_ASSERT(meas.lidar != nullptr); if (imu_need_init_) { if(!Gril_Calib_done){ /// The very first lidar frame IMU_init(meas, stat, init_iter_num); imu_need_init_ = true; last_imu_ = meas.imu.back(); if (init_iter_num > MAX_INI_COUNT) { cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); imu_need_init_ = false; cov_acc = cov_acc_scale; cov_gyr = cov_gyr_scale; ROS_INFO("IMU Initialization Done: Gravity: %.4f %.4f %.4f, Acc norm: %.4f", stat.gravity[0], stat.gravity[1], stat.gravity[2], mean_acc.norm()); IMU_mean_acc_norm = mean_acc.norm(); } } else{ cout << endl; printf(BOLDMAGENTA "[Refinement] Switch to LIO mode, online refinement begins.\n\n" RESET); last_imu_ = meas.imu.back(); imu_need_init_ = false; cov_acc = cov_acc_scale; cov_gyr = cov_gyr_scale; } return; } // propagation_and_undist(meas, stat, *cur_pcl_un_); } else { Forward_propagation_without_imu(meas, stat, *cur_pcl_un_); } } #endif ================================================ FILE: include/color.h ================================================ #ifndef COLOR_H #define COLOR_H #define RESET "\033[0m" #define BLACK "\033[30m" /* Black */ #define RED "\033[31m" /* Red */ #define GREEN "\033[32m" /* Green */ #define YELLOW "\033[33m" /* Yellow */ #define BLUE "\033[34m" /* Blue */ #define MAGENTA "\033[35m" /* Magenta */ #define CYAN "\033[36m" /* Cyan */ #define WHITE "\033[37m" /* White */ #define REDPURPLE "\033[95m" /* Red Purple */ #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ #define BOLDRED "\033[1m\033[31m" /* Bold Red */ #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ #define BOLDREDPURPLE "\033[1m\033[95m" /* Bold Red Purple */ #endif ================================================ FILE: include/common_lib.h ================================================ #ifndef COMMON_LIB_H #define COMMON_LIB_H #include #include #include #include #include #include #include #include #include #include #include #include // For ImageProjection.hpp #include #include #include #include using namespace std; using namespace Eigen; #define PBSTR "||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||" #define PBWIDTH 30 #define PI_M (3.14159265358) #define G_m_s2 (9.81) // Gravity const in GuangDong/China #define DIM_STATE (24) // Dimension of states (Let Dim(SO(3)) = 3) #define LIDAR_SP_LEN (2) #define INIT_COV (1) #define NUM_MATCH_POINTS (5) #define VEC_FROM_ARRAY(v) v[0],v[1],v[2] #define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) #define RESULT_FILE_DIR(name) (string(string(ROOT_DIR) + "result/"+ name)) typedef gril_calib::Pose6D Pose6D; typedef pcl::PointXYZINormal PointType; typedef pcl::PointXYZRGB PointTypeRGB; typedef pcl::PointCloud PointCloudXYZI; typedef pcl::PointCloud PointCloudXYZRGB; typedef vector> PointVector; typedef Vector3d V3D; typedef Matrix3d M3D; typedef Vector3f V3F; typedef Vector2d V2D; typedef Matrix2d M2D; #define MD(a,b) Matrix #define VD(a) Matrix const M3D Eye3d(M3D::Identity()); const V3D Zero3d(0, 0, 0); // Vector3d Lidar_offset_to_IMU(0.05512, 0.02226, -0.0297); // Horizon // Vector3d Lidar_offset_to_IMU(0.04165, 0.02326, -0.0284); // Avia enum LID_TYPE{AVIA = 1, VELO, OUSTER, L515, PANDAR, VELO_NCLT, VELO_without_Time}; //{1, 2, 3} struct MeasureGroup // Lidar data and imu dates for the curent process { MeasureGroup() { lidar_beg_time = 0.0; this->lidar.reset(new PointCloudXYZI()); }; double lidar_beg_time; PointCloudXYZI::Ptr lidar; deque imu; }; struct StatesGroup { StatesGroup() { this->rot_end = M3D::Identity(); this->pos_end = Zero3d; this->offset_R_L_I = M3D::Identity(); this->offset_T_L_I = Zero3d; this->vel_end = Zero3d; this->bias_g = Zero3d; this->bias_a = Zero3d; this->gravity = Zero3d; this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV; this->cov.block<9,9>(15,15) = MD(9,9)::Identity() * 0.00001; }; StatesGroup(const StatesGroup& b) { this->rot_end = b.rot_end; this->pos_end = b.pos_end; this->offset_R_L_I = b.offset_R_L_I; this->offset_T_L_I = b.offset_T_L_I; this->vel_end = b.vel_end; this->bias_g = b.bias_g; this->bias_a = b.bias_a; this->gravity = b.gravity; this->cov = b.cov; }; StatesGroup& operator=(const StatesGroup& b) { this->rot_end = b.rot_end; this->pos_end = b.pos_end; this->offset_R_L_I = b.offset_R_L_I; this->offset_T_L_I = b.offset_T_L_I; this->vel_end = b.vel_end; this->bias_g = b.bias_g; this->bias_a = b.bias_a; this->gravity = b.gravity; this->cov = b.cov; return *this; }; StatesGroup operator+(const Matrix &state_add) { StatesGroup a; a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); a.pos_end = this->pos_end + state_add.block<3,1>(3,0); a.offset_R_L_I = this->offset_R_L_I * Exp(state_add(6,0), state_add(7,0), state_add(8,0)); a.offset_T_L_I = this->offset_T_L_I + state_add.block<3,1>(9,0); a.vel_end = this->vel_end + state_add.block<3,1>(12,0); a.bias_g = this->bias_g + state_add.block<3,1>(15,0); a.bias_a = this->bias_a + state_add.block<3,1>(18,0); a.gravity = this->gravity + state_add.block<3,1>(21,0); a.cov = this->cov; return a; }; StatesGroup& operator+=(const Matrix &state_add) { this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); this->pos_end += state_add.block<3,1>(3,0); this->offset_R_L_I = this->offset_R_L_I * Exp(state_add(6,0), state_add(7,0), state_add(8,0)); this->offset_T_L_I += state_add.block<3,1>(9,0); this->vel_end += state_add.block<3,1>(12,0); this->bias_g += state_add.block<3,1>(15,0); this->bias_a += state_add.block<3,1>(18,0); this->gravity += state_add.block<3,1>(21,0); return *this; }; Matrix operator-(const StatesGroup& b) { Matrix a; M3D rotd(b.rot_end.transpose() * this->rot_end); a.block<3,1>(0,0) = Log(rotd); a.block<3,1>(3,0) = this->pos_end - b.pos_end; M3D offsetd(b.offset_R_L_I.transpose() * this->offset_R_L_I); a.block<3,1>(6,0) = Log(offsetd); a.block<3,1>(9,0) = this->offset_T_L_I - b.offset_T_L_I; a.block<3,1>(12,0) = this->vel_end - b.vel_end; a.block<3,1>(15,0) = this->bias_g - b.bias_g; a.block<3,1>(18,0) = this->bias_a - b.bias_a; a.block<3,1>(21,0) = this->gravity - b.gravity; return a; }; void resetpose() { this->rot_end = M3D::Identity(); this->pos_end = Zero3d; this->vel_end = Zero3d; } M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point V3D pos_end; // the estimated position at the end lidar point (world frame) M3D offset_R_L_I; // Rotation from Lidar frame L to IMU frame I V3D offset_T_L_I; // Translation from Lidar frame L to IMU frame I (from IMU to LiDAR) V3D vel_end; // the estimated velocity at the end lidar point (world frame) V3D bias_g; // gyroscope bias V3D bias_a; // accelerator bias V3D gravity; // the estimated gravity acceleration Matrix cov; // states covariance }; template T rad2deg(T radians) { return radians * 180.0 / PI_M; } template T deg2rad(T degrees) { return degrees * PI_M / 180.0; } // Taeyoung Add // Convert SI unit to g unit template T SI2g(T SI) { return SI / G_m_s2; } // Convert g unit to SI unit template T g2SI(T g) { return g * G_m_s2; } // So(2) rotation matrix // theta - radian template M2D SO2(T theta) { M2D R; R << std::cos(theta), -std::sin(theta), std::sin(theta), std::cos(theta); return R; } // Skew matrix 2D // theta - radian template M2D Skew2D(T theta) { M2D exp; exp << 0, -theta, theta, 0; return exp; } template auto set_pose6d(const double t, const Matrix &a, const Matrix &g, \ const Matrix &v, const Matrix &p, const Matrix &R) { Pose6D rot_kp; rot_kp.offset_time = t; for (int i = 0; i < 3; i++) { rot_kp.acc[i] = a(i); rot_kp.gyr[i] = g(i); rot_kp.vel[i] = v(i); rot_kp.pos[i] = p(i); for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); } // Map(rot_kp.rot, 3,3) = R; return move(rot_kp); } static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R){ Eigen::Vector3d n = R.col(0); Eigen::Vector3d o = R.col(1); Eigen::Vector3d a = R.col(2); Eigen::Vector3d ypr(3); double y = atan2(n(1), n(0)); double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); ypr(0) = y; ypr(1) = p; ypr(2) = r; return ypr / M_PI * 180.0; } template static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) { typedef typename Derived::Scalar Scalar_t; Scalar_t y = ypr(0) / 180.0 * M_PI; Scalar_t p = ypr(1) / 180.0 * M_PI; Scalar_t r = ypr(2) / 180.0 * M_PI; Eigen::Matrix Rz; Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1; Eigen::Matrix Ry; Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p); Eigen::Matrix Rx; Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r); return Rz * Ry * Rx; } /* comment plane equation: Ax + By + Cz + D = 0 convert to: A/D*x + B/D*y + C/D*z = -1 solve: A0*x0 = b0 where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T normvec: normalized x0 */ template bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) { MatrixXf A(point_num, 3); MatrixXf b(point_num, 1); b.setOnes(); b *= -1.0f; for (int j = 0; j < point_num; j++) { A(j,0) = point[j].x; A(j,1) = point[j].y; A(j,2) = point[j].z; } normvec = A.colPivHouseholderQr().solve(b); for (int j = 0; j < point_num; j++) { if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) { return false; } } normvec.normalize(); return true; } template bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) { Matrix A; Matrix b; A.setZero(); b.setOnes(); b *= -1.0f; for (int j = 0; j < NUM_MATCH_POINTS; j++) { A(j,0) = point[j].x; A(j,1) = point[j].y; A(j,2) = point[j].z; } Matrix normvec = A.colPivHouseholderQr().solve(b); T n = normvec.norm(); pca_result(0) = normvec(0) / n; pca_result(1) = normvec(1) / n; pca_result(2) = normvec(2) / n; pca_result(3) = 1.0 / n; for (int j = 0; j < NUM_MATCH_POINTS; j++) { if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) { return false; } } return true; } #endif ================================================ FILE: include/ikd-Tree/README.md ================================================ # ikd-Tree ikd-Tree is an incremental k-d tree for robotic applications. ================================================ FILE: include/ikd-Tree/ikd_Tree.cpp ================================================ #include "ikd_Tree.h" /* Description: ikd-Tree: an incremental k-d tree for robotic applications Author: Yixi Cai email: yixicai@connect.hku.hk */ KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) { delete_criterion_param = delete_param; balance_criterion_param = balance_param; downsample_size = box_length; Rebuild_Logger.clear(); termination_flag = false; start_thread(); } KD_TREE::~KD_TREE() { stop_thread(); Delete_Storage_Disabled = true; delete_tree_nodes(&Root_Node); PointVector ().swap(PCL_Storage); Rebuild_Logger.clear(); } void KD_TREE::Set_delete_criterion_param(float delete_param){ delete_criterion_param = delete_param; } void KD_TREE::Set_balance_criterion_param(float balance_param){ balance_criterion_param = balance_param; } void KD_TREE::set_downsample_param(float downsample_param){ downsample_size = downsample_param; } void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length){ Set_delete_criterion_param(delete_param); Set_balance_criterion_param(balance_param); set_downsample_param(box_length); } void KD_TREE::InitTreeNode(KD_TREE_NODE * root){ root->point.x = 0.0f; root->point.y = 0.0f; root->point.z = 0.0f; root->node_range_x[0] = 0.0f; root->node_range_x[1] = 0.0f; root->node_range_y[0] = 0.0f; root->node_range_y[1] = 0.0f; root->node_range_z[0] = 0.0f; root->node_range_z[1] = 0.0f; root->division_axis = 0; root->father_ptr = nullptr; root->left_son_ptr = nullptr; root->right_son_ptr = nullptr; root->TreeSize = 0; root->invalid_point_num = 0; root->down_del_num = 0; root->point_deleted = false; root->tree_deleted = false; root->need_push_down_to_left = false; root->need_push_down_to_right = false; root->point_downsample_deleted = false; root->working_flag = false; pthread_mutex_init(&(root->push_down_mutex_lock),NULL); } int KD_TREE::size(){ int s = 0; if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ if (Root_Node != nullptr) { return Root_Node->TreeSize; } else { return 0; } } else { if (!pthread_mutex_trylock(&working_flag_mutex)){ s = Root_Node->TreeSize; pthread_mutex_unlock(&working_flag_mutex); return s; } else { return Treesize_tmp; } } } BoxPointType KD_TREE::tree_range(){ BoxPointType range; if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ if (Root_Node != nullptr) { range.vertex_min[0] = Root_Node->node_range_x[0]; range.vertex_min[1] = Root_Node->node_range_y[0]; range.vertex_min[2] = Root_Node->node_range_z[0]; range.vertex_max[0] = Root_Node->node_range_x[1]; range.vertex_max[1] = Root_Node->node_range_y[1]; range.vertex_max[2] = Root_Node->node_range_z[1]; } else { memset(&range, 0, sizeof(range)); } } else { if (!pthread_mutex_trylock(&working_flag_mutex)){ range.vertex_min[0] = Root_Node->node_range_x[0]; range.vertex_min[1] = Root_Node->node_range_y[0]; range.vertex_min[2] = Root_Node->node_range_z[0]; range.vertex_max[0] = Root_Node->node_range_x[1]; range.vertex_max[1] = Root_Node->node_range_y[1]; range.vertex_max[2] = Root_Node->node_range_z[1]; pthread_mutex_unlock(&working_flag_mutex); } else { memset(&range, 0, sizeof(range)); } } return range; } int KD_TREE::validnum(){ int s = 0; if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ if (Root_Node != nullptr) return (Root_Node->TreeSize - Root_Node->invalid_point_num); else return 0; } else { if (!pthread_mutex_trylock(&working_flag_mutex)){ s = Root_Node->TreeSize-Root_Node->invalid_point_num; pthread_mutex_unlock(&working_flag_mutex); return s; } else { return -1; } } } void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del){ if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ alpha_bal = Root_Node->alpha_bal; alpha_del = Root_Node->alpha_del; return; } else { if (!pthread_mutex_trylock(&working_flag_mutex)){ alpha_bal = Root_Node->alpha_bal; alpha_del = Root_Node->alpha_del; pthread_mutex_unlock(&working_flag_mutex); return; } else { alpha_bal = alpha_bal_tmp; alpha_del = alpha_del_tmp; return; } } } void KD_TREE::start_thread(){ pthread_mutex_init(&termination_flag_mutex_lock, NULL); pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); pthread_mutex_init(&working_flag_mutex, NULL); pthread_mutex_init(&search_flag_mutex, NULL); pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void*) this); printf("Multi thread started \n"); } void KD_TREE::stop_thread(){ pthread_mutex_lock(&termination_flag_mutex_lock); termination_flag = true; pthread_mutex_unlock(&termination_flag_mutex_lock); if (rebuild_thread) pthread_join(rebuild_thread, NULL); pthread_mutex_destroy(&termination_flag_mutex_lock); pthread_mutex_destroy(&rebuild_logger_mutex_lock); pthread_mutex_destroy(&rebuild_ptr_mutex_lock); pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); pthread_mutex_destroy(&working_flag_mutex); pthread_mutex_destroy(&search_flag_mutex); } void * KD_TREE::multi_thread_ptr(void * arg){ KD_TREE * handle = (KD_TREE*) arg; handle->multi_thread_rebuild(); return nullptr; } void KD_TREE::multi_thread_rebuild(){ bool terminated = false; KD_TREE_NODE * father_ptr, ** new_node_ptr; pthread_mutex_lock(&termination_flag_mutex_lock); terminated = termination_flag; pthread_mutex_unlock(&termination_flag_mutex_lock); while (!terminated){ pthread_mutex_lock(&rebuild_ptr_mutex_lock); pthread_mutex_lock(&working_flag_mutex); if (Rebuild_Ptr != nullptr ){ /* Traverse and copy */ if (!Rebuild_Logger.empty()){ printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); } rebuild_flag = true; if (*Rebuild_Ptr == Root_Node) { Treesize_tmp = Root_Node->TreeSize; Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; alpha_bal_tmp = Root_Node->alpha_bal; alpha_del_tmp = Root_Node->alpha_del; } KD_TREE_NODE * old_root_node = (*Rebuild_Ptr); father_ptr = (*Rebuild_Ptr)->father_ptr; PointVector ().swap(Rebuild_PCL_Storage); // Lock Search pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter != 0){ pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter = -1; pthread_mutex_unlock(&search_flag_mutex); // Lock deleted points cache pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); // Unlock deleted points cache pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); // Unlock Search pthread_mutex_lock(&search_flag_mutex); search_mutex_counter = 0; pthread_mutex_unlock(&search_flag_mutex); pthread_mutex_unlock(&working_flag_mutex); /* Rebuild and update missed operations*/ Operation_Logger_Type Operation; KD_TREE_NODE * new_root_node = nullptr; if (int(Rebuild_PCL_Storage.size()) > 0){ BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size()-1, Rebuild_PCL_Storage); // Rebuild has been done. Updates the blocked operations into the new tree pthread_mutex_lock(&working_flag_mutex); pthread_mutex_lock(&rebuild_logger_mutex_lock); int tmp_counter = 0; while (!Rebuild_Logger.empty()){ Operation = Rebuild_Logger.front(); max_queue_size = max(max_queue_size, Rebuild_Logger.size()); Rebuild_Logger.pop(); pthread_mutex_unlock(&rebuild_logger_mutex_lock); pthread_mutex_unlock(&working_flag_mutex); run_operation(&new_root_node, Operation); tmp_counter ++; if (tmp_counter % 10 == 0) usleep(1); pthread_mutex_lock(&working_flag_mutex); pthread_mutex_lock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&rebuild_logger_mutex_lock); } /* Replace to original tree*/ // pthread_mutex_lock(&working_flag_mutex); pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter != 0){ pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter = -1; pthread_mutex_unlock(&search_flag_mutex); if (father_ptr->left_son_ptr == *Rebuild_Ptr) { father_ptr->left_son_ptr = new_root_node; } else if (father_ptr->right_son_ptr == *Rebuild_Ptr){ father_ptr->right_son_ptr = new_root_node; } else { throw "Error: Father ptr incompatible with current node\n"; } if (new_root_node != nullptr) new_root_node->father_ptr = father_ptr; (*Rebuild_Ptr) = new_root_node; int valid_old = old_root_node->TreeSize-old_root_node->invalid_point_num; int valid_new = new_root_node->TreeSize-new_root_node->invalid_point_num; if (father_ptr == STATIC_ROOT_NODE) Root_Node = STATIC_ROOT_NODE->left_son_ptr; KD_TREE_NODE * update_root = *Rebuild_Ptr; while (update_root != nullptr && update_root != Root_Node){ update_root = update_root->father_ptr; if (update_root->working_flag) break; if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) break; if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) break; Update(update_root); } pthread_mutex_lock(&search_flag_mutex); search_mutex_counter = 0; pthread_mutex_unlock(&search_flag_mutex); Rebuild_Ptr = nullptr; pthread_mutex_unlock(&working_flag_mutex); rebuild_flag = false; /* Delete discarded tree nodes */ delete_tree_nodes(&old_root_node); } else { pthread_mutex_unlock(&working_flag_mutex); } pthread_mutex_unlock(&rebuild_ptr_mutex_lock); pthread_mutex_lock(&termination_flag_mutex_lock); terminated = termination_flag; pthread_mutex_unlock(&termination_flag_mutex_lock); usleep(100); } printf("Rebuild thread terminated normally\n"); } void KD_TREE::run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation){ switch (operation.op) { case ADD_POINT: Add_by_point(root, operation.point, false, (*root)->division_axis); break; case ADD_BOX: Add_by_range(root, operation.boxpoint, false); break; case DELETE_POINT: Delete_by_point(root, operation.point, false); break; case DELETE_BOX: Delete_by_range(root, operation.boxpoint, false, false); break; case DOWNSAMPLE_DELETE: Delete_by_range(root, operation.boxpoint, false, true); break; case PUSH_DOWN: (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; if (operation.tree_downsample_deleted) (*root)->down_del_num = (*root)->TreeSize; if (operation.tree_deleted) (*root)->invalid_point_num = (*root)->TreeSize; else (*root)->invalid_point_num = (*root)->down_del_num; (*root)->need_push_down_to_left = true; (*root)->need_push_down_to_right = true; break; default: break; } } void KD_TREE::Build(PointVector point_cloud){ if (Root_Node != nullptr){ delete_tree_nodes(&Root_Node); } if (point_cloud.size() == 0) return; STATIC_ROOT_NODE = new KD_TREE_NODE; InitTreeNode(STATIC_ROOT_NODE); BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size()-1, point_cloud); Update(STATIC_ROOT_NODE); STATIC_ROOT_NODE->TreeSize = 0; Root_Node = STATIC_ROOT_NODE->left_son_ptr; } void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector& Nearest_Points, vector & Point_Distance, double max_dist){ MANUAL_HEAP q(2*k_nearest); q.clear(); vector ().swap(Point_Distance); if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ Search(Root_Node, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(Root_Node, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } int k_found = min(k_nearest,int(q.size())); PointVector ().swap(Nearest_Points); vector ().swap(Point_Distance); for (int i=0;i < k_found;i++){ Nearest_Points.insert(Nearest_Points.begin(), q.top().point); Point_Distance.insert(Point_Distance.begin(), q.top().dist); q.pop(); } return; } int KD_TREE::Add_Points(PointVector & PointToAdd, bool downsample_on){ int NewPointSize = PointToAdd.size(); int tree_size = size(); BoxPointType Box_of_Point; PointType downsample_result, mid_point; bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; float min_dist, tmp_dist; int tmp_counter = 0; for (int i=0; i 1 || same_point(PointToAdd[i], downsample_result)){ if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, true, true); Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); tmp_counter ++; } } else { if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)){ Operation_Logger_Type operation_delete, operation; operation_delete.boxpoint = Box_of_Point; operation_delete.op = DOWNSAMPLE_DELETE; operation.point = downsample_result; operation.op = ADD_POINT; pthread_mutex_lock(&working_flag_mutex); if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, false , true); Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); tmp_counter ++; if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); if (Downsample_Storage.size() > 0) Rebuild_Logger.push(operation_delete); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); }; } } else { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); } else { Operation_Logger_Type operation; operation.point = PointToAdd[i]; operation.op = ADD_POINT; pthread_mutex_lock(&working_flag_mutex); Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } } return tmp_counter; } void KD_TREE::Add_Point_Boxes(vector & BoxPoints){ for (int i=0;i < BoxPoints.size();i++){ if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ Add_by_range(&Root_Node ,BoxPoints[i], true); } else { Operation_Logger_Type operation; operation.boxpoint = BoxPoints[i]; operation.op = ADD_BOX; pthread_mutex_lock(&working_flag_mutex); Add_by_range(&Root_Node ,BoxPoints[i], false); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } return; } void KD_TREE::Delete_Points(PointVector & PointToDel){ for (int i=0;i & BoxPoints){ int tmp_counter = 0; for (int i=0;i < BoxPoints.size();i++){ if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ tmp_counter += Delete_by_range(&Root_Node ,BoxPoints[i], true, false); } else { Operation_Logger_Type operation; operation.boxpoint = BoxPoints[i]; operation.op = DELETE_BOX; pthread_mutex_lock(&working_flag_mutex); tmp_counter += Delete_by_range(&Root_Node ,BoxPoints[i], false, false); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } return tmp_counter; } void KD_TREE::acquire_removed_points(PointVector & removed_points){ pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); for (int i = 0; i < Points_deleted.size();i++){ removed_points.push_back(Points_deleted[i]); } for (int i = 0; i < Multithread_Points_deleted.size();i++){ removed_points.push_back(Multithread_Points_deleted[i]); } Points_deleted.clear(); Multithread_Points_deleted.clear(); pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); return; } void KD_TREE::BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage){ if (l>r) return; *root = new KD_TREE_NODE; InitTreeNode(*root); int mid = (l+r)>>1; int div_axis = 0; int i; // Find the best division Axis float min_value[3] = {INFINITY, INFINITY, INFINITY}; float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; float dim_range[3] = {0,0,0}; for (i=l;i<=r;i++){ min_value[0] = min(min_value[0], Storage[i].x); min_value[1] = min(min_value[1], Storage[i].y); min_value[2] = min(min_value[2], Storage[i].z); max_value[0] = max(max_value[0], Storage[i].x); max_value[1] = max(max_value[1], Storage[i].y); max_value[2] = max(max_value[2], Storage[i].z); } // Select the longest dimension as division axis for (i=0;i<3;i++) dim_range[i] = max_value[i] - min_value[i]; for (i=1;i<3;i++) if (dim_range[i] > dim_range[div_axis]) div_axis = i; // Divide by the division axis and recursively build. (*root)->division_axis = div_axis; switch (div_axis) { case 0: nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x); break; case 1: nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_y); break; case 2: nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_z); break; default: nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x); break; } (*root)->point = Storage[mid]; KD_TREE_NODE * left_son = nullptr, * right_son = nullptr; BuildTree(&left_son, l, mid-1, Storage); BuildTree(&right_son, mid+1, r, Storage); (*root)->left_son_ptr = left_son; (*root)->right_son_ptr = right_son; Update((*root)); return; } void KD_TREE::Rebuild(KD_TREE_NODE ** root){ KD_TREE_NODE * father_ptr; if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) { if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)){ if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) { Rebuild_Ptr = root; } pthread_mutex_unlock(&rebuild_ptr_mutex_lock); } } else { father_ptr = (*root)->father_ptr; int size_rec = (*root)->TreeSize; PCL_Storage.clear(); flatten(*root, PCL_Storage, DELETE_POINTS_REC); delete_tree_nodes(root); BuildTree(root, 0, PCL_Storage.size()-1, PCL_Storage); if (*root != nullptr) (*root)->father_ptr = father_ptr; if (*root == Root_Node) STATIC_ROOT_NODE->left_son_ptr = *root; } return; } int KD_TREE::Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample){ if ((*root) == nullptr || (*root)->tree_deleted) return 0; (*root)->working_flag = true; Push_Down(*root); int tmp_counter = 0; if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return 0; if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return 0; if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return 0; if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]){ (*root)->tree_deleted = true; (*root)->point_deleted = true; (*root)->need_push_down_to_left = true; (*root)->need_push_down_to_right = true; tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; (*root)->invalid_point_num = (*root)->TreeSize; if (is_downsample){ (*root)->tree_downsample_deleted = true; (*root)->point_downsample_deleted = true; (*root)->down_del_num = (*root)->TreeSize; } return tmp_counter; } if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z){ (*root)->point_deleted = true; tmp_counter += 1; if (is_downsample) (*root)->point_downsample_deleted = true; } Operation_Logger_Type delete_box_log; struct timespec Timeout; if (is_downsample) delete_box_log.op = DOWNSAMPLE_DELETE; else delete_box_log.op = DELETE_BOX; delete_box_log.boxpoint = boxpoint; if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); } else { pthread_mutex_lock(&working_flag_mutex); tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); } else { pthread_mutex_lock(&working_flag_mutex); tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } Update(*root); if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; bool need_rebuild = allow_rebuild & Criterion_Check((*root)); if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return tmp_counter; } void KD_TREE::Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild){ if ((*root) == nullptr || (*root)->tree_deleted) return; (*root)->working_flag = true; Push_Down(*root); if (same_point((*root)->point, point) && !(*root)->point_deleted) { (*root)->point_deleted = true; (*root)->invalid_point_num += 1; if ((*root)->invalid_point_num == (*root)->TreeSize) (*root)->tree_deleted = true; return; } Operation_Logger_Type delete_log; struct timespec Timeout; delete_log.op = DELETE_POINT; delete_log.point = point; if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)){ if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Delete_by_point(&(*root)->left_son_ptr, point,false); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } else { if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Delete_by_point(&(*root)->right_son_ptr, point, false); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(delete_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } Update(*root); if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; bool need_rebuild = allow_rebuild & Criterion_Check((*root)); if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return; } void KD_TREE::Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild){ if ((*root) == nullptr) return; (*root)->working_flag = true; Push_Down(*root); if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return; if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return; if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return; if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1]> (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]){ (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; (*root)->point_deleted = false || (*root)->point_downsample_deleted; (*root)->need_push_down_to_left = true; (*root)->need_push_down_to_right = true; (*root)->invalid_point_num = (*root)->down_del_num; return; } if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z){ (*root)->point_deleted = (*root)->point_downsample_deleted; } Operation_Logger_Type add_box_log; struct timespec Timeout; add_box_log.op = ADD_BOX; add_box_log.boxpoint = boxpoint; if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Add_by_range(&((*root)->left_son_ptr), boxpoint, false); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); } else { pthread_mutex_lock(&working_flag_mutex); Add_by_range(&((*root)->right_son_ptr), boxpoint, false); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_box_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } Update(*root); if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; bool need_rebuild = allow_rebuild & Criterion_Check((*root)); if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return; } void KD_TREE::Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis){ if (*root == nullptr){ *root = new KD_TREE_NODE; InitTreeNode(*root); (*root)->point = point; (*root)->division_axis = (father_axis + 1) % 3; Update(*root); return; } (*root)->working_flag = true; Operation_Logger_Type add_log; struct timespec Timeout; add_log.op = ADD_POINT; add_log.point = point; Push_Down(*root); if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)){ if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); } else { pthread_mutex_lock(&working_flag_mutex); Add_by_point(&(*root)->left_son_ptr, point, false,(*root)->division_axis); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } else { if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild,(*root)->division_axis); } else { pthread_mutex_lock(&working_flag_mutex); Add_by_point(&(*root)->right_son_ptr, point, false,(*root)->division_axis); if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(add_log); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } pthread_mutex_unlock(&working_flag_mutex); } } Update(*root); if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; bool need_rebuild = allow_rebuild & Criterion_Check((*root)); if (need_rebuild) Rebuild(root); if ((*root) != nullptr) (*root)->working_flag = false; return; } void KD_TREE::Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist){ if (root == nullptr || root->tree_deleted) return; double cur_dist = calc_box_dist(root, point); if (cur_dist > max_dist * max_dist) return; int retval; if (root->need_push_down_to_left || root->need_push_down_to_right) { retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); if (retval == 0){ Push_Down(root); pthread_mutex_unlock(&(root->push_down_mutex_lock)); } else { pthread_mutex_lock(&(root->push_down_mutex_lock)); pthread_mutex_unlock(&(root->push_down_mutex_lock)); } } if (!root->point_deleted){ float dist = calc_dist(point, root->point); if (dist <= max_dist && (q.size() < k_nearest || dist < q.top().dist)){ if (q.size() >= k_nearest) q.pop(); PointType_CMP current_point{root->point, dist}; q.push(current_point); } } int cur_search_counter; float dist_left_node = calc_box_dist(root->left_son_ptr, point); float dist_right_node = calc_box_dist(root->right_son_ptr, point); if (q.size()< k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist){ if (dist_left_node <= dist_right_node) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ Search(root->left_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->left_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } if (q.size() < k_nearest || dist_right_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ Search(root->right_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->right_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } } else { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ Search(root->right_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->right_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } if (q.size() < k_nearest || dist_left_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ Search(root->left_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->left_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } } } else { if (dist_left_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ Search(root->left_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->left_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } if (dist_right_node < q.top().dist) { if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ Search(root->right_son_ptr, k_nearest, point, q, max_dist); } else { pthread_mutex_lock(&search_flag_mutex); while (search_mutex_counter == -1) { pthread_mutex_unlock(&search_flag_mutex); usleep(1); pthread_mutex_lock(&search_flag_mutex); } search_mutex_counter += 1; pthread_mutex_unlock(&search_flag_mutex); Search(root->right_son_ptr, k_nearest, point, q, max_dist); pthread_mutex_lock(&search_flag_mutex); search_mutex_counter -= 1; pthread_mutex_unlock(&search_flag_mutex); } } } return; } void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector & Storage){ if (root == nullptr) return; Push_Down(root); if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) return; if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) return; if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) return; if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]){ flatten(root, Storage, NOT_RECORD); return; } if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z){ if (!root->point_deleted) Storage.push_back(root->point); } if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr){ Search_by_range(root->left_son_ptr, boxpoint, Storage); } else { pthread_mutex_lock(&search_flag_mutex); Search_by_range(root->left_son_ptr, boxpoint, Storage); pthread_mutex_unlock(&search_flag_mutex); } if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr){ Search_by_range(root->right_son_ptr, boxpoint, Storage); } else { pthread_mutex_lock(&search_flag_mutex); Search_by_range(root->right_son_ptr, boxpoint, Storage); pthread_mutex_unlock(&search_flag_mutex); } return; } bool KD_TREE::Criterion_Check(KD_TREE_NODE * root){ if (root->TreeSize <= Minimal_Unbalanced_Tree_Size){ return false; } float balance_evaluation = 0.0f; float delete_evaluation = 0.0f; KD_TREE_NODE * son_ptr = root->left_son_ptr; if (son_ptr == nullptr) son_ptr = root->right_son_ptr; delete_evaluation = float(root->invalid_point_num)/ root->TreeSize; balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize-1); if (delete_evaluation > delete_criterion_param){ return true; } if (balance_evaluation > balance_criterion_param || balance_evaluation < 1-balance_criterion_param){ return true; } return false; } void KD_TREE::Push_Down(KD_TREE_NODE *root){ if (root == nullptr) return; Operation_Logger_Type operation; operation.op = PUSH_DOWN; operation.tree_deleted = root->tree_deleted; operation.tree_downsample_deleted = root->tree_downsample_deleted; if (root->need_push_down_to_left && root->left_son_ptr != nullptr){ if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; else root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; root->left_son_ptr->need_push_down_to_left = true; root->left_son_ptr->need_push_down_to_right = true; root->need_push_down_to_left = false; } else { pthread_mutex_lock(&working_flag_mutex); root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; else root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; root->left_son_ptr->need_push_down_to_left = true; root->left_son_ptr->need_push_down_to_right = true; if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } root->need_push_down_to_left = false; pthread_mutex_unlock(&working_flag_mutex); } } if (root->need_push_down_to_right && root->right_son_ptr != nullptr){ if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; else root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; root->right_son_ptr->need_push_down_to_left = true; root->right_son_ptr->need_push_down_to_right = true; root->need_push_down_to_right = false; } else { pthread_mutex_lock(&working_flag_mutex); root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; if (root->tree_downsample_deleted) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; else root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; root->right_son_ptr->need_push_down_to_left = true; root->right_son_ptr->need_push_down_to_right = true; if (rebuild_flag){ pthread_mutex_lock(&rebuild_logger_mutex_lock); Rebuild_Logger.push(operation); pthread_mutex_unlock(&rebuild_logger_mutex_lock); } root->need_push_down_to_right = false; pthread_mutex_unlock(&working_flag_mutex); } } return; } void KD_TREE::Update(KD_TREE_NODE * root){ KD_TREE_NODE * left_son_ptr = root->left_son_ptr; KD_TREE_NODE * right_son_ptr = root->right_son_ptr; float tmp_range_x[2] = {INFINITY, -INFINITY}; float tmp_range_y[2] = {INFINITY, -INFINITY}; float tmp_range_z[2] = {INFINITY, -INFINITY}; // Update Tree Size if (left_son_ptr != nullptr && right_son_ptr != nullptr){ root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted? 1:0); root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted? 1:0); root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)){ tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0],right_son_ptr->node_range_x[0]),root->point.x); tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1],right_son_ptr->node_range_x[1]),root->point.x); tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0],right_son_ptr->node_range_y[0]),root->point.y); tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1],right_son_ptr->node_range_y[1]),root->point.y); tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0],right_son_ptr->node_range_z[0]),root->point.z); tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1],right_son_ptr->node_range_z[1]),root->point.z); } else { if (!left_son_ptr->tree_deleted){ tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); } if (!right_son_ptr->tree_deleted){ tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); } if (!root->point_deleted){ tmp_range_x[0] = min(tmp_range_x[0], root->point.x); tmp_range_x[1] = max(tmp_range_x[1], root->point.x); tmp_range_y[0] = min(tmp_range_y[0], root->point.y); tmp_range_y[1] = max(tmp_range_y[1], root->point.y); tmp_range_z[0] = min(tmp_range_z[0], root->point.z); tmp_range_z[1] = max(tmp_range_z[1], root->point.z); } } } else if (left_son_ptr != nullptr){ root->TreeSize = left_son_ptr->TreeSize + 1; root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted?1:0); root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted?1:0); root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)){ tmp_range_x[0] = min(left_son_ptr->node_range_x[0],root->point.x); tmp_range_x[1] = max(left_son_ptr->node_range_x[1],root->point.x); tmp_range_y[0] = min(left_son_ptr->node_range_y[0],root->point.y); tmp_range_y[1] = max(left_son_ptr->node_range_y[1],root->point.y); tmp_range_z[0] = min(left_son_ptr->node_range_z[0],root->point.z); tmp_range_z[1] = max(left_son_ptr->node_range_z[1],root->point.z); } else { if (!left_son_ptr->tree_deleted){ tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); } if (!root->point_deleted){ tmp_range_x[0] = min(tmp_range_x[0], root->point.x); tmp_range_x[1] = max(tmp_range_x[1], root->point.x); tmp_range_y[0] = min(tmp_range_y[0], root->point.y); tmp_range_y[1] = max(tmp_range_y[1], root->point.y); tmp_range_z[0] = min(tmp_range_z[0], root->point.z); tmp_range_z[1] = max(tmp_range_z[1], root->point.z); } } } else if (right_son_ptr != nullptr){ root->TreeSize = right_son_ptr->TreeSize + 1; root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted? 1:0); root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted? 1:0); root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)){ tmp_range_x[0] = min(right_son_ptr->node_range_x[0],root->point.x); tmp_range_x[1] = max(right_son_ptr->node_range_x[1],root->point.x); tmp_range_y[0] = min(right_son_ptr->node_range_y[0],root->point.y); tmp_range_y[1] = max(right_son_ptr->node_range_y[1],root->point.y); tmp_range_z[0] = min(right_son_ptr->node_range_z[0],root->point.z); tmp_range_z[1] = max(right_son_ptr->node_range_z[1],root->point.z); } else { if (!right_son_ptr->tree_deleted){ tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); } if (!root->point_deleted){ tmp_range_x[0] = min(tmp_range_x[0], root->point.x); tmp_range_x[1] = max(tmp_range_x[1], root->point.x); tmp_range_y[0] = min(tmp_range_y[0], root->point.y); tmp_range_y[1] = max(tmp_range_y[1], root->point.y); tmp_range_z[0] = min(tmp_range_z[0], root->point.z); tmp_range_z[1] = max(tmp_range_z[1], root->point.z); } } } else { root->TreeSize = 1; root->invalid_point_num = (root->point_deleted? 1:0); root->down_del_num = (root->point_downsample_deleted? 1:0); root->tree_downsample_deleted = root->point_downsample_deleted; root->tree_deleted = root->point_deleted; tmp_range_x[0] = root->point.x; tmp_range_x[1] = root->point.x; tmp_range_y[0] = root->point.y; tmp_range_y[1] = root->point.y; tmp_range_z[0] = root->point.z; tmp_range_z[1] = root->point.z; } memcpy(root->node_range_x,tmp_range_x,sizeof(tmp_range_x)); memcpy(root->node_range_y,tmp_range_y,sizeof(tmp_range_y)); memcpy(root->node_range_z,tmp_range_z,sizeof(tmp_range_z)); if (left_son_ptr != nullptr) left_son_ptr -> father_ptr = root; if (right_son_ptr != nullptr) right_son_ptr -> father_ptr = root; if (root == Root_Node && root->TreeSize > 3){ KD_TREE_NODE * son_ptr = root->left_son_ptr; if (son_ptr == nullptr) son_ptr = root->right_son_ptr; float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize-1); root->alpha_del = float(root->invalid_point_num)/ root->TreeSize; root->alpha_bal = (tmp_bal>=0.5-EPSS)?tmp_bal:1-tmp_bal; } return; } void KD_TREE::flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type){ if (root == nullptr) return; Push_Down(root); if (!root->point_deleted) { Storage.push_back(root->point); } flatten(root->left_son_ptr, Storage, storage_type); flatten(root->right_son_ptr, Storage, storage_type); switch (storage_type) { case NOT_RECORD: break; case DELETE_POINTS_REC: if (root->point_deleted && !root->point_downsample_deleted) { Points_deleted.push_back(root->point); } break; case MULTI_THREAD_REC: if (root->point_deleted && !root->point_downsample_deleted) { Multithread_Points_deleted.push_back(root->point); } break; default: break; } return; } void KD_TREE::delete_tree_nodes(KD_TREE_NODE ** root){ if (*root == nullptr) return; Push_Down(*root); delete_tree_nodes(&(*root)->left_son_ptr); delete_tree_nodes(&(*root)->right_son_ptr); pthread_mutex_destroy( &(*root)->push_down_mutex_lock); delete *root; *root = nullptr; return; } bool KD_TREE::same_point(PointType a, PointType b){ return (fabs(a.x-b.x) < EPSS && fabs(a.y-b.y) < EPSS && fabs(a.z-b.z) < EPSS ); } float KD_TREE::calc_dist(PointType a, PointType b){ float dist = 0.0f; dist = (a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y) + (a.z-b.z)*(a.z-b.z); return dist; } float KD_TREE::calc_box_dist(KD_TREE_NODE * node, PointType point){ if (node == nullptr) return INFINITY; float min_dist = 0.0; if (point.x < node->node_range_x[0]) min_dist += (point.x - node->node_range_x[0])*(point.x - node->node_range_x[0]); if (point.x > node->node_range_x[1]) min_dist += (point.x - node->node_range_x[1])*(point.x - node->node_range_x[1]); if (point.y < node->node_range_y[0]) min_dist += (point.y - node->node_range_y[0])*(point.y - node->node_range_y[0]); if (point.y > node->node_range_y[1]) min_dist += (point.y - node->node_range_y[1])*(point.y - node->node_range_y[1]); if (point.z < node->node_range_z[0]) min_dist += (point.z - node->node_range_z[0])*(point.z - node->node_range_z[0]); if (point.z > node->node_range_z[1]) min_dist += (point.z - node->node_range_z[1])*(point.z - node->node_range_z[1]); return min_dist; } bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x;} bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y;} bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z;} // Manual heap MANUAL_HEAP::MANUAL_HEAP(int max_capacity){ cap = max_capacity; heap = new PointType_CMP[max_capacity]; heap_size = 0; } MANUAL_HEAP::~MANUAL_HEAP(){ delete[] heap; } void MANUAL_HEAP::pop(){ if (heap_size == 0) return; heap[0] = heap[heap_size-1]; heap_size--; MoveDown(0); return; } PointType_CMP MANUAL_HEAP::top(){ return heap[0]; } void MANUAL_HEAP::push(PointType_CMP point){ if (heap_size >= cap) return; heap[heap_size] = point; FloatUp(heap_size); heap_size++; return; } int MANUAL_HEAP::size(){ return heap_size; } void MANUAL_HEAP::clear(){ heap_size = 0; return; } void MANUAL_HEAP::MoveDown(int heap_index){ int l = heap_index * 2 + 1; PointType_CMP tmp = heap[heap_index]; while (l < heap_size){ if (l + 1 < heap_size && heap[l] < heap[l+1]) l++; if (tmp < heap[l]){ heap[heap_index] = heap[l]; heap_index = l; l = heap_index * 2 + 1; } else break; } heap[heap_index] = tmp; return; } void MANUAL_HEAP::FloatUp(int heap_index){ int ancestor = (heap_index-1)/2; PointType_CMP tmp = heap[heap_index]; while (heap_index > 0){ if (heap[ancestor] < tmp){ heap[heap_index] = heap[ancestor]; heap_index = ancestor; ancestor = (heap_index-1)/2; } else break; } heap[heap_index] = tmp; return; } // manual queue void MANUAL_Q::clear(){ head = 0; tail = 0; counter = 0; is_empty = true; return; } void MANUAL_Q::pop(){ if (counter == 0) return; head ++; head %= Q_LEN; counter --; if (counter == 0) is_empty = true; return; } Operation_Logger_Type MANUAL_Q::front(){ return q[head]; } Operation_Logger_Type MANUAL_Q::back(){ return q[tail]; } void MANUAL_Q::push(Operation_Logger_Type op){ q[tail] = op; counter ++; if (is_empty) is_empty = false; tail ++; tail %= Q_LEN; } bool MANUAL_Q::empty(){ return is_empty; } int MANUAL_Q::size(){ return counter; } ================================================ FILE: include/ikd-Tree/ikd_Tree.h ================================================ #pragma once #include #include #include #include #include #include #include #include #define EPSS 1e-6 #define Minimal_Unbalanced_Tree_Size 10 #define Multi_Thread_Rebuild_Point_Num 1500 #define DOWNSAMPLE_SWITCH true #define ForceRebuildPercentage 0.2 #define Q_LEN 1000000 using namespace std; typedef pcl::PointXYZINormal PointType; typedef vector> PointVector; const PointType ZeroP; struct KD_TREE_NODE { PointType point; int division_axis; int TreeSize = 1; int invalid_point_num = 0; int down_del_num = 0; bool point_deleted = false; bool tree_deleted = false; bool point_downsample_deleted = false; bool tree_downsample_deleted = false; bool need_push_down_to_left = false; bool need_push_down_to_right = false; bool working_flag = false; pthread_mutex_t push_down_mutex_lock; float node_range_x[2], node_range_y[2], node_range_z[2]; KD_TREE_NODE *left_son_ptr = nullptr; KD_TREE_NODE *right_son_ptr = nullptr; KD_TREE_NODE *father_ptr = nullptr; // For paper data record float alpha_del; float alpha_bal; }; struct PointType_CMP{ PointType point; float dist = 0.0; PointType_CMP (PointType p = ZeroP, float d = INFINITY){ this->point = p; this->dist = d; }; bool operator < (const PointType_CMP &a)const{ if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x; else return dist < a.dist; } }; struct BoxPointType{ float vertex_min[3]; float vertex_max[3]; }; enum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN}; enum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC}; struct Operation_Logger_Type{ PointType point; BoxPointType boxpoint; bool tree_deleted, tree_downsample_deleted; operation_set op; }; class MANUAL_Q{ private: int head = 0,tail = 0, counter = 0; Operation_Logger_Type q[Q_LEN]; bool is_empty; public: void pop(); Operation_Logger_Type front(); Operation_Logger_Type back(); void clear(); void push(Operation_Logger_Type op); bool empty(); int size(); }; class MANUAL_HEAP { public: MANUAL_HEAP(int max_capacity = 100); ~MANUAL_HEAP(); void pop(); PointType_CMP top(); void push(PointType_CMP point); int size(); void clear(); private: PointType_CMP * heap; void MoveDown(int heap_index); void FloatUp(int heap_index); int heap_size = 0; int cap = 0; }; class KD_TREE { private: // Multi-thread Tree Rebuild bool termination_flag = false; bool rebuild_flag = false; pthread_t rebuild_thread; pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; // queue Rebuild_Logger; MANUAL_Q Rebuild_Logger; PointVector Rebuild_PCL_Storage; KD_TREE_NODE ** Rebuild_Ptr = nullptr; int search_mutex_counter = 0; static void * multi_thread_ptr(void *arg); void multi_thread_rebuild(); void start_thread(); void stop_thread(); void run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation); // KD Tree Functions and augmented variables int Treesize_tmp = 0, Validnum_tmp = 0; float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; float delete_criterion_param = 0.5f; float balance_criterion_param = 0.7f; float downsample_size = 0.2f; bool Delete_Storage_Disabled = false; KD_TREE_NODE * STATIC_ROOT_NODE = nullptr; PointVector Points_deleted; PointVector Downsample_Storage; PointVector Multithread_Points_deleted; void InitTreeNode(KD_TREE_NODE * root); void Test_Lock_States(KD_TREE_NODE *root); void BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage); void Rebuild(KD_TREE_NODE ** root); int Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); void Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild); void Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis); void Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild); void Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist);//priority_queue void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); bool Criterion_Check(KD_TREE_NODE * root); void Push_Down(KD_TREE_NODE * root); void Update(KD_TREE_NODE * root); void delete_tree_nodes(KD_TREE_NODE ** root); void downsample(KD_TREE_NODE ** root); bool same_point(PointType a, PointType b); float calc_dist(PointType a, PointType b); float calc_box_dist(KD_TREE_NODE * node, PointType point); static bool point_cmp_x(PointType a, PointType b); static bool point_cmp_y(PointType a, PointType b); static bool point_cmp_z(PointType a, PointType b); public: KD_TREE(float delete_param = 0.5, float balance_param = 0.6 , float box_length = 0.2); ~KD_TREE(); void Set_delete_criterion_param(float delete_param); void Set_balance_criterion_param(float balance_param); void set_downsample_param(float box_length); void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); int size(); int validnum(); void root_alpha(float &alpha_bal, float &alpha_del); void Build(PointVector point_cloud); void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector & Point_Distance, double max_dist = INFINITY); int Add_Points(PointVector & PointToAdd, bool downsample_on); void Add_Point_Boxes(vector & BoxPoints); void Delete_Points(PointVector & PointToDel); int Delete_Point_Boxes(vector & BoxPoints); void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type); void acquire_removed_points(PointVector & removed_points); BoxPointType tree_range(); PointVector PCL_Storage; KD_TREE_NODE * Root_Node = nullptr; int max_queue_size = 0; }; ================================================ FILE: include/matplotlibcpp.h ================================================ #pragma once // Python headers must be included before any system headers, since // they define _POSIX_C_SOURCE #include #include #include #include #include #include #include #include #include // requires c++11 support #include #ifndef WITHOUT_NUMPY # define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION # include # ifdef WITH_OPENCV # include # endif // WITH_OPENCV /* * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so * define the ones we need here. */ # if CV_MAJOR_VERSION > 3 # define CV_BGR2RGB cv::COLOR_BGR2RGB # define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA # endif #endif // WITHOUT_NUMPY #if PY_MAJOR_VERSION >= 3 # define PyString_FromString PyUnicode_FromString # define PyInt_FromLong PyLong_FromLong # define PyString_FromString PyUnicode_FromString #endif namespace matplotlibcpp { namespace detail { static std::string s_backend; struct _interpreter { PyObject* s_python_function_arrow; PyObject *s_python_function_show; PyObject *s_python_function_close; PyObject *s_python_function_draw; PyObject *s_python_function_pause; PyObject *s_python_function_save; PyObject *s_python_function_figure; PyObject *s_python_function_fignum_exists; PyObject *s_python_function_plot; PyObject *s_python_function_quiver; PyObject* s_python_function_contour; PyObject *s_python_function_semilogx; PyObject *s_python_function_semilogy; PyObject *s_python_function_loglog; PyObject *s_python_function_fill; PyObject *s_python_function_fill_between; PyObject *s_python_function_hist; PyObject *s_python_function_imshow; PyObject *s_python_function_scatter; PyObject *s_python_function_boxplot; PyObject *s_python_function_subplot; PyObject *s_python_function_subplot2grid; PyObject *s_python_function_legend; PyObject *s_python_function_xlim; PyObject *s_python_function_ion; PyObject *s_python_function_ginput; PyObject *s_python_function_ylim; PyObject *s_python_function_title; PyObject *s_python_function_axis; PyObject *s_python_function_axvline; PyObject *s_python_function_axvspan; PyObject *s_python_function_xlabel; PyObject *s_python_function_ylabel; PyObject *s_python_function_gca; PyObject *s_python_function_xticks; PyObject *s_python_function_yticks; PyObject* s_python_function_margins; PyObject *s_python_function_tick_params; PyObject *s_python_function_grid; PyObject* s_python_function_cla; PyObject *s_python_function_clf; PyObject *s_python_function_errorbar; PyObject *s_python_function_annotate; PyObject *s_python_function_tight_layout; PyObject *s_python_colormap; PyObject *s_python_empty_tuple; PyObject *s_python_function_stem; PyObject *s_python_function_xkcd; PyObject *s_python_function_text; PyObject *s_python_function_suptitle; PyObject *s_python_function_bar; PyObject *s_python_function_colorbar; PyObject *s_python_function_subplots_adjust; /* For now, _interpreter is implemented as a singleton since its currently not possible to have multiple independent embedded python interpreters without patching the python source code or starting a separate process for each. http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program */ static _interpreter& get() { static _interpreter ctx; return ctx; } PyObject* safe_import(PyObject* module, std::string fname) { PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); if (!fn) throw std::runtime_error(std::string("Couldn't find required function: ") + fname); if (!PyFunction_Check(fn)) throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); return fn; } private: #ifndef WITHOUT_NUMPY # if PY_MAJOR_VERSION >= 3 void *import_numpy() { import_array(); // initialize C-API return NULL; } # else void import_numpy() { import_array(); // initialize C-API } # endif #endif _interpreter() { // optional but recommended #if PY_MAJOR_VERSION >= 3 wchar_t name[] = L"plotting"; #else char name[] = "plotting"; #endif Py_SetProgramName(name); Py_Initialize(); #ifndef WITHOUT_NUMPY import_numpy(); // initialize numpy C-API #endif PyObject* matplotlibname = PyString_FromString("matplotlib"); PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); PyObject* cmname = PyString_FromString("matplotlib.cm"); PyObject* pylabname = PyString_FromString("pylab"); if (!pyplotname || !pylabname || !matplotlibname || !cmname) { throw std::runtime_error("couldnt create string"); } PyObject* matplotlib = PyImport_Import(matplotlibname); Py_DECREF(matplotlibname); if (!matplotlib) { PyErr_Print(); throw std::runtime_error("Error loading module matplotlib!"); } // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, // or matplotlib.backends is imported for the first time if (!s_backend.empty()) { PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); } PyObject* pymod = PyImport_Import(pyplotname); Py_DECREF(pyplotname); if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } s_python_colormap = PyImport_Import(cmname); Py_DECREF(cmname); if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } PyObject* pylabmod = PyImport_Import(pylabname); Py_DECREF(pylabname); if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } s_python_function_arrow = safe_import(pymod, "arrow"); s_python_function_show = safe_import(pymod, "show"); s_python_function_close = safe_import(pymod, "close"); s_python_function_draw = safe_import(pymod, "draw"); s_python_function_pause = safe_import(pymod, "pause"); s_python_function_figure = safe_import(pymod, "figure"); s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); s_python_function_plot = safe_import(pymod, "plot"); s_python_function_quiver = safe_import(pymod, "quiver"); s_python_function_contour = safe_import(pymod, "contour"); s_python_function_semilogx = safe_import(pymod, "semilogx"); s_python_function_semilogy = safe_import(pymod, "semilogy"); s_python_function_loglog = safe_import(pymod, "loglog"); s_python_function_fill = safe_import(pymod, "fill"); s_python_function_fill_between = safe_import(pymod, "fill_between"); s_python_function_hist = safe_import(pymod,"hist"); s_python_function_scatter = safe_import(pymod,"scatter"); s_python_function_boxplot = safe_import(pymod,"boxplot"); s_python_function_subplot = safe_import(pymod, "subplot"); s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); s_python_function_legend = safe_import(pymod, "legend"); s_python_function_ylim = safe_import(pymod, "ylim"); s_python_function_title = safe_import(pymod, "title"); s_python_function_axis = safe_import(pymod, "axis"); s_python_function_axvline = safe_import(pymod, "axvline"); s_python_function_axvspan = safe_import(pymod, "axvspan"); s_python_function_xlabel = safe_import(pymod, "xlabel"); s_python_function_ylabel = safe_import(pymod, "ylabel"); s_python_function_gca = safe_import(pymod, "gca"); s_python_function_xticks = safe_import(pymod, "xticks"); s_python_function_yticks = safe_import(pymod, "yticks"); s_python_function_margins = safe_import(pymod, "margins"); s_python_function_tick_params = safe_import(pymod, "tick_params"); s_python_function_grid = safe_import(pymod, "grid"); s_python_function_xlim = safe_import(pymod, "xlim"); s_python_function_ion = safe_import(pymod, "ion"); s_python_function_ginput = safe_import(pymod, "ginput"); s_python_function_save = safe_import(pylabmod, "savefig"); s_python_function_annotate = safe_import(pymod,"annotate"); s_python_function_cla = safe_import(pymod, "cla"); s_python_function_clf = safe_import(pymod, "clf"); s_python_function_errorbar = safe_import(pymod, "errorbar"); s_python_function_tight_layout = safe_import(pymod, "tight_layout"); s_python_function_stem = safe_import(pymod, "stem"); s_python_function_xkcd = safe_import(pymod, "xkcd"); s_python_function_text = safe_import(pymod, "text"); s_python_function_suptitle = safe_import(pymod, "suptitle"); s_python_function_bar = safe_import(pymod,"bar"); s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); #ifndef WITHOUT_NUMPY s_python_function_imshow = safe_import(pymod, "imshow"); #endif s_python_empty_tuple = PyTuple_New(0); } ~_interpreter() { Py_Finalize(); } }; } // end namespace detail /// Select the backend /// /// **NOTE:** This must be called before the first plot command to have /// any effect. /// /// Mainly useful to select the non-interactive 'Agg' backend when running /// matplotlibcpp in headless mode, for example on a machine with no display. /// /// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use inline void backend(const std::string& name) { detail::s_backend = name; } inline bool annotate(std::string annotation, double x, double y) { detail::_interpreter::get(); PyObject * xy = PyTuple_New(2); PyObject * str = PyString_FromString(annotation.c_str()); PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "xy", xy); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, str); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } namespace detail { #ifndef WITHOUT_NUMPY // Type selector for numpy array conversion template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; // Sanity checks; comment them out or change the numpy type below if you're compiling on // a platform where they don't apply static_assert(sizeof(long long) == 8); template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; static_assert(sizeof(unsigned long long) == 8); template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; // TODO: add int, long, etc. template PyObject* get_array(const std::vector& v) { npy_intp vsize = v.size(); NPY_TYPES type = select_npy_type::type; if (type == NPY_NOTYPE) { size_t memsize = v.size()*sizeof(double); double* dp = static_cast(::malloc(memsize)); for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); return varray; } PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); return varray; } template PyObject* get_2darray(const std::vector<::std::vector>& v) { if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); npy_intp vsize[2] = {static_cast(v.size()), static_cast(v[0].size())}; PyArrayObject *varray = (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); double *vd_begin = static_cast(PyArray_DATA(varray)); for (const ::std::vector &v_row : v) { if (v_row.size() != static_cast(vsize[1])) throw std::runtime_error("Missmatched array size"); std::copy(v_row.begin(), v_row.end(), vd_begin); vd_begin += vsize[1]; } return reinterpret_cast(varray); } #else // fallback if we don't have numpy: copy every element of the given vector template PyObject* get_array(const std::vector& v) { PyObject* list = PyList_New(v.size()); for(size_t i = 0; i < v.size(); ++i) { PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); } return list; } #endif // WITHOUT_NUMPY // sometimes, for labels and such, we need string arrays inline PyObject * get_array(const std::vector& strings) { PyObject* list = PyList_New(strings.size()); for (std::size_t i = 0; i < strings.size(); ++i) { PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); } return list; } // not all matplotlib need 2d arrays, some prefer lists of lists template PyObject* get_listlist(const std::vector>& ll) { PyObject* listlist = PyList_New(ll.size()); for (std::size_t i = 0; i < ll.size(); ++i) { PyList_SetItem(listlist, i, get_array(ll[i])); } return listlist; } } // namespace detail /// Plot a line through the given x and y data points.. /// /// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html template bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) { assert(x.size() == y.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } // TODO - it should be possible to make this work by implementing // a non-numpy alternative for `detail::get_2darray()`. #ifndef WITHOUT_NUMPY template void plot_surface(const std::vector<::std::vector> &x, const std::vector<::std::vector> &y, const std::vector<::std::vector> &z, const std::map &keywords = std::map()) { detail::_interpreter::get(); // We lazily load the modules here the first time this function is called // because I'm not sure that we can assume "matplotlib installed" implies // "mpl_toolkits installed" on all platforms, and we don't want to require // it for people who don't need 3d plots. static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; if (!mpl_toolkitsmod) { detail::_interpreter::get(); PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } mpl_toolkitsmod = PyImport_Import(mpl_toolkits); Py_DECREF(mpl_toolkits); if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } axis3dmod = PyImport_Import(axis3d); Py_DECREF(axis3d); if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } } assert(x.size() == y.size()); assert(y.size() == z.size()); // using numpy arrays PyObject *xarray = detail::get_2darray(x); PyObject *yarray = detail::get_2darray(y); PyObject *zarray = detail::get_2darray(z); // construct positional args PyObject *args = PyTuple_New(3); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); PyTuple_SetItem(args, 2, zarray); // Build up the kw args. PyObject *kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); PyObject *python_colormap_coolwarm = PyObject_GetAttrString( detail::_interpreter::get().s_python_colormap, "coolwarm"); PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject *fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); if (!fig) throw std::runtime_error("Call to figure() failed."); PyObject *gca_kwargs = PyDict_New(); PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); PyObject *gca = PyObject_GetAttrString(fig, "gca"); if (!gca) throw std::runtime_error("No gca"); Py_INCREF(gca); PyObject *axis = PyObject_Call( gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); if (!axis) throw std::runtime_error("No axis"); Py_INCREF(axis); Py_DECREF(gca); Py_DECREF(gca_kwargs); PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); if (!plot_surface) throw std::runtime_error("No surface"); Py_INCREF(plot_surface); PyObject *res = PyObject_Call(plot_surface, args, kwargs); if (!res) throw std::runtime_error("failed surface"); Py_DECREF(plot_surface); Py_DECREF(axis); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); } #endif // WITHOUT_NUMPY template void plot3(const std::vector &x, const std::vector &y, const std::vector &z, const std::map &keywords = std::map()) { detail::_interpreter::get(); // Same as with plot_surface: We lazily load the modules here the first time // this function is called because I'm not sure that we can assume "matplotlib // installed" implies "mpl_toolkits installed" on all platforms, and we don't // want to require it for people who don't need 3d plots. static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; if (!mpl_toolkitsmod) { detail::_interpreter::get(); PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } mpl_toolkitsmod = PyImport_Import(mpl_toolkits); Py_DECREF(mpl_toolkits); if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } axis3dmod = PyImport_Import(axis3d); Py_DECREF(axis3d); if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } } assert(x.size() == y.size()); assert(y.size() == z.size()); PyObject *xarray = detail::get_array(x); PyObject *yarray = detail::get_array(y); PyObject *zarray = detail::get_array(z); // construct positional args PyObject *args = PyTuple_New(3); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); PyTuple_SetItem(args, 2, zarray); // Build up the kw args. PyObject *kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject *fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); if (!fig) throw std::runtime_error("Call to figure() failed."); PyObject *gca_kwargs = PyDict_New(); PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); PyObject *gca = PyObject_GetAttrString(fig, "gca"); if (!gca) throw std::runtime_error("No gca"); Py_INCREF(gca); PyObject *axis = PyObject_Call( gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); if (!axis) throw std::runtime_error("No axis"); Py_INCREF(axis); Py_DECREF(gca); Py_DECREF(gca_kwargs); PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); if (!plot3) throw std::runtime_error("No 3D line plot"); Py_INCREF(plot3); PyObject *res = PyObject_Call(plot3, args, kwargs); if (!res) throw std::runtime_error("Failed 3D line plot"); Py_DECREF(plot3); Py_DECREF(axis); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); } template bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) { assert(x.size() == y.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); // construct keyword args PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call( detail::_interpreter::get().s_python_function_stem, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template< typename Numeric > bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) { assert(x.size() == y.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, yarray); // construct keyword args PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template< typename Numeric > bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) { assert(x.size() == y1.size()); assert(x.size() == y2.size()); detail::_interpreter::get(); // using numpy arrays PyObject* xarray = detail::get_array(x); PyObject* y1array = detail::get_array(y1); PyObject* y2array = detail::get_array(y2); // construct positional args PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, xarray); PyTuple_SetItem(args, 1, y1array); PyTuple_SetItem(args, 2, y2array); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { PyObject* obj_x = PyFloat_FromDouble(x); PyObject* obj_y = PyFloat_FromDouble(y); PyObject* obj_end_x = PyFloat_FromDouble(end_x); PyObject* obj_end_y = PyFloat_FromDouble(end_y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); PyObject* plot_args = PyTuple_New(4); PyTuple_SetItem(plot_args, 0, obj_x); PyTuple_SetItem(plot_args, 1, obj_y); PyTuple_SetItem(plot_args, 2, obj_end_x); PyTuple_SetItem(plot_args, 3, obj_end_y); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template< typename Numeric> bool hist(const std::vector& y, long bins=10,std::string color="b", double alpha=1.0, bool cumulative=false) { detail::_interpreter::get(); PyObject* yarray = detail::get_array(y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); PyObject* plot_args = PyTuple_New(1); PyTuple_SetItem(plot_args, 0, yarray); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } #ifndef WITHOUT_NUMPY namespace detail { inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) { assert(type == NPY_UINT8 || type == NPY_FLOAT); assert(colors == 1 || colors == 3 || colors == 4); detail::_interpreter::get(); // construct args npy_intp dims[3] = { rows, columns, colors }; PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (!res) throw std::runtime_error("Call to imshow() failed"); if (out) *out = res; else Py_DECREF(res); } } // namespace detail inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) { detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); } inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) { detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); } #ifdef WITH_OPENCV void imshow(const cv::Mat &image, const std::map &keywords = {}) { // Convert underlying type of matrix, if needed cv::Mat image2; NPY_TYPES npy_type = NPY_UINT8; switch (image.type() & CV_MAT_DEPTH_MASK) { case CV_8U: image2 = image; break; case CV_32F: image2 = image; npy_type = NPY_FLOAT; break; default: image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); } // If color image, convert from BGR to RGB switch (image2.channels()) { case 3: cv::cvtColor(image2, image2, CV_BGR2RGB); break; case 4: cv::cvtColor(image2, image2, CV_BGRA2RGBA); } detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); } #endif // WITH_OPENCV #endif // WITHOUT_NUMPY template bool scatter(const std::vector& x, const std::vector& y, const double s=1.0, // The marker size in points**2 const std::map & keywords = {}) { detail::_interpreter::get(); assert(x.size() == y.size()); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); for (const auto& it : keywords) { PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); } PyObject* plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool boxplot(const std::vector>& data, const std::vector& labels = {}, const std::map & keywords = {}) { detail::_interpreter::get(); PyObject* listlist = detail::get_listlist(data); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, listlist); PyObject* kwargs = PyDict_New(); // kwargs needs the labels, if there are (the correct number of) labels if (!labels.empty() && labels.size() == data.size()) { PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); } // take care of the remaining keywords for (const auto& it : keywords) { PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool boxplot(const std::vector& data, const std::map & keywords = {}) { detail::_interpreter::get(); PyObject* vector = detail::get_array(data); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, vector); PyObject* kwargs = PyDict_New(); for (const auto& it : keywords) { PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool bar(const std::vector & x, const std::vector & y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map & keywords = {}) { detail::_interpreter::get(); PyObject * xarray = detail::get_array(x); PyObject * yarray = detail::get_array(y); PyObject * kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString( kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject * plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject * res = PyObject_Call( detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if (res) Py_DECREF(res); return res; } template bool bar(const std::vector & y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map & keywords = {}) { using T = typename std::remove_reference::type::value_type; detail::_interpreter::get(); std::vector x; for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } return bar(x, y, ec, ls, lw, keywords); } inline bool subplots_adjust(const std::map& keywords = {}) { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); } PyObject* plot_args = PyTuple_New(0); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template< typename Numeric> bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) { detail::_interpreter::get(); PyObject* yarray = detail::get_array(y); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); PyObject* plot_args = PyTuple_New(1); PyTuple_SetItem(plot_args, 0, yarray); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); Py_DECREF(plot_args); Py_DECREF(kwargs); if(res) Py_DECREF(res); return res; } template bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool contour(const std::vector& x, const std::vector& y, const std::vector& z, const std::map& keywords = {}) { assert(x.size() == y.size() && x.size() == z.size()); PyObject* xarray = get_array(x); PyObject* yarray = get_array(y); PyObject* zarray = get_array(z); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, zarray); // construct keyword args PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) { assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* uarray = detail::get_array(u); PyObject* warray = detail::get_array(w); PyObject* plot_args = PyTuple_New(4); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, uarray); PyTuple_SetItem(plot_args, 3, warray); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call( detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject( detail::_interpreter::get().s_python_function_stem, plot_args); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(s.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } template bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) { assert(x.size() == y.size()); detail::_interpreter::get(); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* yerrarray = detail::get_array(yerr); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyDict_SetItemString(kwargs, "yerr", yerrarray); PyObject *plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); else throw std::runtime_error("Call to errorbar() failed."); return res; } template bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, yarray); PyTuple_SetItem(plot_args, 1, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if (res) Py_DECREF(res); return res; } template bool plot(const std::vector& y, const std::string& format = "") { std::vector x(y.size()); for(size_t i=0; i bool plot(const std::vector& y, const std::map& keywords) { std::vector x(y.size()); for(size_t i=0; i bool stem(const std::vector& y, const std::string& format = "") { std::vector x(y.size()); for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; return stem(x, y, format); } template void text(Numeric x, Numeric y, const std::string& s = "") { detail::_interpreter::get(); PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); if(!res) throw std::runtime_error("Call to text() failed."); Py_DECREF(args); Py_DECREF(res); } inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) { if (mappable == NULL) throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); detail::_interpreter::get(); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, mappable); PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); if(!res) throw std::runtime_error("Call to colorbar() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline long figure(long number = -1) { detail::_interpreter::get(); PyObject *res; if (number == -1) res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); else { assert(number > 0); // Make sure interpreter is initialised detail::_interpreter::get(); PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyLong_FromLong(number)); res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); Py_DECREF(args); } if(!res) throw std::runtime_error("Call to figure() failed."); PyObject* num = PyObject_GetAttrString(res, "number"); if (!num) throw std::runtime_error("Could not get number attribute of figure object"); const long figureNumber = PyLong_AsLong(num); Py_DECREF(num); Py_DECREF(res); return figureNumber; } inline bool fignum_exists(long number) { detail::_interpreter::get(); PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyLong_FromLong(number)); PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); if(!res) throw std::runtime_error("Call to fignum_exists() failed."); bool ret = PyObject_IsTrue(res); Py_DECREF(res); Py_DECREF(args); return ret; } inline void figure_size(size_t w, size_t h) { detail::_interpreter::get(); const size_t dpi = 100; PyObject* size = PyTuple_New(2); PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); PyObject* kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "figsize", size); PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple, kwargs); Py_DECREF(kwargs); if(!res) throw std::runtime_error("Call to figure_size() failed."); Py_DECREF(res); } inline void legend() { detail::_interpreter::get(); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); if(!res) throw std::runtime_error("Call to legend() failed."); Py_DECREF(res); } inline void legend(const std::map& keywords) { detail::_interpreter::get(); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); if(!res) throw std::runtime_error("Call to legend() failed."); Py_DECREF(kwargs); Py_DECREF(res); } template void ylim(Numeric left, Numeric right) { detail::_interpreter::get(); PyObject* list = PyList_New(2); PyList_SetItem(list, 0, PyFloat_FromDouble(left)); PyList_SetItem(list, 1, PyFloat_FromDouble(right)); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, list); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); if(!res) throw std::runtime_error("Call to ylim() failed."); Py_DECREF(args); Py_DECREF(res); } template void xlim(Numeric left, Numeric right) { detail::_interpreter::get(); PyObject* list = PyList_New(2); PyList_SetItem(list, 0, PyFloat_FromDouble(left)); PyList_SetItem(list, 1, PyFloat_FromDouble(right)); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, list); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); if(!res) throw std::runtime_error("Call to xlim() failed."); Py_DECREF(args); Py_DECREF(res); } inline double* xlim() { detail::_interpreter::get(); PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); PyObject* left = PyTuple_GetItem(res,0); PyObject* right = PyTuple_GetItem(res,1); double* arr = new double[2]; arr[0] = PyFloat_AsDouble(left); arr[1] = PyFloat_AsDouble(right); if(!res) throw std::runtime_error("Call to xlim() failed."); Py_DECREF(res); return arr; } inline double* ylim() { detail::_interpreter::get(); PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); PyObject* left = PyTuple_GetItem(res,0); PyObject* right = PyTuple_GetItem(res,1); double* arr = new double[2]; arr[0] = PyFloat_AsDouble(left); arr[1] = PyFloat_AsDouble(right); if(!res) throw std::runtime_error("Call to ylim() failed."); Py_DECREF(res); return arr; } template inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) { assert(labels.size() == 0 || ticks.size() == labels.size()); detail::_interpreter::get(); // using numpy array PyObject* ticksarray = detail::get_array(ticks); PyObject* args; if(labels.size() == 0) { // construct positional args args = PyTuple_New(1); PyTuple_SetItem(args, 0, ticksarray); } else { // make tuple of tick labels PyObject* labelstuple = PyTuple_New(labels.size()); for (size_t i = 0; i < labels.size(); i++) PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); // construct positional args args = PyTuple_New(2); PyTuple_SetItem(args, 0, ticksarray); PyTuple_SetItem(args, 1, labelstuple); } // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(!res) throw std::runtime_error("Call to xticks() failed"); Py_DECREF(res); } template inline void xticks(const std::vector &ticks, const std::map& keywords) { xticks(ticks, {}, keywords); } template inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) { assert(labels.size() == 0 || ticks.size() == labels.size()); detail::_interpreter::get(); // using numpy array PyObject* ticksarray = detail::get_array(ticks); PyObject* args; if(labels.size() == 0) { // construct positional args args = PyTuple_New(1); PyTuple_SetItem(args, 0, ticksarray); } else { // make tuple of tick labels PyObject* labelstuple = PyTuple_New(labels.size()); for (size_t i = 0; i < labels.size(); i++) PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); // construct positional args args = PyTuple_New(2); PyTuple_SetItem(args, 0, ticksarray); PyTuple_SetItem(args, 1, labelstuple); } // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(!res) throw std::runtime_error("Call to yticks() failed"); Py_DECREF(res); } template inline void yticks(const std::vector &ticks, const std::map& keywords) { yticks(ticks, {}, keywords); } template inline void margins(Numeric margin) { // construct positional args PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); if (!res) throw std::runtime_error("Call to margins() failed."); Py_DECREF(args); Py_DECREF(res); } template inline void margins(Numeric margin_x, Numeric margin_y) { // construct positional args PyObject* args = PyTuple_New(2); PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); if (!res) throw std::runtime_error("Call to margins() failed."); Py_DECREF(args); Py_DECREF(res); } inline void tick_params(const std::map& keywords, const std::string axis = "both") { detail::_interpreter::get(); // construct positional args PyObject* args; args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); // construct keyword args PyObject* kwargs = PyDict_New(); for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if (!res) throw std::runtime_error("Call to tick_params() failed"); Py_DECREF(res); } inline void subplot(long nrows, long ncols, long plot_number) { detail::_interpreter::get(); // construct positional args PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); if(!res) throw std::runtime_error("Call to subplot() failed."); Py_DECREF(args); Py_DECREF(res); } inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) { detail::_interpreter::get(); PyObject* shape = PyTuple_New(2); PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); PyObject* loc = PyTuple_New(2); PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); PyObject* args = PyTuple_New(4); PyTuple_SetItem(args, 0, shape); PyTuple_SetItem(args, 1, loc); PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); if(!res) throw std::runtime_error("Call to subplot2grid() failed."); Py_DECREF(shape); Py_DECREF(loc); Py_DECREF(args); Py_DECREF(res); } inline void title(const std::string &titlestr, const std::map &keywords = {}) { detail::_interpreter::get(); PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pytitlestr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); if(!res) throw std::runtime_error("Call to title() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) { detail::_interpreter::get(); PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pysuptitlestr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); if(!res) throw std::runtime_error("Call to suptitle() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void axis(const std::string &axisstr) { detail::_interpreter::get(); PyObject* str = PyString_FromString(axisstr.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, str); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); if(!res) throw std::runtime_error("Call to title() failed."); Py_DECREF(args); Py_DECREF(res); } inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) { detail::_interpreter::get(); // construct positional args PyObject* args = PyTuple_New(3); PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); } inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) { // construct positional args PyObject* args = PyTuple_New(4); PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { if (it->first == "linewidth" || it->first == "alpha") PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); else PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); Py_DECREF(args); Py_DECREF(kwargs); if(res) Py_DECREF(res); } inline void xlabel(const std::string &str, const std::map &keywords = {}) { detail::_interpreter::get(); PyObject* pystr = PyString_FromString(str.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pystr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); if(!res) throw std::runtime_error("Call to xlabel() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void ylabel(const std::string &str, const std::map& keywords = {}) { detail::_interpreter::get(); PyObject* pystr = PyString_FromString(str.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pystr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); if(!res) throw std::runtime_error("Call to ylabel() failed."); Py_DECREF(args); Py_DECREF(kwargs); Py_DECREF(res); } inline void set_zlabel(const std::string &str, const std::map& keywords = {}) { detail::_interpreter::get(); // Same as with plot_surface: We lazily load the modules here the first time // this function is called because I'm not sure that we can assume "matplotlib // installed" implies "mpl_toolkits installed" on all platforms, and we don't // want to require it for people who don't need 3d plots. static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; if (!mpl_toolkitsmod) { PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } mpl_toolkitsmod = PyImport_Import(mpl_toolkits); Py_DECREF(mpl_toolkits); if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } axis3dmod = PyImport_Import(axis3d); Py_DECREF(axis3d); if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } } PyObject* pystr = PyString_FromString(str.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pystr); PyObject* kwargs = PyDict_New(); for (auto it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject *ax = PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, detail::_interpreter::get().s_python_empty_tuple); if (!ax) throw std::runtime_error("Call to gca() failed."); Py_INCREF(ax); PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); Py_INCREF(zlabel); PyObject *res = PyObject_Call(zlabel, args, kwargs); if (!res) throw std::runtime_error("Call to set_zlabel() failed."); Py_DECREF(zlabel); Py_DECREF(ax); Py_DECREF(args); Py_DECREF(kwargs); if (res) Py_DECREF(res); } inline void grid(bool flag) { detail::_interpreter::get(); PyObject* pyflag = flag ? Py_True : Py_False; Py_INCREF(pyflag); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pyflag); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); if(!res) throw std::runtime_error("Call to grid() failed."); Py_DECREF(args); Py_DECREF(res); } inline void show(const bool block = true) { detail::_interpreter::get(); PyObject* res; if(block) { res = PyObject_CallObject( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple); } else { PyObject *kwargs = PyDict_New(); PyDict_SetItemString(kwargs, "block", Py_False); res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); Py_DECREF(kwargs); } if (!res) throw std::runtime_error("Call to show() failed."); Py_DECREF(res); } inline void close() { detail::_interpreter::get(); PyObject* res = PyObject_CallObject( detail::_interpreter::get().s_python_function_close, detail::_interpreter::get().s_python_empty_tuple); if (!res) throw std::runtime_error("Call to close() failed."); Py_DECREF(res); } inline void xkcd() { detail::_interpreter::get(); PyObject* res; PyObject *kwargs = PyDict_New(); res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, detail::_interpreter::get().s_python_empty_tuple, kwargs); Py_DECREF(kwargs); if (!res) throw std::runtime_error("Call to show() failed."); Py_DECREF(res); } inline void draw() { detail::_interpreter::get(); PyObject* res = PyObject_CallObject( detail::_interpreter::get().s_python_function_draw, detail::_interpreter::get().s_python_empty_tuple); if (!res) throw std::runtime_error("Call to draw() failed."); Py_DECREF(res); } template inline void pause(Numeric interval) { detail::_interpreter::get(); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); if(!res) throw std::runtime_error("Call to pause() failed."); Py_DECREF(args); Py_DECREF(res); } inline void save(const std::string& filename) { detail::_interpreter::get(); PyObject* pyfilename = PyString_FromString(filename.c_str()); PyObject* args = PyTuple_New(1); PyTuple_SetItem(args, 0, pyfilename); std::cout<<"args:"<> ginput(const int numClicks = 1, const std::map& keywords = {}) { detail::_interpreter::get(); PyObject *args = PyTuple_New(1); PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); // construct keyword args PyObject* kwargs = PyDict_New(); for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); } PyObject* res = PyObject_Call( detail::_interpreter::get().s_python_function_ginput, args, kwargs); Py_DECREF(kwargs); Py_DECREF(args); if (!res) throw std::runtime_error("Call to ginput() failed."); const size_t len = PyList_Size(res); std::vector> out; out.reserve(len); for (size_t i = 0; i < len; i++) { PyObject *current = PyList_GetItem(res, i); std::array position; position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); out.push_back(position); } Py_DECREF(res); return out; } // Actually, is there any reason not to call this automatically for every plot? inline void tight_layout() { detail::_interpreter::get(); PyObject *res = PyObject_CallObject( detail::_interpreter::get().s_python_function_tight_layout, detail::_interpreter::get().s_python_empty_tuple); if (!res) throw std::runtime_error("Call to tight_layout() failed."); Py_DECREF(res); } // Support for variadic plot() and initializer lists: namespace detail { template using is_function = typename std::is_function>>::type; template struct is_callable_impl; template struct is_callable_impl { typedef is_function type; }; // a non-object is callable iff it is a function template struct is_callable_impl { struct Fallback { void operator()(); }; struct Derived : T, Fallback { }; template struct Check; template static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match template static std::false_type test( Check* ); public: typedef decltype(test(nullptr)) type; typedef decltype(&Fallback::operator()) dtype; static constexpr bool value = type::value; }; // an object is callable iff it defines operator() template struct is_callable { // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not typedef typename is_callable_impl::value, T>::type type; }; template struct plot_impl { }; template<> struct plot_impl { template bool operator()(const IterableX& x, const IterableY& y, const std::string& format) { // 2-phase lookup for distance, begin, end using std::distance; using std::begin; using std::end; auto xs = distance(begin(x), end(x)); auto ys = distance(begin(y), end(y)); assert(xs == ys && "x and y data must have the same number of elements!"); PyObject* xlist = PyList_New(xs); PyObject* ylist = PyList_New(ys); PyObject* pystring = PyString_FromString(format.c_str()); auto itx = begin(x), ity = begin(y); for(size_t i = 0; i < xs; ++i) { PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); } PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xlist); PyTuple_SetItem(plot_args, 1, ylist); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); Py_DECREF(plot_args); if(res) Py_DECREF(res); return res; } }; template<> struct plot_impl { template bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) { if(begin(ticks) == end(ticks)) return true; // We could use additional meta-programming to deduce the correct element type of y, // but all values have to be convertible to double anyways std::vector y; for(auto x : ticks) y.push_back(f(x)); return plot_impl()(ticks,y,format); } }; } // end namespace detail // recursion stop for the above template bool plot() { return true; } template bool plot(const A& a, const B& b, const std::string& format, Args... args) { return detail::plot_impl::type>()(a,b,format) && plot(args...); } /* * This group of plot() functions is needed to support initializer lists, i.e. calling * plot( {1,2,3,4} ) */ inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { return plot(x,y,format); } inline bool plot(const std::vector& y, const std::string& format = "") { return plot(y,format); } inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { return plot(x,y,keywords); } /* * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting */ class Plot { public: // default calibration with plot label, some data and format template Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { detail::_interpreter::get(); assert(x.size() == y.size()); PyObject* kwargs = PyDict_New(); if(name != "") PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* pystring = PyString_FromString(format.c_str()); PyObject* plot_args = PyTuple_New(3); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyTuple_SetItem(plot_args, 2, pystring); PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); Py_DECREF(kwargs); Py_DECREF(plot_args); if(res) { line= PyList_GetItem(res, 0); if(line) set_data_fct = PyObject_GetAttrString(line,"set_data"); else Py_DECREF(line); Py_DECREF(res); } } // shorter calibration with name or format only // basically calls line, = plot([], []) Plot(const std::string& name = "", const std::string& format = "") : Plot(name, std::vector(), std::vector(), format) {} template bool update(const std::vector& x, const std::vector& y) { assert(x.size() == y.size()); if(set_data_fct) { PyObject* xarray = detail::get_array(x); PyObject* yarray = detail::get_array(y); PyObject* plot_args = PyTuple_New(2); PyTuple_SetItem(plot_args, 0, xarray); PyTuple_SetItem(plot_args, 1, yarray); PyObject* res = PyObject_CallObject(set_data_fct, plot_args); if (res) Py_DECREF(res); return res; } return false; } // clears the plot but keep it available bool clear() { return update(std::vector(), std::vector()); } // definitely remove this line void remove() { if(line) { auto remove_fct = PyObject_GetAttrString(line,"remove"); PyObject* args = PyTuple_New(0); PyObject* res = PyObject_CallObject(remove_fct, args); if (res) Py_DECREF(res); } decref(); } ~Plot() { decref(); } private: void decref() { if(line) Py_DECREF(line); if(set_data_fct) Py_DECREF(set_data_fct); } PyObject* line = nullptr; PyObject* set_data_fct = nullptr; }; } // end namespace matplotlibcpp ================================================ FILE: include/preprocess.h ================================================ #ifndef PREPROCESS_H #define PREPROCESS_H #include "common_lib.h" #include #include #include #include using namespace std; #define SCAN_RATE 10 enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; enum Surround{Prev, Next}; enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; const bool time_list_cut_frame(PointType &x, PointType &y); struct orgtype { double range; double dista; double angle[2]; double intersect; E_jump edj[2]; Feature ftype; orgtype() { range = 0; edj[Prev] = Nr_nor; edj[Next] = Nr_nor; ftype = Nor; intersect = 2; } }; namespace velodyne_without_time { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; // uint32_t t; uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_without_time POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_without_time::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) // (std::uint32_t, t, t) (uint16_t, ring, ring) ) namespace velodyne_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; float time; uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_ros POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, time, time) (uint16_t, ring, ring) ) namespace ouster_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; uint32_t t; uint16_t reflectivity; uint8_t ring; uint16_t ambient; uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace ouster_ros POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) (std::uint8_t, ring, ring) (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) // namespace pandar_ros namespace pandar_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; double timestamp; uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (double, timestamp, timestamp) (std::uint16_t, ring, ring) ) class Preprocess { public: // EIGEN_MAKE_ALIGNED_OPERATOR_NEW Preprocess(); ~Preprocess(); void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 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); void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 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); void set(bool feat_en, int lid_type, double bld, int pfilt_num); // sensor_msgs::PointCloud2::ConstPtr pointcloud; PointCloudXYZI pl_full, pl_corn, pl_surf; PointCloudXYZI pl_buff[128]; //maximum 128 line lidar vector typess[128]; //maximum 128 line lidar int lidar_type, point_filter_num, N_SCANS;; double blind; bool feature_enabled, given_offset_time; ros::Publisher pub_full, pub_surf, pub_corn; private: void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); void oust_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void velodyne_handler_kitti(const sensor_msgs::PointCloud2::ConstPtr &msg); void velodyne_handler_nclt(const sensor_msgs::PointCloud2::ConstPtr &msg); void l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); void give_feature(PointCloudXYZI &pl, vector &types); // void pub_func(PointCloudXYZI &pl, const ros::Time &ct); int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); // bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); int group_size; double disA, disB, inf_bound; double limit_maxmid, limit_midmin, limit_maxmin; double p2l_ratio; double jump_up_limit, jump_down_limit; double cos160; double edgea, edgeb; double smallp_intersect, smallp_ratio; double vx, vy, vz; }; #endif ================================================ FILE: include/scope_timer.hpp ================================================ // // Created by yunfan on 2021/3/19. // Version: 1.0.0 // #ifndef SCROPE_TIMER_HPP//SRC_POLY_VISUAL_UTILS_HPP #define SCROPE_TIMER_HPP #include #include "color.h" #include "cstring" using namespace std; class TimeConsuming { public: TimeConsuming(); TimeConsuming(string msg, int repeat_time) { repeat_time_ = repeat_time; msg_ = msg; tc_start = std::chrono::high_resolution_clock::now(); has_shown = false; } TimeConsuming(string msg) { msg_ = msg; repeat_time_ = 1; tc_start = std::chrono::high_resolution_clock::now(); has_shown = false; } ~TimeConsuming() { if (!has_shown && enable_) { tc_end = std::chrono::high_resolution_clock::now(); double dt = std::chrono::duration_cast>(tc_end - tc_start).count(); double t_us = (double) dt * 1e6 / repeat_time_; if (t_us < 1) { t_us *= 1000; printf("[TIMER] %s time consuming \033[32m %lf ns\033[0m\n", msg_.c_str(), t_us); } else if (t_us > 1e6) { t_us /= 1e6; printf("[TIMER] %s time consuming \033[32m %lf s\033[0m\n", msg_.c_str(), t_us); } else if (t_us > 1e3) { t_us /= 1e3; printf("[TIMER] %s time consuming \033[32m %lf ms\033[0m\n", msg_.c_str(), t_us); }else printf("[TIMER] %s time consuming \033[32m %lf us\033[0m\n", msg_.c_str(), t_us); } } void set_enbale(bool enable){ enable_ = enable; } void start() { tc_start = std::chrono::high_resolution_clock::now(); } void stop() { if(!enable_){return;} has_shown = true; tc_end = std::chrono::high_resolution_clock::now(); double dt = std::chrono::duration_cast>(tc_end - tc_start).count(); // ROS_WARN("%s time consuming %lf us.",msg_.c_str(),(double)(end_t - start_t).toNSec()/ 1e3); double t_us = (double) dt * 1e6 / repeat_time_; if (t_us < 1) { t_us *= 1000; printf(" -- [TIMER] %s time consuming \033[32m %lf ns\033[0m\n", msg_.c_str(), t_us); } else if (t_us > 1e6) { t_us /= 1e6; printf(" -- [TIMER] %s time consuming \033[32m %lf s\033[0m\n", msg_.c_str(), t_us); } else if (t_us > 1e3) { t_us /= 1e3; printf(" -- [TIMER] %s time consuming \033[32m %lf ms\033[0m\n", msg_.c_str(), t_us); }else printf(" -- [TIMER] %s time consuming \033[32m %lf us\033[0m\n", msg_.c_str(), t_us); } private: std::chrono::high_resolution_clock::time_point tc_start, tc_end; string msg_; int repeat_time_; bool has_shown = false; bool enable_{true}; }; #endif //SRC_POLY_VISUAL_UTILS_HPP ================================================ FILE: include/so3_math.h ================================================ #ifndef SO3_MATH_H #define SO3_MATH_H #include #include #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 #define SKEW_2D(w) 0.0, -w, w, 0.0 template Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) { Eigen::Matrix skew_sym_mat; skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; return skew_sym_mat; } template Eigen::Matrix Exp(const Eigen::Matrix &ang) { T ang_norm = ang.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_norm > 0.0000001) { Eigen::Matrix r_axis = ang / ang_norm; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_axis); /// Roderigous Tranformation return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) { T ang_vel_norm = ang_vel.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_vel_norm > 0.0000001) { Eigen::Matrix r_axis = ang_vel / ang_vel_norm; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_axis); T r_ang = ang_vel_norm * dt; /// Roderigous Tranformation return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) { T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (norm > 0.00001) { T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_ang); /// Roderigous Tranformation return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; } else { return Eye3; } } template Eigen::Matrix A_cal(const Eigen::Matrix & ang_vel) { T norm = ang_vel.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (norm > 0.00001) { Eigen::Matrix r_ang = ang_vel / norm; Eigen::Matrix K; K << SKEW_SYM_MATRX(r_ang); return Eye3 + (1.0 - std::cos(norm)/norm) * K + (1.0 - std::sin(norm)/norm) * K * K ; } else { return Eye3; } } /* Logrithm of a Rotation Matrix */ template Eigen::Matrix Log(const Eigen::Matrix &R) { T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); } // convert Rotation Matrix to euler angle // unit - radian template Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) { T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); bool singular = sy < 1e-6; T x, y, z; if(!singular) { x = atan2(rot(2, 1), rot(2, 2)); y = atan2(-rot(2, 0), sy); z = atan2(rot(1, 0), rot(0, 0)); } else { x = atan2(-rot(1, 2), rot(1, 1)); y = atan2(-rot(2, 0), sy); z = 0; } Eigen::Matrix ang(x, y, z); return ang; } // convert euler angle to Rotation Matrix // unit - radian template Eigen::Matrix EulerToRotM(const Eigen::Matrix &ang) { Eigen::Matrix R_x, R_y, R_z; R_x << 1, 0, 0, 0, cos(ang[0]), -sin(ang[0]), 0, sin(ang[0]), cos(ang[0]); R_y << cos(ang[1]), 0, sin(ang[1]), 0, 1, 0, -sin(ang[1]), 0, cos(ang[1]); R_z << cos(ang[2]), -sin(ang[2]), 0, sin(ang[2]), cos(ang[2]), 0, 0, 0, 1; return R_z * R_y * R_x; } #endif ================================================ FILE: launch/hilti/hilti_basement_3.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/hilti/hilti_basement_4.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/m2dgr/m2dgr_gate01.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/m2dgr/m2dgr_hall04.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/m2dgr/m2dgr_lift04.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/m2dgr/m2dgr_rotation02.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/m2dgr/m2dgr_street08.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/ouster.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/s3e/s3e_bob_lab01.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/s3e/s3e_bob_lab02.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: launch/velodyne.launch ================================================ launch-prefix="gdb -ex run --args" ================================================ FILE: msg/Pose6D.msg ================================================ # the preintegrated Lidar states at the time of IMU measurements in a frame float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin float64[3] pos # the preintegrated position (global frame) at the Lidar origin float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin ================================================ FILE: msg/States.msg ================================================ Header header # timestamp of the first lidar in a frame float64[] rot_end # the estimated attitude (rotation matrix) at the end lidar point float64[] pos_end # the estimated position at the end lidar point (world frame) float64[] vel_end # the estimated velocity at the end lidar point (world frame) float64[] bias_gyr # gyroscope bias float64[] bias_acc # accelerator bias float64[] gravity # the estimated gravity acceleration float64[] cov # states covariance # Pose6D[] IMUpose # 6D pose at each imu measurements ================================================ FILE: package.xml ================================================ gril_calib 0.0.0 This is a modified version of LOAM which is original algorithm is described in the following paper: J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. claydergc BSD Ji Zhang catkin geometry_msgs nav_msgs roscpp rospy std_msgs sensor_msgs tf pcl_ros livox_ros_driver message_generation geometry_msgs nav_msgs sensor_msgs roscpp rospy std_msgs tf pcl_ros livox_ros_driver message_runtime rostest rosbag ================================================ FILE: result/traj.txt ================================================ ================================================ FILE: rviz_cfg/.gitignore ================================================ ================================================ FILE: rviz_cfg/hilti.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /mapping1 - /mapping1/currPoints1/Autocompute Value Bounds1 - /Odometry1/Odometry1/Shape1 Splitter Ratio: 0.6432291865348816 Tree Height: 288 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: currPoints Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 1 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 100 Reference Frame: Value: false - Class: rviz/Axes Enabled: false Length: 0.699999988079071 Name: Axes Radius: 0.05999999865889549 Reference Frame: Value: false - Class: rviz/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 2.8887665271759033 Min Value: -1.7356979846954346 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 238; 238; 236 Color Transformer: AxisColor Decay Time: 0.10000000149011612 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: surround Position Transformer: XYZ Queue Size: 1 Selectable: false Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 7 Min Value: -1 Value: false Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 1000 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: currPoints Position Transformer: XYZ Queue Size: 100000 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: Ground Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.029999999329447746 Style: Flat Squares Topic: /patchworkpp/ground Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true Enabled: true Name: mapping - Class: rviz/Group Displays: - Angle Tolerance: 0.009999999776482582 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 Shape: Alpha: 1 Axes Length: 0.699999988079071 Axes Radius: 0.10000000149011612 Color: 255; 85; 0 Head Length: 0 Head Radius: 0 Shaft Length: 0.05000000074505806 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /aft_mapped_to_init Unreliable: false Value: true Enabled: true Name: Odometry - Class: rviz/Axes Enabled: true Length: 0.699999988079071 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: true - Alpha: 0 Buffer Length: 2 Class: rviz/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 Head Length: 0 Length: 0.6000000238418579 Line Style: Billboards Line Width: 0.05000000074505806 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 25; 255; 255 Pose Style: None Radius: 0.07999999821186066 Shaft Diameter: 0.4000000059604645 Shaft Length: 0.4000000059604645 Topic: /path Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: RawPoints Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /points_raw Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Marker Enabled: true Marker Topic: /planner_normal Name: Marker Namespaces: {} Queue Size: 100 Value: true - Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /projected_map Unreliable: false Use Timestamp: false Value: true - Class: rviz/Image Enabled: true Image Topic: /alphasense/cam1/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: LiDAR Frame Rate: 10 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 53.233673095703125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 15.627415657043457 Y: -13.479835510253906 Z: 6.167871106299572e-6 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.5597963333129883 Target Frame: LiDAR Value: ThirdPersonFollower (rviz) Yaw: 4.626343727111816 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1025 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 000000ff00000000fd000000040000000000000256000003abfc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000015b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000019c0000024a0000001600fffffffb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006500000002f0000000f60000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002620000019f0000000000000000fb0000000a0049006d0061006700650100000257000001aa0000000000000000fb0000000a0049006d006100670065010000026e0000017800000000000000000000000100000152000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000039fc0100000002fb0000000800540069006d006500000000000000073d0000042700fffffffb0000000800540069006d00650100000000000004500000000000000000000004e1000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1853 X: 67 Y: 27 ================================================ FILE: rviz_cfg/m2dgr.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /mapping1/currPoints1/Autocompute Value Bounds1 - /Odometry1/Odometry1/Shape1 Splitter Ratio: 0.6432291865348816 Tree Height: 452 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: surround Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 1 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 100 Reference Frame: Value: false - Class: rviz/Axes Enabled: false Length: 0.699999988079071 Name: Axes Radius: 0.05999999865889549 Reference Frame: Value: false - Class: rviz/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 7.801704406738281 Min Value: -4.55991268157959 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 238; 238; 236 Color Transformer: AxisColor Decay Time: 0.10000000149011612 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: surround Position Transformer: XYZ Queue Size: 1 Selectable: false Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 7 Min Value: -1 Value: false Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 1000 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: currPoints Position Transformer: XYZ Queue Size: 100000 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 0.38099047541618347 Min Value: -0.8690725564956665 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: Ground Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: /patchworkpp/ground Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false Enabled: true Name: mapping - Class: rviz/Group Displays: - Angle Tolerance: 0.009999999776482582 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 Shape: Alpha: 1 Axes Length: 0.699999988079071 Axes Radius: 0.10000000149011612 Color: 255; 85; 0 Head Length: 0 Head Radius: 0 Shaft Length: 0.05000000074505806 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /aft_mapped_to_init Unreliable: false Value: true Enabled: true Name: Odometry - Class: rviz/Axes Enabled: true Length: 0.699999988079071 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: true - Alpha: 0 Buffer Length: 2 Class: rviz/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 Head Length: 0 Length: 0.6000000238418579 Line Style: Billboards Line Width: 0.10000000149011612 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 25; 255; 255 Pose Style: None Radius: 0.07999999821186066 Shaft Diameter: 0.4000000059604645 Shaft Length: 0.4000000059604645 Topic: /path Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 239; 41; 41 Max Intensity: 0 Min Color: 239; 41; 41 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.05000000074505806 Style: Spheres Topic: /cloud_effected Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 13.139549255371094 Min Value: -32.08251953125 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 138; 226; 52 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 138; 226; 52 Min Color: 138; 226; 52 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /Laser_map Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /points_raw Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Marker Enabled: true Marker Topic: /planner_normal Name: Marker Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: true Image Topic: /camera/color/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: LiDAR Frame Rate: 10 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 25.7800235748291 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 6.16567850112915 Y: -7.189380645751953 Z: 7.152557373046875e-7 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.2247965335845947 Target Frame: LiDAR Value: ThirdPersonFollower (rviz) Yaw: 4.431344985961914 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1052 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 000000ff00000000fd0000000400000000000001e7000003c6fc0200000010fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001ff000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000240000001c10000001600fffffffb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002fb000000eb0000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d0061006700650100000252000001af0000000000000000fb0000000a0049006d00610067006501000002aa000001570000000000000000fb0000000a0049006d00610067006501000002930000016e00000000000000000000000100000152000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000039fc0100000002fb0000000800540069006d006500000000000000073d0000042700fffffffb0000000800540069006d0065010000000000000450000000000000000000000593000003c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1920 X: 1920 Y: 0 ================================================ FILE: rviz_cfg/s3e.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 126 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /mapping1 - /mapping1/currPoints1/Autocompute Value Bounds1 - /Odometry1/Odometry1/Shape1 - /Path1 Splitter Ratio: 0.6432989835739136 Tree Height: 396 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: currPoints Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 1 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 100 Reference Frame: Value: false - Class: rviz/Axes Enabled: false Length: 0.699999988079071 Name: Axes Radius: 0.05999999865889549 Reference Frame: Value: false - Class: rviz/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 3.7455544471740723 Min Value: -1.0043435096740723 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 238; 238; 236 Color Transformer: AxisColor Decay Time: 0.10000000149011612 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: surround Position Transformer: XYZ Queue Size: 1 Selectable: false Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 7 Min Value: -1 Value: false Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 1000 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: currPoints Position Transformer: XYZ Queue Size: 100000 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 0.38099047541618347 Min Value: -0.8690725564956665 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: Ground Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: /patchworkpp/ground Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true Enabled: true Name: mapping - Class: rviz/Group Displays: - Angle Tolerance: 0.009999999776482582 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 Shape: Alpha: 1 Axes Length: 0.699999988079071 Axes Radius: 0.10000000149011612 Color: 255; 85; 0 Head Length: 0 Head Radius: 0 Shaft Length: 0.05000000074505806 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /aft_mapped_to_init Unreliable: false Value: true Enabled: true Name: Odometry - Class: rviz/Axes Enabled: true Length: 0.699999988079071 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: true - Alpha: 0 Buffer Length: 2 Class: rviz/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 Head Length: 0 Length: 0.6000000238418579 Line Style: Billboards Line Width: 0.05000000074505806 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 25; 255; 255 Pose Style: None Radius: 0.07999999821186066 Shaft Diameter: 0.4000000059604645 Shaft Length: 0.4000000059604645 Topic: /path Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 239; 41; 41 Max Intensity: 0 Min Color: 239; 41; 41 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.05000000074505806 Style: Spheres Topic: /cloud_effected Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 13.139549255371094 Min Value: -32.08251953125 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 138; 226; 52 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 138; 226; 52 Min Color: 138; 226; 52 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /Laser_map Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /points_raw Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Marker Enabled: true Marker Topic: /planner_normal Name: Marker Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: false Image Topic: /camera/color/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Image Enabled: true Image Topic: /Bob/left_camera Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: LiDAR Frame Rate: 10 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 21.599872589111328 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 2.402665138244629 Y: -5.730081558227539 Z: 2.6226043701171875e-6 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.589796781539917 Target Frame: LiDAR Value: ThirdPersonFollower (rviz) Yaw: 3.696343183517456 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1025 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 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 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1853 X: 67 Y: 27 ================================================ FILE: rviz_cfg/spinning.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /mapping1/currPoints1/Autocompute Value Bounds1 - /Odometry1/Odometry1/Shape1 Splitter Ratio: 0.6432291865348816 Tree Height: 475 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: surround Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 1 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 100 Reference Frame: Value: false - Class: rviz/Axes Enabled: false Length: 0.699999988079071 Name: Axes Radius: 0.05999999865889549 Reference Frame: Value: false - Class: rviz/Group Displays: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 18.853248596191406 Min Value: -1.1457874774932861 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 238; 238; 236 Color Transformer: AxisColor Decay Time: 0.10000000149011612 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: surround Position Transformer: XYZ Queue Size: 1 Selectable: false Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.10000000149011612 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 7 Min Value: -1 Value: false Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 1000 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: currPoints Position Transformer: XYZ Queue Size: 100000 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: /cloud_registered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true Enabled: true Name: mapping - Class: rviz/Group Displays: - Angle Tolerance: 0.009999999776482582 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.0010000000474974513 Shape: Alpha: 1 Axes Length: 0.699999988079071 Axes Radius: 0.10000000149011612 Color: 255; 85; 0 Head Length: 0 Head Radius: 0 Shaft Length: 0.05000000074505806 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /aft_mapped_to_init Unreliable: false Value: true Enabled: true Name: Odometry - Class: rviz/Axes Enabled: true Length: 0.699999988079071 Name: Axes Radius: 0.10000000149011612 Reference Frame: Value: true - Alpha: 0 Buffer Length: 2 Class: rviz/Path Color: 25; 255; 255 Enabled: true Head Diameter: 0 Head Length: 0 Length: 0.6000000238418579 Line Style: Billboards Line Width: 0.05000000074505806 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 25; 255; 255 Pose Style: None Radius: 0.07999999821186066 Shaft Diameter: 0.4000000059604645 Shaft Length: 0.4000000059604645 Topic: /path Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 239; 41; 41 Max Intensity: 0 Min Color: 239; 41; 41 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.05000000074505806 Style: Spheres Topic: /cloud_effected Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 13.139549255371094 Min Value: -32.08251953125 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 138; 226; 52 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 138; 226; 52 Min Color: 138; 226; 52 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /Laser_map Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /points_raw Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Marker Enabled: true Marker Topic: /planner_normal Name: Marker Namespaces: {} Queue Size: 100 Value: true - Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /projected_map Unreliable: false Use Timestamp: false Value: true - Class: rviz/Image Enabled: false Image Topic: /camera/infra1/image_rect_raw Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Image Enabled: true Image Topic: /camera/infra1/image_rect_raw Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: LiDAR Frame Rate: 10 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 68.43583679199219 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: -0.8573362827301025 Y: 1.0231647491455078 Z: 1.1920928955078125e-6 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.5297970175743103 Target Frame: LiDAR Value: ThirdPersonFollower (rviz) Yaw: 4.016337871551514 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1052 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 000000ff00000000fd000000040000000000000256000003c6fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000216000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000001600fffffffb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002620000019f0000000000000000fb0000000a0049006d0061006700650100000257000001aa0000001600ffffff0000000100000152000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000039fc0100000002fb0000000800540069006d006500000000000000073d0000042700fffffffb0000000800540069006d0065010000000000000450000000000000000000000524000003c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1920 X: 1920 Y: 0 ================================================ FILE: src/laserMapping.cpp ================================================ // This is an advanced implementation of the algorithm described in the // following paper: // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. // Modifier: Livox dev@livoxtech.com // Modifier : Taeyoung Kim (https://github.com/Taeyoung96) // Copyright 2013, Ji Zhang, Carnegie Mellon University // Further contributions copyright (c) 2016, Southwest Research Institute // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // #include "ros/package.h" // 1. Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // 2. Redistributions in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // 3. Neither the name of the copyright holder nor the names of its // contributors may be used to endorse or promote products derived from this // software without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. #include #include "IMU_Processing.hpp" #include "ros/package.h" #include #include #include #include #include #include #include #include #include #include #include #include #include "preprocess.h" #include #include // For visualize #ifndef DEPLOY #include "matplotlibcpp.h" namespace plt = matplotlibcpp; #endif // For AHRS algorithm #include "Fusion/Fusion.h" #define IMU_Hz (200) // replace this with actual sample rate TODO ros::Time estimate_timestamp_; // For ground segmentation #include "GroundSegmentation/patchworkpp.hpp" ros::Publisher pub_cloud; ros::Publisher pub_ground; ros::Publisher pub_non_ground; double time_taken; pcl::PointCloud curr_points; pcl::PointCloud ground_points; pcl::PointCloud non_ground_points; // For disable PCL complile lib, to use PointXYZIR #define PCL_NO_PRECOMPILE // For surface estimation #include #include double GYRO_FACTOR_; double ACC_FACTOR_ ; double GROUND_FACTOR_; // Origin LiDAR-IMU init #define LASER_POINT_COV (0.001) #define MAXN (720000) #define PUBFRAME_PERIOD (20) float DET_RANGE = 300.0f; const float MOV_THRESHOLD = 1.5f; mutex mtx_buffer; condition_variable sig_buffer; string root_dir = ROOT_DIR; string map_file_path, lid_topic, imu_topic; int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, \ effect_feat_num = 0, scan_count = 0, publish_count = 0; double res_mean_last = 0.05; double 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; double last_timestamp_lidar = 0, last_timestamp_imu = 0.0; double filter_size_surf_min = 0, filter_size_map_min = 0; double cube_len = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0; // Time Log Variables double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0; int kdtree_delete_counter = 0, kdtree_size_end = 0, add_point_size = 0; double search_time_rec[100000]; int lidar_type, pcd_save_interval = -1, pcd_index = 0; bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited = true; bool imu_en = false; bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false; bool runtime_pos_log = false, pcd_save_en = false, extrinsic_est_en = true, path_en = true; // Trajectory save bool traj_save_en = false; string traj_save_path; // LI-Init Parameters bool cut_frame = true, data_accum_finished = false, data_accum_start = false, online_calib_finish = false, refine_print = false; int cut_frame_num = 1, orig_odom_freq = 10, frame_num = 0; double time_lag_IMU_wtr_lidar = 0.0, move_start_time = 0.0, online_calib_starts_time = 0.0, mean_acc_norm = 9.81; double online_refine_time = 20.0; //unit: s double time_result = 0.0; vector Trans_LI_cov(3, 0.0005); vector Rot_LI_cov(3, 0.00005); V3D mean_acc = Zero3d; ofstream fout_result; vector cub_needrm; deque lidar_buffer; deque time_buffer; deque imu_buffer; vector> pointSearchInd_surf; vector Nearest_Points; bool point_selected_surf[100000] = {0}; float res_last[100000] = {0.0}; double total_residual; //surf feature in map PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); PointCloudXYZI::Ptr _featsArray; pcl::VoxelGrid downSizeFilterSurf; pcl::VoxelGrid downSizeFilterMap; KD_TREE ikdtree; M3D last_rot(M3D::Zero()); V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); V3D euler_cur; V3D position_last(Zero3d); V3D last_odom(Zero3d); //estimator inputs and output; MeasureGroup Measures; StatesGroup state; PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); pcl::PCDWriter pcd_writer; string all_points_dir; nav_msgs::Path path; nav_msgs::Odometry odomAftMapped; geometry_msgs::Quaternion geoQuat; geometry_msgs::PoseStamped msg_body_pose; sensor_msgs::Imu IMU_sync; shared_ptr p_pre(new Preprocess()); shared_ptr Calib_LI(new Gril_Calib()); boost::shared_ptr> PatchworkppGroundSeg; // visualize ground segmenation result template sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id = "aft_mapped") { sensor_msgs::PointCloud2 cloud_ROS; pcl::toROSMsg(cloud, cloud_ROS); cloud_ROS.header.frame_id = frame_id; return cloud_ROS; } float calc_dist(PointType p1, PointType p2) { float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); return d; } void calcBodyVar(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &var) { float range = sqrt(pb[0] * pb[0] + pb[1] * pb[1] + pb[2] * pb[2]); float range_var = range_inc * range_inc; Eigen::Matrix2d direction_var; direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0, pow(sin(DEG2RAD(degree_inc)), 2); Eigen::Vector3d direction(pb); direction.normalize(); Eigen::Matrix3d direction_hat; direction_hat << 0, -direction(2), direction(1), direction(2), 0, -direction(0), -direction(1), direction(0), 0; Eigen::Vector3d base_vector1(1, 1, -(direction(0) + direction(1)) / direction(2)); base_vector1.normalize(); Eigen::Vector3d base_vector2 = base_vector1.cross(direction); base_vector2.normalize(); Eigen::Matrix N; N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), base_vector1(2), base_vector2(2); Eigen::Matrix A = range * direction_hat * N; var = direction * range_var * direction.transpose() + A * direction_var * A.transpose(); } void SigHandle(int sig) { if (pcd_save_en && pcd_save_interval < 0){ all_points_dir = string(root_dir + "/PCD/PCD_all" + string(".pcd")); pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); } flg_exit = true; ROS_WARN("catch sig %d", sig); sig_buffer.notify_all(); } inline void dump_lio_state_to_log(FILE *fp) { V3D rot_ang(Log(state.rot_end)); fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle fprintf(fp, "%lf %lf %lf ", state.pos_end(0), state.pos_end(1), state.pos_end(2)); // Pos fprintf(fp, "%lf %lf %lf ", state.vel_end(0), state.vel_end(1), state.vel_end(2)); // Vel fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc fprintf(fp, "%lf %lf %lf ", state.bias_g(0), state.bias_g(1), state.bias_g(2)); // Bias_g fprintf(fp, "%lf %lf %lf ", state.bias_a(0), state.bias_a(1), state.bias_a(2)); // Bias_a fprintf(fp, "%lf %lf %lf ", state.gravity(0), state.gravity(1), state.gravity(2)); // Bias_a fprintf(fp, "\r\n"); fflush(fp); } void pointBodyToWorld(PointType const *const pi, PointType *const po) { V3D p_body(pi->x, pi->y, pi->z); V3D p_global(state.rot_end * (state.offset_R_L_I * p_body + state.offset_T_L_I) + state.pos_end); po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); po->normal_x = pi->normal_x; po->normal_y = pi->normal_y; po->normal_z = pi->normal_z; po->intensity = pi->intensity; } template void pointBodyToWorld(const Matrix &pi, Matrix &po) { V3D p_body(pi[0], pi[1], pi[2]); V3D p_global(state.rot_end * (state.offset_R_L_I * p_body + state.offset_T_L_I) + state.pos_end); po[0] = p_global(0); po[1] = p_global(1); po[2] = p_global(2); } void RGBpointBodyToWorld(PointType const *const pi, PointTypeRGB *const po) { V3D p_body(pi->x, pi->y, pi->z); V3D p_global(state.rot_end * (state.offset_R_L_I * p_body + state.offset_T_L_I) + state.pos_end); po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); po->r = pi->normal_x; po->g = pi->normal_y; po->b = pi->normal_z; float intensity = pi->intensity; intensity = intensity - floor(intensity); int reflection_map = intensity * 10000; } int points_cache_size = 0; void points_cache_collect() { PointVector points_history; ikdtree.acquire_removed_points(points_history); points_cache_size = points_history.size(); for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]); } BoxPointType LocalMap_Points; bool Localmap_Initialized = false; void lasermap_fov_segment() { cub_needrm.clear(); kdtree_delete_counter = 0; kdtree_delete_time = 0.0; pointBodyToWorld(XAxisPoint_body, XAxisPoint_world); V3D pos_LiD = state.pos_end; if (!Localmap_Initialized) { for (int i = 0; i < 3; i++) { LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; } Localmap_Initialized = true; return; } float dist_to_map_edge[3][2]; bool need_move = false; for (int i = 0; i < 3; i++) { dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true; } if (!need_move) return; BoxPointType New_LocalMap_Points, tmp_boxpoints; New_LocalMap_Points = LocalMap_Points; float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1))); for (int i = 0; i < 3; i++) { tmp_boxpoints = LocalMap_Points; if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) { New_LocalMap_Points.vertex_max[i] -= mov_dist; New_LocalMap_Points.vertex_min[i] -= mov_dist; tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; cub_needrm.push_back(tmp_boxpoints); } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) { New_LocalMap_Points.vertex_max[i] += mov_dist; New_LocalMap_Points.vertex_min[i] += mov_dist; tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; cub_needrm.push_back(tmp_boxpoints); } } LocalMap_Points = New_LocalMap_Points; points_cache_collect(); double delete_begin = omp_get_wtime(); if (cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); kdtree_delete_time = omp_get_wtime() - delete_begin; printf("Delete time: %0.6f, delete size: %d\n", kdtree_delete_time, kdtree_delete_counter); } double timediff_imu_wrt_lidar = 0.0; bool timediff_set_flg = false; // ** livox LiDAR callback ** // void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) { mtx_buffer.lock(); double preprocess_start_time = omp_get_wtime(); scan_count++; if (msg->header.stamp.toSec() < last_timestamp_lidar) { ROS_WARN("lidar loop back, clear buffer"); lidar_buffer.clear(); time_buffer.clear(); } last_timestamp_lidar = msg->header.stamp.toSec(); if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_buffer.empty()) { timediff_set_flg = true; timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar; printf("Self sync IMU and LiDAR, HARD time lag is %.10lf \n \n", timediff_imu_wrt_lidar); } if (cut_frame) { deque ptr; deque timestamp_lidar; p_pre->process_cut_frame_livox(msg, ptr, timestamp_lidar, cut_frame_num, scan_count); while (!ptr.empty() && !timestamp_lidar.empty()) { lidar_buffer.push_back(ptr.front()); ptr.pop_front(); time_buffer.push_back(timestamp_lidar.front() / double(1000));//unit:s timestamp_lidar.pop_front(); } } else { PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); p_pre->process(msg, ptr); lidar_buffer.push_back(ptr); time_buffer.push_back(last_timestamp_lidar); } mtx_buffer.unlock(); sig_buffer.notify_all(); } // ** mechnical LiDAR point cloud callback ** // void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) { mtx_buffer.lock(); scan_count++; double preprocess_start_time = omp_get_wtime(); if (msg->header.stamp.toSec() < last_timestamp_lidar) { ROS_ERROR("lidar loop back, clear Lidar buffer."); lidar_buffer.clear(); time_buffer.clear(); } last_timestamp_lidar = msg->header.stamp.toSec(); if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_buffer.empty()) { timediff_set_flg = true; timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar; printf("Self sync IMU and LiDAR, HARD time lag is %.10lf \n \n", timediff_imu_wrt_lidar); } if ((lidar_type == VELO || lidar_type == OUSTER || lidar_type == PANDAR || lidar_type == VELO_without_Time) && cut_frame) { deque ptr; deque timestamp_lidar; p_pre->process_cut_frame_pcl2(msg, ptr, timestamp_lidar, cut_frame_num, scan_count); while (!ptr.empty() && !timestamp_lidar.empty()) { lidar_buffer.push_back(ptr.front()); ptr.pop_front(); time_buffer.push_back(timestamp_lidar.front() / double(1000)); //unit:s timestamp_lidar.pop_front(); } } else { PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); p_pre->process(msg, ptr); lidar_buffer.push_back(ptr); time_buffer.push_back(msg->header.stamp.toSec()); } // Ground Segmentation TODO : other lidar type if (lidar_type == VELO || lidar_type == VELO_NCLT || lidar_type == OUSTER || lidar_type == PANDAR || lidar_type == VELO_without_Time) { pcl::fromROSMsg(*msg, curr_points); PatchworkppGroundSeg->estimate_ground(curr_points, ground_points, non_ground_points, time_taken); pub_cloud.publish(cloud2msg(curr_points)); pub_ground.publish(cloud2msg(ground_points)); pub_non_ground.publish(cloud2msg(non_ground_points)); } mtx_buffer.unlock(); sig_buffer.notify_all(); } // ** IMU callback ** // void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in, const ros::Publisher &pubIMU_sync) { publish_count++; mtx_buffer.lock(); static double IMU_period, time_msg_in, last_time_msg_in; static int imu_cnt = 0; time_msg_in = msg_in->header.stamp.toSec(); if (imu_cnt < 100) { imu_cnt++; mean_acc += (V3D(msg_in->linear_acceleration.x, msg_in->linear_acceleration.y, msg_in->linear_acceleration.z) - mean_acc) / (imu_cnt); if (imu_cnt > 1) { IMU_period += (time_msg_in - last_time_msg_in - IMU_period) / (imu_cnt - 1); } if (imu_cnt == 99) { cout << endl << "Acceleration norm : " << mean_acc.norm() << endl; if (IMU_period > 0.01) { cout << "IMU data frequency : " << 1 / IMU_period << " Hz" << endl; ROS_WARN("IMU data frequency too low. Higher than 150 Hz is recommended."); } cout << endl; } } last_time_msg_in = time_msg_in; sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); //IMU Time Compensation msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar); double timestamp = msg->header.stamp.toSec(); if (timestamp < last_timestamp_imu) { ROS_WARN("IMU loop back, clear IMU buffer."); imu_buffer.clear(); Calib_LI->IMU_buffer_clear(); } last_timestamp_imu = timestamp; imu_buffer.push_back(msg); // push all IMU meas into Calib_LI if (!imu_en && !data_accum_finished) Calib_LI->push_ALL_IMU_CalibState(msg, mean_acc_norm); mtx_buffer.unlock(); sig_buffer.notify_all(); } bool sync_packages(MeasureGroup &meas) { if (lidar_buffer.empty() || imu_buffer.empty()) return false; /** push a lidar scan **/ if (!lidar_pushed) { meas.lidar = lidar_buffer.front(); if (meas.lidar->points.size() <= 1) { ROS_WARN("Too few input point cloud!\n"); lidar_buffer.pop_front(); time_buffer.pop_front(); return false; } meas.lidar_beg_time = time_buffer.front(); //unit:s if (lidar_type == L515) lidar_end_time = meas.lidar_beg_time; else lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); //unit:s lidar_pushed = true; } if (last_timestamp_imu < lidar_end_time) return false; /** push imu data, and pop from imu buffer **/ double imu_time = imu_buffer.front()->header.stamp.toSec(); meas.imu.clear(); while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) { imu_time = imu_buffer.front()->header.stamp.toSec(); if (imu_time > lidar_end_time) break; meas.imu.push_back(imu_buffer.front()); imu_buffer.pop_front(); } lidar_buffer.pop_front(); time_buffer.pop_front(); lidar_pushed = false; return true; } void map_incremental() { PointVector PointToAdd; PointVector PointNoNeedDownsample; PointToAdd.reserve(feats_down_size); PointNoNeedDownsample.reserve(feats_down_size); for (int i = 0; i < feats_down_size; i++) { /* transform to world frame */ pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); /* decide if need add to map */ if (!Nearest_Points[i].empty() && flg_EKF_inited) { const PointVector &points_near = Nearest_Points[i]; bool need_add = true; BoxPointType Box_of_Point; PointType downsample_result, mid_point; mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; float dist = calc_dist(feats_down_world->points[i], mid_point); if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min) { PointNoNeedDownsample.push_back(feats_down_world->points[i]); continue; } for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i++) { if (points_near.size() < NUM_MATCH_POINTS) break; if (calc_dist(points_near[readd_i], mid_point) < dist) { need_add = false; break; } } if (need_add) PointToAdd.push_back(feats_down_world->points[i]); } else { PointToAdd.push_back(feats_down_world->points[i]); } } double st_time = omp_get_wtime(); add_point_size = ikdtree.Add_Points(PointToAdd, true); ikdtree.Add_Points(PointNoNeedDownsample, false); add_point_size = PointToAdd.size() + PointNoNeedDownsample.size(); kdtree_incremental_time = omp_get_wtime() - st_time; } void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes) { if (scan_pub_en) { PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); int size = laserCloudFullRes->points.size(); PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB(size, 1)); PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); for (int i = 0; i < size; i++) { if (lidar_type == L515) RGBpointBodyToWorld(&laserCloudFullRes->points[i], \ &laserCloudWorldRGB->points[i]); else pointBodyToWorld(&laserCloudFullRes->points[i], \ &laserCloudWorld->points[i]); } sensor_msgs::PointCloud2 laserCloudmsg; if (lidar_type == L515) pcl::toROSMsg(*laserCloudWorldRGB, laserCloudmsg); else pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudmsg.header.frame_id = "LiDAR"; pubLaserCloudFullRes.publish(laserCloudmsg); publish_count -= PUBFRAME_PERIOD; } /**************** save map ****************/ /* 1. make sure you have enough memories 2. noted that pcd save will influence the real-time performences **/ if (pcd_save_en) { boost::filesystem::create_directories(root_dir + "/PCD"); int size = feats_undistort->points.size(); PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); for (int i = 0; i < size; i++) { pointBodyToWorld(&feats_undistort->points[i], &laserCloudWorld->points[i]); } *pcl_wait_save += *laserCloudWorld; static int scan_wait_num = 0; scan_wait_num++; if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) { pcd_index++; all_points_dir = string(root_dir + "/PCD/PCD") + to_string(pcd_index) + string(".pcd"); cout << "current scan saved to " << all_points_dir << endl; pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); pcl_wait_save->clear(); scan_wait_num = 0; } } } void publish_frame_body(const ros::Publisher &pubLaserCloudFullRes_body) { PointCloudXYZI::Ptr laserCloudFullRes(feats_undistort); sensor_msgs::PointCloud2 laserCloudmsg; pcl::toROSMsg(*feats_undistort, laserCloudmsg); laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudmsg.header.frame_id = "LiDAR"; pubLaserCloudFullRes_body.publish(laserCloudmsg); } void publish_effect_world(const ros::Publisher &pubLaserCloudEffect) { PointCloudXYZI::Ptr laserCloudWorld(\ new PointCloudXYZI(effect_feat_num, 1)); for (int i = 0; i < effect_feat_num; i++) { pointBodyToWorld(&laserCloudOri->points[i], &laserCloudWorld->points[i]); } sensor_msgs::PointCloud2 laserCloudFullRes3; pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudFullRes3.header.frame_id = "LiDAR"; pubLaserCloudEffect.publish(laserCloudFullRes3); } void publish_map(const ros::Publisher &pubLaserCloudMap) { sensor_msgs::PointCloud2 laserCloudMap; pcl::toROSMsg(*featsFromMap, laserCloudMap); laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time); laserCloudMap.header.frame_id = "LiDAR"; pubLaserCloudMap.publish(laserCloudMap); } template void set_posestamp(T &out) { if (!imu_en) { out.position.x = state.pos_end(0); out.position.y = state.pos_end(1); out.position.z = state.pos_end(2); } else { //Pubulish LiDAR's pose and position V3D pos_cur_lidar = state.rot_end * state.offset_T_L_I + state.pos_end; out.position.x = pos_cur_lidar(0); out.position.y = pos_cur_lidar(1); out.position.z = pos_cur_lidar(2); } out.orientation.x = geoQuat.x; out.orientation.y = geoQuat.y; out.orientation.z = geoQuat.z; out.orientation.w = geoQuat.w; } void publish_odometry(const ros::Publisher &pubOdomAftMapped) { odomAftMapped.header.frame_id = "LiDAR"; odomAftMapped.child_frame_id = "aft_mapped"; odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); set_posestamp(odomAftMapped.pose.pose); pubOdomAftMapped.publish(odomAftMapped); static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \ odomAftMapped.pose.pose.position.y, \ odomAftMapped.pose.pose.position.z)); q.setW(odomAftMapped.pose.pose.orientation.w); q.setX(odomAftMapped.pose.pose.orientation.x); q.setY(odomAftMapped.pose.pose.orientation.y); q.setZ(odomAftMapped.pose.pose.orientation.z); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "LiDAR", "aft_mapped")); } void publish_path(const ros::Publisher pubPath) { set_posestamp(msg_body_pose.pose); msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); msg_body_pose.header.frame_id = "LiDAR"; static int jjj = 0; jjj++; if (jjj % 5 == 0) // if path is too large, the RVIZ will crash { path.poses.push_back(msg_body_pose); pubPath.publish(path); } } void fileout_calib_result() { fout_result.setf(ios::fixed); fout_result << setprecision(6) << "Rotation LiDAR to IMU (degree) = " << RotMtoEuler(state.offset_R_L_I).transpose() * 57.3 << endl; fout_result << "Translation LiDAR to IMU (meter) = " << state.offset_T_L_I.transpose() << endl; fout_result << "Time Lag IMU to LiDAR (second) = " << time_result << endl; fout_result << "Bias of Gyroscope (rad/s) = " << state.bias_g.transpose() << endl; fout_result << "Bias of Accelerometer (meters/s^2) = " << state.bias_a.transpose() << endl; fout_result << endl; MD(4, 4) Transform; Transform.setIdentity(); Transform.block<3, 3>(0, 0) = state.offset_R_L_I; Transform.block<3, 1>(0, 3) = state.offset_T_L_I; fout_result << "Homogeneous Transformation Matrix from LiDAR frmae L to IMU frame I: " << endl; fout_result << Transform << endl << endl << endl; } void printProgress(double percentage) { int val = (int) (percentage * 100); int lpad = (int) (percentage * PBWIDTH); int rpad = PBWIDTH - lpad; printf("\033[1A\r"); printf(BOLDMAGENTA "[Refinement] "); if (percentage < 1) { printf(BOLDYELLOW "Online Refinement: "); printf(YELLOW "%3d%% [%.*s%*s]\n", val, lpad, PBSTR, rpad, ""); cout << RESET; } else { printf(BOLDGREEN " Online Refinement "); printf(GREEN "%3d%% [%.*s%*s]\n", val, lpad, PBSTR, rpad, ""); cout << RESET; } fflush(stdout); } /* * @brief : Save the whole trajectory to a txt file */ void saveTrajectory(const std::string &traj_file) { std::string filename(traj_file); std::fstream output_fstream; output_fstream.open(filename, std::ios_base::out); if (!output_fstream.is_open()) { std::cerr << "Failed to open " << filename << '\n'; } else { output_fstream << "#timestamp x y z q_x q_y q_z q_w" << std::endl; for (const auto &p : path.poses) { output_fstream << std::setprecision(15) << p.header.stamp.toSec() << " " << p.pose.position.x << " " << p.pose.position.y << " " << p.pose.position.z << " " << p.pose.orientation.x << " " << p.pose.orientation.y << " " << p.pose.orientation.z << " " << p.pose.orientation.w << std::endl; } } } /* * @brief : Caculate the rotation matrix (quaternion)) * from LiDAR ground point cloud normal vector frame to earth gravity vector frame * @output : quaternion (from world gravity frame to LiDAR plane frame) * @output : estimated height of LiDAR sensor */ void get_quat_LiDAR_plane_to_gravity(Eigen::Quaterniond &lidar_q, V3D& normal_vector, double &lidar_estimate_height) { /** Normal vector estimation as a result of ground segmentation **/ Eigen::Vector4f plane_parameters; float curvature; computePointNormal(ground_points, plane_parameters, curvature); /** Roll, pitch, and yaw estimation with respect to the Earth's gravity direction vector using the ground plane normal vector **/ V3D normal_plane = {plane_parameters[0], plane_parameters[1], plane_parameters[2]}; normal_plane.normalize(); V3D normal_gravity{0, 0, 1.0}; // the built rotation represent a rotation sending the line of direction a to the line of direction b, both lines passing through the origin. // (from world gravity frame to LiDAR plane frame) lidar_q = Eigen::Quaterniond::FromTwoVectors(normal_gravity, normal_plane); lidar_estimate_height = plane_parameters[3]; normal_vector = normal_plane; } int main(int argc, char **argv) { ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh; nh.param("max_iteration", NUM_MAX_ITERATIONS, 4); nh.param("point_filter_num", p_pre->point_filter_num, 2); nh.param("map_file_path", map_file_path, ""); nh.param("common/lid_topic", lid_topic, "/livox/lidar"); nh.param("common/imu_topic", imu_topic, "/livox/imu"); nh.param("mapping/filter_size_surf", filter_size_surf_min, 0.5); nh.param("mapping/filter_size_map", filter_size_map_min, 0.5); nh.param("cube_side_length", cube_len, 200); nh.param("mapping/det_range", DET_RANGE, 300.f); nh.param("mapping/gyr_cov", gyr_cov, 0.1); nh.param("mapping/acc_cov", acc_cov, 0.1); nh.param("mapping/grav_cov", grav_cov, 0.001); nh.param("mapping/b_gyr_cov", b_gyr_cov, 0.0001); nh.param("mapping/b_acc_cov", b_acc_cov, 0.0001); nh.param("mapping/ground_cov", ground_cov, 1000.0); nh.param("preprocess/blind", p_pre->blind, 1.0); nh.param("preprocess/lidar_type", lidar_type, AVIA); nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); nh.param("preprocess/feature_extract_en", p_pre->feature_enabled, 0); nh.param("calibration/cut_frame", cut_frame, true); nh.param("calibration/cut_frame_num", cut_frame_num, 1); nh.param("calibration/orig_odom_freq", orig_odom_freq, 10); nh.param("calibration/mean_acc_norm", mean_acc_norm, 9.81); nh.param("calibration/data_accum_length", Calib_LI->data_accum_length, 300); nh.param("calibration/x_accumulate", Calib_LI->x_accumulate, 0.1); nh.param("calibration/y_accumulate", Calib_LI->y_accumulate, 0.1); nh.param("calibration/z_accumulate", Calib_LI->z_accumulate, 0.1); nh.param("calibration/imu_sensor_height", Calib_LI->imu_sensor_height, 0.1); nh.param("calibration/verbose", Calib_LI->verbose , false); nh.param("calibration/trans_IL_x", Calib_LI->trans_IL_x, 0.0); nh.param("calibration/trans_IL_y", Calib_LI->trans_IL_y, 0.0); nh.param("calibration/trans_IL_z", Calib_LI->trans_IL_z, 0.0); nh.param("calibration/bound_th", Calib_LI->bound_th, 0.1); nh.param("calibration/set_boundary", Calib_LI->set_boundary, false); nh.param("calibration/gyro_factor", GYRO_FACTOR_, 1.0); nh.param("calibration/acc_factor", ACC_FACTOR_, 1.0); nh.param("calibration/ground_factor", GROUND_FACTOR_, 1.0); nh.param("publish/path_en", path_en, true); nh.param("publish/scan_publish_en", scan_pub_en, 1); nh.param("publish/dense_publish_en", dense_pub_en, 1); nh.param("publish/scan_bodyframe_pub_en", scan_body_pub_en, 1); nh.param("runtime_pos_log_enable", runtime_pos_log, 0); nh.param("pcd_save/pcd_save_en", pcd_save_en, false); nh.param("pcd_save/interval", pcd_save_interval, -1); nh.param("trajectory_save/traj_save_en", traj_save_en, false); nh.param("trajectory_save/traj_file_path", traj_save_path, "/home/catkin_ws/src/result/traj.txt"); cout << "lidar_type: " << lidar_type << endl; cout << "LiDAR-only odometry starts." << endl; path.header.stamp = ros::Time().fromSec(lidar_end_time); path.header.frame_id = "LiDAR"; /*** variables definition ***/ VD(DIM_STATE) solution; MD(DIM_STATE, DIM_STATE) G, H_T_H, I_STATE; V3D rot_add, T_add, vel_add, gyr_add; StatesGroup state_propagat; PointType pointOri, pointSel, coeff; double deltaT, deltaR; bool flg_EKF_converged, EKF_stop_flg = 0; _featsArray.reset(new PointCloudXYZI()); memset(point_selected_surf, true, sizeof(point_selected_surf)); memset(res_last, -1000.0f, sizeof(res_last)); downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); memset(point_selected_surf, true, sizeof(point_selected_surf)); memset(res_last, -1000.0f, sizeof(res_last)); shared_ptr p_imu(new ImuProcess()); p_imu->lidar_type = p_pre->lidar_type = lidar_type; p_imu->imu_en = imu_en; p_imu->Gril_Calib_done = false; p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); p_imu->set_R_LI_cov(V3D(VEC_FROM_ARRAY(Rot_LI_cov))); p_imu->set_T_LI_cov(V3D(VEC_FROM_ARRAY(Trans_LI_cov))); p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); G.setZero(); H_T_H.setZero(); I_STATE.setIdentity(); /*** Ground Segmentation ***/ cout << "Operating patchwork++..." << endl; PatchworkppGroundSeg.reset(new PatchWorkpp(&nh)); // LI Init Related MatrixXd Jaco_rot(30000, 3); Jaco_rot.setZero(); /*** debug record ***/ ofstream fout_out; fout_out.open(DEBUG_FILE_DIR("state_out.txt"), ios::out); fout_result.open(RESULT_FILE_DIR("GRIL_Calib_result.txt"), ios::out); if (fout_out) cout << "~~~~" << ROOT_DIR << " file opened" << endl; else cout << "~~~~" << ROOT_DIR << " doesn't exist" << endl; /*** ROS subscribe initialization ***/ ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \ nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ nh.subscribe(lid_topic, 200000, standard_pcl_cbk); ros::Publisher pubIMU_sync = nh.advertise ("/livox/imu/async", 100000); ros::Subscriber sub_imu = nh.subscribe (imu_topic, 200000, boost::bind(&imu_cbk, _1, pubIMU_sync)); ros::Publisher pubLaserCloudFullRes = nh.advertise ("/cloud_registered", 100000); ros::Publisher pubLaserCloudFullRes_body = nh.advertise ("/cloud_registered_body", 100000); ros::Publisher pubLaserCloudEffect = nh.advertise ("/cloud_effected", 100000); ros::Publisher pubLaserCloudMap = nh.advertise ("/Laser_map", 100000); ros::Publisher pubOdomAftMapped = nh.advertise ("/aft_mapped_to_init", 100000); ros::Publisher pubPath = nh.advertise ("/path", 100000); pub_cloud = nh.advertise("/patchworkpp/cloud", 100, true); pub_ground = nh.advertise("/patchworkpp/ground", 100, true); pub_non_ground = nh.advertise("/patchworkpp/nonground", 100, true); //** For AHRS algirithm **// // Define calibration (replace with actual calibration data) const FusionMatrix gyroscopeMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f}; const FusionVector gyroscopeSensitivity = {1.0f, 1.0f, 1.0f}; const FusionVector gyroscopeOffset = {0.0f, 0.0f, 0.0f}; const FusionMatrix accelerometerMisalignment = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f}; const FusionVector accelerometerSensitivity = {1.0f, 1.0f, 1.0f}; const FusionVector accelerometerOffset = {0.0f, 0.0f, 0.0f}; // IMU AHRS algorithm initialization FusionOffset offset; FusionAhrs ahrs; FusionOffsetInitialise(&offset, IMU_Hz); FusionAhrsInitialise(&ahrs); // Set AHRS algorithm settings const FusionAhrsSettings settings = { .gain = 0.5f, .accelerationRejection = 10.0f, .magneticRejection = 0.0f, .rejectionTimeout = 5 * IMU_Hz, // TODO : Change IMU hz }; FusionAhrsSetSettings(&ahrs, &settings); FusionEuler euler_imu = {0.0f, 0.0f, 0.0f}; // roll, pitch, yaw V3D euler_lidar = V3D(0, 0, 0); FusionQuaternion imu_q = {1.0f, 0.0f, 0.0f, 0.0f}; // qw, qx, qy, qz (from earth frame to IMU frame) 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) Eigen::Quaterniond lidar_q = Eigen::Quaterniond(1, 0, 0, 0); // qw, qx, qy, qz (from earth frame to lidar frame) V3D normal_lidar = V3D(0, 0, 1); // normal vector of lidar frame double lidar_estimate_height = 0.0; //------------------------------------------------------------------------------------------------------ signal(SIGINT, SigHandle); ros::Rate rate(5000); bool status = ros::ok(); while (status) { if (flg_exit) break; ros::spinOnce(); if (sync_packages(Measures)) { if (flg_reset) { ROS_WARN("reset when rosbag play back."); p_imu->Reset(); flg_reset = false; continue; } kdtree_search_time = 0.0; /** IMU pre-processing **/ p_imu->Process(Measures, state, feats_undistort); state_propagat = state; if (feats_undistort->empty() || (feats_undistort == NULL)) { first_lidar_time = Measures.lidar_beg_time; p_imu->first_lidar_time = first_lidar_time; ROS_WARN("FAST-LIO not ready, no points stored."); online_calib_starts_time = first_lidar_time; continue; } /** AHRS algorithm and estimate Rotation matrix (from earth to IMU & from earth to LiDAR plane) **/ auto v_imu = Measures.imu; auto imu_timestamp = Measures.imu.front()->header.stamp; // first imu ROS timestamp estimate_timestamp_ = imu_timestamp; for (auto imu_msg : v_imu) { const sensor_msgs::Imu thisImu = *imu_msg; float delta_time = (thisImu.header.stamp - estimate_timestamp_).toSec(); // unit : s auto gyr = thisImu.angular_velocity; // gyr : rad/s -> deg/s FusionVector gyroscope = {static_cast(rad2deg(gyr.x)), static_cast(rad2deg(gyr.y)), static_cast(rad2deg(gyr.z))}; // TODO : IMU gyro calibration gyroscope = FusionCalibrationInertial(gyroscope, gyroscopeMisalignment, gyroscopeSensitivity, gyroscopeOffset); // Update gyroscope offset correction algorithm gyroscope = FusionOffsetUpdate(&offset, gyroscope); // ref : http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html auto acc = thisImu.linear_acceleration; // acc : m/s^2 -> g (SI unit to g unit) // ref : https://github.com/Livox-SDK/livox_ros_driver/issues/63 FusionVector accelerometer = {static_cast(SI2g(acc.x)), static_cast(SI2g(acc.y)), static_cast(SI2g(acc.z))}; // TODO : IMU acc calibration accelerometer = FusionCalibrationInertial(accelerometer, accelerometerMisalignment, accelerometerSensitivity, accelerometerOffset); FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, delta_time); FusionAhrsFlags ahrsFlag = FusionAhrsGetFlags(&ahrs); bool ahrsInitializing = ahrsFlag.initialising; FusionAhrsInternalStates ahrsState = FusionAhrsGetInternalStates(&ahrs); // Initalization is finished if(!ahrsInitializing){ // from ground frame to IMU frame euler_imu = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); imu_q = FusionAhrsGetQuaternion(&ahrs); imu_q_eigen = Eigen::Quaterniond(imu_q.element.w, imu_q.element.x, imu_q.element.y, imu_q.element.z); // from ground frame to LiDAR frame get_quat_LiDAR_plane_to_gravity(lidar_q, normal_lidar,lidar_estimate_height); } estimate_timestamp_ = thisImu.header.stamp; } /*** Segment the map in lidar FOV ***/ lasermap_fov_segment(); /*** downsample the feature points in a scan ***/ downSizeFilterSurf.setInputCloud(feats_undistort); downSizeFilterSurf.filter(*feats_down_body); feats_down_size = feats_down_body->points.size(); /*** initialize the map kdtree ***/ if (ikdtree.Root_Node == nullptr) { if (feats_down_size > 5) { ikdtree.set_downsample_param(filter_size_map_min); feats_down_world->resize(feats_down_size); for (int i = 0; i < feats_down_size; i++) { pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); } ikdtree.Build(feats_down_world->points); } continue; } /*** ICP and iterated Kalman filter update ***/ normvec->resize(feats_down_size); feats_down_world->resize(feats_down_size); euler_cur = RotMtoEuler(state.rot_end); pointSearchInd_surf.resize(feats_down_size); Nearest_Points.resize(feats_down_size); int rematch_num = 0; bool nearest_search_en = true; /*** iterated state estimation ***/ std::vector body_var; std::vector crossmat_list; body_var.reserve(feats_down_size); crossmat_list.reserve(feats_down_size); double t_update_start = omp_get_wtime(); for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) { laserCloudOri->clear(); corr_normvect->clear(); total_residual = 0.0; /** closest surface search and residual computation **/ #ifdef MP_EN omp_set_num_threads(MP_PROC_NUM); #pragma omp parallel for #endif for (int i = 0; i < feats_down_size; i++) { PointType &point_body = feats_down_body->points[i]; PointType &point_world = feats_down_world->points[i]; V3D p_body(point_body.x, point_body.y, point_body.z); /// transform to world frame pointBodyToWorld(&point_body, &point_world); vector pointSearchSqDis(NUM_MATCH_POINTS); auto &points_near = Nearest_Points[i]; uint8_t search_flag = 0; double search_start = omp_get_wtime(); if (nearest_search_en) { /** Find the closest surfaces in the map **/ ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 5); if (points_near.size() < NUM_MATCH_POINTS) point_selected_surf[i] = false; else point_selected_surf[i] = !(pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5); search_time_rec[i] = omp_get_wtime() - search_start; } res_last[i] = -1000.0f; if (!point_selected_surf[i] || points_near.size() < NUM_MATCH_POINTS) { point_selected_surf[i] = false; continue; } point_selected_surf[i] = false; VD(4) pabcd; pabcd.setZero(); if (esti_plane(pabcd, points_near, 0.1)) //(planeValid) { float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3); float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm()); if (s > 0.9) { point_selected_surf[i] = true; normvec->points[i].x = pabcd(0); normvec->points[i].y = pabcd(1); normvec->points[i].z = pabcd(2); normvec->points[i].intensity = pd2; res_last[i] = abs(pd2); } } } effect_feat_num = 0; for (int i = 0; i < feats_down_size; i++) { if (point_selected_surf[i]) { laserCloudOri->points[effect_feat_num] = feats_down_body->points[i]; corr_normvect->points[effect_feat_num] = normvec->points[i]; effect_feat_num++; } } res_mean_last = total_residual / effect_feat_num; /*** Computation of Measurement Jacobian matrix H and measurents vector ***/ int residual_dim = effect_feat_num + 3; // Ground plane residual MatrixXd Hsub(residual_dim, 12); MatrixXd Hsub_T_R_inv(12, residual_dim); VectorXd R_inv(residual_dim); VectorXd meas_vec(residual_dim); Hsub.setZero(); Hsub_T_R_inv.setZero(); meas_vec.setZero(); /*** Measurement: point-to-plane ***/ for (int i = 0; i < effect_feat_num; i++) { const PointType &laser_p = laserCloudOri->points[i]; V3D point_this_L(laser_p.x, laser_p.y, laser_p.z); V3D point_this = state.offset_R_L_I * point_this_L + state.offset_T_L_I; M3D var; calcBodyVar(point_this, 0.02, 0.05, var); var = state.rot_end * var * state.rot_end.transpose(); M3D point_crossmat; point_crossmat << SKEW_SYM_MATRX(point_this); /*** get the normal vector of closest surface/corner ***/ const PointType &norm_p = corr_normvect->points[i]; V3D norm_vec(norm_p.x, norm_p.y, norm_p.z); R_inv(i) = 1000; laserCloudOri->points[i].intensity = sqrt(R_inv(i)); /*** calculate the Measurement Jacobian matrix H ***/ if (imu_en) { M3D point_this_L_cross; point_this_L_cross << SKEW_SYM_MATRX(point_this_L); V3D H_R_LI = point_this_L_cross * state.offset_R_L_I.transpose() * state.rot_end.transpose() * norm_vec; V3D H_T_LI = state.rot_end.transpose() * norm_vec; V3D A(point_crossmat * state.rot_end.transpose() * norm_vec); Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY( H_R_LI), VEC_FROM_ARRAY(H_T_LI); } else { V3D A(point_crossmat * state.rot_end.transpose() * norm_vec); Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, 0, 0, 0, 0, 0, 0; } Hsub_T_R_inv.col(i) = Hsub.row(i).transpose() * 1000; /*** Measurement: distance to the closest surface/corner ***/ meas_vec(i) = -norm_p.intensity; } /*** Ground plane residual : Calculate the Measurement Jacobian matrix H ***/ M3D R_GL_ = lidar_q.toRotationMatrix(); M3D R_LG_ = R_GL_.transpose(); V3D e1(1.0, 0.0, 0.0); V3D e2(0.0, 1.0, 0.0); V3D e3(0.0, 0.0, 1.0); M3D skew_sym_ground_norm; skew_sym_ground_norm << SKEW_SYM_MATRX(normal_lidar); V3D Jac_gp_first = skew_sym_ground_norm * R_LG_ * state.rot_end * e1; V3D Jac_gp_second = skew_sym_ground_norm * R_LG_ * state.rot_end * e2; V3D Jac_gp_third = R_LG_ * e3; Hsub.row(effect_feat_num) << VEC_FROM_ARRAY(Jac_gp_first), 0, 0, 0, 0, 0, 0, 0, 0, 0; Hsub_T_R_inv.col(effect_feat_num) = Hsub.row(effect_feat_num).transpose() * ground_cov; R_inv(effect_feat_num) = ground_cov; Hsub.row(effect_feat_num+1) << VEC_FROM_ARRAY(Jac_gp_second), 0, 0, 0, 0, 0, 0, 0, 0, 0; Hsub_T_R_inv.col(effect_feat_num+1) = Hsub.row(effect_feat_num+1).transpose() * ground_cov; R_inv(effect_feat_num+1) = ground_cov; Hsub.row(effect_feat_num+2) << 0, 0, 0, VEC_FROM_ARRAY(Jac_gp_third), 0, 0, 0, 0, 0, 0; Hsub_T_R_inv.col(effect_feat_num+2) = Hsub.row(effect_feat_num+2).transpose() * ground_cov; R_inv(effect_feat_num+2) = ground_cov; /* Ground plane residual */ V3D meas_gp_ = R_LG_ * state.rot_end * normal_lidar; V3D meas_gp_last = R_LG_ * state.pos_end; double meas_gp_first_result = e1.transpose() * meas_gp_; double meas_gp_second_result = e2.transpose() * meas_gp_; double meas_gp_last_result = e3.transpose() * meas_gp_last; meas_vec(effect_feat_num) = meas_gp_first_result; meas_vec(effect_feat_num+1) = meas_gp_second_result; meas_vec(effect_feat_num+2) = meas_gp_last_result - 1.0; /* Kalman Gain */ MatrixXd K(DIM_STATE, residual_dim); EKF_stop_flg = false; flg_EKF_converged = false; /*** Iterative Kalman Filter Update ***/ H_T_H.block<12, 12>(0, 0) = Hsub_T_R_inv * Hsub; MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + state.cov.inverse()).inverse(); K = K_1.block(0, 0) * Hsub_T_R_inv; auto vec = state_propagat - state; solution = K * meas_vec + vec - K * Hsub * vec.block<12, 1>(0, 0); //state update state += solution; rot_add = solution.block<3, 1>(0, 0); T_add = solution.block<3, 1>(3, 0); if ((rot_add.norm() * 57.3 < 0.01) && (T_add.norm() * 100 < 0.015)) flg_EKF_converged = true; deltaR = rot_add.norm() * 57.3; deltaT = T_add.norm() * 100; euler_cur = RotMtoEuler(state.rot_end); /*** Rematch Judgement ***/ nearest_search_en = false; if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (NUM_MAX_ITERATIONS - 2)))) { nearest_search_en = true; rematch_num++; } /*** Convergence Judgements and Covariance Update ***/ if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == NUM_MAX_ITERATIONS - 1))) { if (flg_EKF_inited) { /*** Covariance Update ***/ G.setZero(); G.block(0, 0) = K * Hsub; state.cov = (I_STATE - G) * state.cov; total_distance += (state.pos_end - position_last).norm(); position_last = state.pos_end; if (!imu_en) { geoQuat = tf::createQuaternionMsgFromRollPitchYaw (euler_cur(0), euler_cur(1), euler_cur(2)); } else { //Publish LiDAR's pose, instead of IMU's pose M3D rot_cur_lidar = state.rot_end * state.offset_R_L_I; V3D euler_cur_lidar = RotMtoEuler(rot_cur_lidar); geoQuat = tf::createQuaternionMsgFromRollPitchYaw (euler_cur_lidar(0), euler_cur_lidar(1), euler_cur_lidar(2)); } VD(DIM_STATE) K_sum = K.rowwise().sum(); VD(DIM_STATE) P_diag = state.cov.diagonal(); } EKF_stop_flg = true; } if (EKF_stop_flg) break; } for (int i = 0; i < feats_down_size; i++) kdtree_search_time += search_time_rec[i]; /******* Publish odometry *******/ publish_odometry(pubOdomAftMapped); /*** add the feature points to map kdtree ***/ map_incremental(); kdtree_size_end = ikdtree.size(); /***** Device starts to move, data accmulation begins. ****/ if (!imu_en && !data_accum_start && state.pos_end.norm() > 0.05) { printf(BOLDCYAN "[Initialization] Movement detected, data accumulation starts.\n\n\n\n\n" RESET); data_accum_start = true; move_start_time = lidar_end_time; } /******* Publish points *******/ if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes); if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body); last_odom = state.pos_end; last_rot = state.rot_end; publish_effect_world(pubLaserCloudEffect); if (path_en) publish_path(pubPath); frame_num++; V3D ext_euler = RotMtoEuler(state.offset_R_L_I); // for mat_out.txt fout_out << euler_cur.transpose() * 57.3 << " " << state.pos_end.transpose() << " " << ext_euler.transpose() * 57.3 << " " \ << state.offset_T_L_I.transpose() << " " << state.vel_end.transpose() << " " \ << " " << state.bias_g.transpose() << " " << state.bias_a.transpose() * 0.9822 / 9.81 << " " << state.gravity.transpose() << " " << total_distance << endl; if (!imu_en && !data_accum_finished && data_accum_start) { // Push Lidar's Angular velocity and linear velocity Calib_LI->push_Lidar_CalibState(state.rot_end, state.pos_end, state.bias_g, state.vel_end, lidar_end_time); Calib_LI->push_Plane_Constraint(lidar_q, imu_q_eigen, normal_lidar, lidar_estimate_height); //Data Accumulation Sufficience Appraisal data_accum_finished = Calib_LI->data_sufficiency_assess(Jaco_rot, frame_num, state.bias_g, orig_odom_freq, cut_frame_num, lidar_q, imu_q_eigen, lidar_estimate_height); if (data_accum_finished) { Calib_LI->LI_Calibration(orig_odom_freq, cut_frame_num, timediff_imu_wrt_lidar, move_start_time); online_calib_starts_time = lidar_end_time; // Transfer to state vector state.offset_R_L_I = Calib_LI->get_R_LI(); state.offset_T_L_I = Calib_LI->get_T_LI(); state.pos_end = -state.rot_end * state.offset_R_L_I.transpose() * state.offset_T_L_I + state.pos_end; // Body frame is IMU frame in FAST-LIO mode state.rot_end = state.rot_end * state.offset_R_L_I.transpose(); state.gravity = Calib_LI->get_Grav_L0(); state.bias_g = Calib_LI->get_gyro_bias(); state.bias_a = Calib_LI->get_acc_bias(); time_result = Calib_LI->get_time_result(); // Output calibration result fout_result << "LiDAR-IMU calibration result:" << endl; fileout_calib_result(); // Save LiDAR trajectory if(traj_save_en){ saveTrajectory(traj_save_path); std::cout << "save LiDAR trajectory !!" << std::endl; } // ros shutdown ros::shutdown(); } } } status = ros::ok(); rate.sleep(); } return 0; } ================================================ FILE: src/preprocess.cpp ================================================ #include "preprocess.h" #define RETURN0 0x00 #define RETURN0AND1 0x10 const bool time_list_cut_frame(PointType &x, PointType &y) { return (x.curvature < y.curvature); } Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(1.0), point_filter_num(1) { inf_bound = 10; N_SCANS = 6; group_size = 8; disA = 0.01; disA = 0.1; // B? p2l_ratio = 225; limit_maxmid = 6.25; limit_midmin = 6.25; limit_maxmin = 3.24; jump_up_limit = 170.0; jump_down_limit = 8.0; cos160 = 160.0; edgea = 2; edgeb = 0.1; smallp_intersect = 172.5; smallp_ratio = 1.2; given_offset_time = false; jump_up_limit = cos(jump_up_limit / 180 * M_PI); jump_down_limit = cos(jump_down_limit / 180 * M_PI); cos160 = cos(cos160 / 180 * M_PI); smallp_intersect = cos(smallp_intersect / 180 * M_PI); } Preprocess::~Preprocess() {} void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) { feature_enabled = feat_en; lidar_type = lid_type; blind = bld; point_filter_num = pfilt_num; } void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { avia_handler(msg); *pcl_out = pl_surf; } void Preprocess::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) { int plsize = msg->point_num; pl_surf.clear(); pl_surf.reserve(plsize); pl_full.clear(); pl_full.resize(plsize); int valid_point_num = 0; for (uint i = 1; i < plsize; i++) { if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { valid_point_num++; if (valid_point_num % point_filter_num == 0) { pl_full[i].x = msg->points[i].x; pl_full[i].y = msg->points[i].y; pl_full[i].z = msg->points[i].z; pl_full[i].intensity = msg->points[i].reflectivity; //use curvature as time of each laser points，unit: ms pl_full[i].curvature = msg->points[i].offset_time / float(1000000); 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; if (dist < blind) continue; if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) { pl_surf.push_back(pl_full[i]); } } } } sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame); //end time of last frame，单位ms double last_frame_end_time = msg->header.stamp.toSec() * 1000; uint valid_num = 0; uint cut_num = 0; uint valid_pcl_size = pl_surf.points.size(); int required_cut_num = required_frame_num; if (scan_count < 5) required_cut_num = 1; PointCloudXYZI pcl_cut; for (uint i = 1; i < valid_pcl_size; i++) { valid_num++; //Compute new opffset time of each point：ms pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time; pcl_cut.push_back(pl_surf[i]); if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) { cut_num++; time_lidar.push_back(last_frame_end_time); PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI()); //Initialize shared_ptr *pcl_temp = pcl_cut; pcl_out.push_back(pcl_temp); //Update frame head last_frame_end_time += pl_surf[i].curvature; pcl_cut.clear(); pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num); } } } #define MAX_LINE_NUM 128 void Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); if (lidar_type == VELO) { pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); pl_surf.reserve(plsize); bool is_first[MAX_LINE_NUM]; double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point double omega_l = 3.61; // scan angular velocity (deg/ms) float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point float time_last[MAX_LINE_NUM] = {0.0}; // last offset time // if (pl_orig.points[plsize - 1].time > 0) { // given_offset_time = true; // } else { // cout << "Compute offset time using constant rotation model." << endl; // given_offset_time = false; // memset(is_first, true, sizeof(is_first)); // } if (pl_orig.points[plsize - 1].time > 0 && pl_orig.points[0].time > 0) { cout << "Use given offset time." << endl; given_offset_time = true; } else { // cout << "Compute offset time using constant rotation model." << endl; given_offset_time = false; memset(is_first, true, sizeof(is_first)); double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; double yaw_end = yaw_first; int layer_first = pl_orig.points[0].ring; for (uint i = plsize - 1; i > 0; i--) { if (pl_orig.points[i].ring == layer_first) { yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; break; } } } for (int i = 0; i < plsize; i++) { PointType added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.curvature = pl_orig.points[i].time * 1000.0; //ms double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z; if ( dist < blind * blind || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z)) continue; if (!given_offset_time) { int layer = pl_orig.points[i].ring; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; if (is_first[layer]) { yaw_fp[layer] = yaw_angle; is_first[layer] = false; added_pt.curvature = 0.0; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; continue; } // compute offset time if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; } if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) { pl_surf.points.push_back(added_pt); } } } else if (lidar_type == VELO_without_Time) { pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); pl_surf.reserve(plsize); bool is_first[MAX_LINE_NUM]; double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point double omega_l = 3.61; // scan angular velocity (deg/ms) float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point float time_last[MAX_LINE_NUM] = {0.0}; // last offset time memset(is_first, true, sizeof(is_first)); double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; double yaw_end = yaw_first; int layer_first = pl_orig.points[0].ring; for (uint i = plsize - 1; i > 0; i--) { if (pl_orig.points[i].ring == layer_first) { yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; break; } } for (int i = 0; i < plsize; i++) { PointType added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; int layer = pl_orig.points[i].ring; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; if (is_first[layer]) { yaw_fp[layer] = yaw_angle; is_first[layer] = false; added_pt.curvature = 0.0; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; continue; } // compute offset time if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) { if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind) pl_surf.points.push_back(added_pt); } } } else if (lidar_type == OUSTER) { pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); pl_surf.reserve(plsize); double time_stamp = msg->header.stamp.toSec(); for (int i = 0; i < pl_orig.points.size(); i++) { if (i % point_filter_num != 0) continue; double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < blind) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; if (yaw_angle >= 180.0) yaw_angle -= 360.0; if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].t / 1e6; pl_surf.points.push_back(added_pt); } } else { ROS_ERROR("Lidar type not supported!"); } sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame); //ms double last_frame_end_time = msg->header.stamp.toSec() * 1000; uint valid_num = 0; uint cut_num = 0; uint valid_pcl_size = pl_surf.points.size(); int required_cut_num = required_frame_num; if (scan_count < 20) required_cut_num = 1; PointCloudXYZI pcl_cut; for (uint i = 1; i < valid_pcl_size; i++) { valid_num++; pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time; pcl_cut.push_back(pl_surf[i]); if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) { cut_num++; time_lidar.push_back(last_frame_end_time); PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI()); *pcl_temp = pcl_cut; pcl_out.push_back(pcl_temp); last_frame_end_time += pl_surf[i].curvature; pcl_cut.clear(); pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num); } } } void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { switch (lidar_type) { case OUSTER: oust_handler(msg); break; case VELO: velodyne_handler(msg); break; case L515: l515_handler(msg); break; case VELO_NCLT: velodyne_handler_nclt(msg); break; default: printf("Error LiDAR Type"); break; } *pcl_out = pl_surf; } void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); double t1 = omp_get_wtime(); int plsize = msg->point_num; pl_corn.reserve(plsize); pl_surf.reserve(plsize); pl_full.resize(plsize); for (int i = 0; i < N_SCANS; i++) { pl_buff[i].clear(); pl_buff[i].reserve(plsize); } uint valid_num = 0; if (feature_enabled) { for (uint i = 1; i < plsize; i++) { if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10) || ((msg->points[i].tag & 0x30) == 0x00)) { pl_full[i].x = msg->points[i].x; pl_full[i].y = msg->points[i].y; pl_full[i].z = msg->points[i].z; pl_full[i].intensity = msg->points[i].reflectivity; pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points 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; if (dist < blind) continue; if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) { pl_buff[msg->points[i].line].push_back(pl_full[i]); } } } static int count = 0; static double time = 0.0; count++; double t0 = omp_get_wtime(); for (int j = 0; j < N_SCANS; j++) { if (pl_buff[j].size() <= 5) continue; pcl::PointCloud &pl = pl_buff[j]; plsize = pl.size(); vector &types = typess[j]; types.clear(); types.resize(plsize); plsize--; for (uint i = 0; i < plsize; i++) { types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y; vx = pl[i].x - pl[i + 1].x; vy = pl[i].y - pl[i + 1].y; vz = pl[i].z - pl[i + 1].z; types[i].dista = vx * vx + vy * vy + vz * vz; } types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y; give_feature(pl, types); // pl_surf += pl; } time += omp_get_wtime() - t0; printf("Feature extraction time: %lf \n", time / count); } else { for (uint i = 1; i < plsize; i++) { if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { valid_num++; if (valid_num % point_filter_num == 0) { pl_full[i].x = msg->points[i].x; pl_full[i].y = msg->points[i].y; pl_full[i].z = msg->points[i].z; pl_full[i].intensity = msg->points[i].reflectivity; pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points 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; if (dist < blind) continue; if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) { pl_surf.push_back(pl_full[i]); } } } } } } void Preprocess::l515_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.size(); pl_corn.reserve(plsize); pl_surf.reserve(plsize); for (int i = 0; i < pl_orig.points.size(); i++) { if (i % point_filter_num != 0) continue; double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < blind) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.normal_x = pl_orig.points[i].r; added_pt.normal_y = pl_orig.points[i].g; added_pt.normal_z = pl_orig.points[i].b; added_pt.curvature = 0.0; pl_surf.points.push_back(added_pt); } } void Preprocess::oust_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.size(); pl_corn.reserve(plsize); pl_surf.reserve(plsize); if (feature_enabled) { for (int i = 0; i < N_SCANS; i++) { pl_buff[i].clear(); pl_buff[i].reserve(plsize); } for (uint i = 0; i < plsize; i++) { double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < blind) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; if (yaw_angle >= 180.0) yaw_angle -= 360.0; if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].t / 1e6; if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } } for (int j = 0; j < N_SCANS; j++) { PointCloudXYZI &pl = pl_buff[j]; int linesize = pl.size(); vector &types = typess[j]; types.clear(); types.resize(linesize); linesize--; for (uint i = 0; i < linesize; i++) { types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); vx = pl[i].x - pl[i + 1].x; vy = pl[i].y - pl[i + 1].y; vz = pl[i].z - pl[i + 1].z; types[i].dista = vx * vx + vy * vy + vz * vz; } types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); give_feature(pl, types); } } else { for (int i = 0; i < pl_orig.points.size(); i++) { if (i % point_filter_num != 0) continue; double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < blind) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; added_pt.curvature = pl_orig.points[i].t / 1e6; //ms if (pl_orig.points[i].ring < N_SCANS) pl_surf.points.push_back(added_pt); } } } // #define MAX_LINE_NUM 64 void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); pl_surf.reserve(plsize); bool is_first[MAX_LINE_NUM]; double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point double omega_l = 3.61; // scan angular velocity (deg/ms) float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point float time_last[MAX_LINE_NUM] = {0.0}; // last offset time if (pl_orig.points[plsize - 1].time > 0 && pl_orig.points[0].time > 0) { cout << "Use given offset time." << endl; given_offset_time = true; } else { cout << "Compute offset time using constant rotation model." << endl; given_offset_time = false; memset(is_first, true, sizeof(is_first)); double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; double yaw_end = yaw_first; int layer_first = pl_orig.points[0].ring; for (uint i = plsize - 1; i > 0; i--) { if (pl_orig.points[i].ring == layer_first) { yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; break; } } } if (feature_enabled) { for (int i = 0; i < N_SCANS; i++) { pl_buff[i].clear(); pl_buff[i].reserve(plsize); } for (int i = 0; i < plsize; i++) { PointType added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; int layer = pl_orig.points[i].ring; if (layer >= N_SCANS) continue; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.curvature = pl_orig.points[i].time * 1000.0; // unit: ms if (!given_offset_time) { double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; if (is_first[layer]) { // printf("layer: %d; is first: %d", layer, is_first[layer]); yaw_fp[layer] = yaw_angle; is_first[layer] = false; added_pt.curvature = 0.0; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; continue; } if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; } pl_buff[layer].points.push_back(added_pt); } for (int j = 0; j < N_SCANS; j++) { PointCloudXYZI &pl = pl_buff[j]; int linesize = pl.size(); if (linesize < 2) continue; vector &types = typess[j]; types.clear(); types.resize(linesize); linesize--; for (uint i = 0; i < linesize; i++) { types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); vx = pl[i].x - pl[i + 1].x; vy = pl[i].y - pl[i + 1].y; vz = pl[i].z - pl[i + 1].z; types[i].dista = vx * vx + vy * vy + vz * vz; } types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); give_feature(pl, types); } } else { for (int i = 0; i < plsize; i++) { PointType added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.curvature = pl_orig.points[i].time * 1000.0; //ms if (!given_offset_time) { int layer = pl_orig.points[i].ring; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; if (is_first[layer]) { yaw_fp[layer] = yaw_angle; is_first[layer] = false; added_pt.curvature = 0.0; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; continue; } // compute offset time if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; } if (i % point_filter_num == 0) { if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind) { pl_surf.points.push_back(added_pt); } } } } } void Preprocess::velodyne_handler_nclt(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); pl_corn.clear(); pl_full.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); if (plsize == 0) return; pl_surf.reserve(plsize); /*** These variables only works when no point timestamps given ***/ double omega_l = 0.361 * SCAN_RATE; // scan angular velocity std::vector is_first(N_SCANS,true); std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point std::vector time_last(N_SCANS, 0.0); // last offset time /*****************************************************************/ if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; } else { given_offset_time = false; double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; double yaw_end = yaw_first; int layer_first = pl_orig.points[0].ring; for (uint i = plsize - 1; i > 0; i--) { if (pl_orig.points[i].ring == layer_first) { yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; break; } } } for (int i = 0; i < plsize; i++) { PointType added_pt; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.curvature = pl_orig.points[i].time * 1.e3f; // curvature unit: sec int layer = pl_orig.points[i].ring; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; if (is_first[layer]) { // printf("layer: %d; is first: %d", layer, is_first[layer]); yaw_fp[layer]=yaw_angle; is_first[layer]=false; added_pt.curvature = 0.0; yaw_last[layer]=yaw_angle; time_last[layer]=added_pt.curvature; continue; } // compute offset time if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l; } else { added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l; } if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l; yaw_last[layer] = yaw_angle; time_last[layer]=added_pt.curvature; if (i % point_filter_num == 0) { if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > (blind * blind)) { pl_surf.points.push_back(added_pt); } } } } void Preprocess::velodyne_handler_kitti(const sensor_msgs::PointCloud2::ConstPtr &msg) { pl_surf.clear(); pl_full.clear(); pcl::fromROSMsg(*msg, pl_full); for (int i = 0; i < pl_full.size(); i++) { if (i % point_filter_num == 0) { 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; if (dist > blind) pl_surf.points.push_back(pl_full[i]); } } } void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) { int plsize = pl.size(); int plsize2; if (plsize == 0) { printf("something wrong\n"); return; } uint head = 0; while (types[head].range < blind) { head++; } // Surf plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); uint i_nex = 0, i2; uint last_i = 0; uint last_i_nex = 0; int last_state = 0; int plane_type; for (uint i = head; i < plsize2; i++) { if (types[i].range < blind) { continue; } i2 = i; plane_type = plane_judge(pl, types, i, i_nex, curr_direct); if (plane_type == 1) { for (uint j = i; j <= i_nex; j++) { if (j != i && j != i_nex) { types[j].ftype = Real_Plane; } else { types[j].ftype = Poss_Plane; } } // if(last_state==1 && fabs(last_direct.sum())>0.5) if (last_state == 1 && last_direct.norm() > 0.1) { double mod = last_direct.transpose() * curr_direct; if (mod > -0.707 && mod < 0.707) { types[i].ftype = Edge_Plane; } else { types[i].ftype = Real_Plane; } } i = i_nex - 1; last_state = 1; } else // if(plane_type == 2) { i = i_nex; last_state = 0; } // else if(plane_type == 0) // { // if(last_state == 1) // { // uint i_nex_tem; // uint j; // for(j=last_i+1; j<=last_i_nex; j++) // { // uint i_nex_tem2 = i_nex_tem; // Eigen::Vector3d curr_direct2; // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); // if(ttem != 1) // { // i_nex_tem = i_nex_tem2; // break; // } // curr_direct = curr_direct2; // } // if(j == last_i+1) // { // last_state = 0; // } // else // { // for(uint k=last_i_nex; k<=i_nex_tem; k++) // { // if(k != i_nex_tem) // { // types[k].ftype = Real_Plane; // } // else // { // types[k].ftype = Poss_Plane; // } // } // i = i_nex_tem-1; // i_nex = i_nex_tem; // i2 = j-1; // last_state = 1; // } // } // } last_i = i2; last_i_nex = i_nex; last_direct = curr_direct; } plsize2 = plsize > 3 ? plsize - 3 : 0; for (uint i = head + 3; i < plsize2; i++) { if (types[i].range < blind || types[i].ftype >= Real_Plane) { continue; } if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) { continue; } Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); Eigen::Vector3d vecs[2]; for (int j = 0; j < 2; j++) { int m = -1; if (j == 1) { m = 1; } if (types[i + m].range < blind) { if (types[i].range > inf_bound) { types[i].edj[j] = Nr_inf; } else { types[i].edj[j] = Nr_blind; } continue; } vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z); vecs[j] = vecs[j] - vec_a; types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); if (types[i].angle[j] < jump_up_limit) { types[i].edj[j] = Nr_180; } else if (types[i].angle[j] > jump_down_limit) { types[i].edj[j] = Nr_zero; } } types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista) { if (types[i].intersect > cos160) { if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } } } else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && types[i - 1].dista > 4 * types[i].dista) { if (types[i].intersect > cos160) { if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } } } else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) { if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } } else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) { if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } } else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) { if (types[i].ftype == Nor) { types[i].ftype = Wire; } } } plsize2 = plsize - 1; double ratio; for (uint i = head + 1; i < plsize2; i++) { if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind) { continue; } if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) { continue; } if (types[i].ftype == Nor) { if (types[i - 1].dista > types[i].dista) { ratio = types[i - 1].dista / types[i].dista; } else { ratio = types[i].dista / types[i - 1].dista; } if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) { if (types[i - 1].ftype == Nor) { types[i - 1].ftype = Real_Plane; } if (types[i + 1].ftype == Nor) { types[i + 1].ftype = Real_Plane; } types[i].ftype = Real_Plane; } } } int last_surface = -1; for (uint j = head; j < plsize; j++) { if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) { if (last_surface == -1) { last_surface = j; } if (j == uint(last_surface + point_filter_num - 1)) { PointType ap; ap.x = pl[j].x; ap.y = pl[j].y; ap.z = pl[j].z; ap.curvature = pl[j].curvature; pl_surf.push_back(ap); last_surface = -1; } } else { if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) { pl_corn.push_back(pl[j]); } if (last_surface != -1) { PointType ap; for (uint k = last_surface; k < j; k++) { ap.x += pl[k].x; ap.y += pl[k].y; ap.z += pl[k].z; ap.curvature += pl[k].curvature; } ap.x /= (j - last_surface); ap.y /= (j - last_surface); ap.z /= (j - last_surface); ap.curvature /= (j - last_surface); pl_surf.push_back(ap); } last_surface = -1; } } } // void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct) { // pl.height = 1; // pl.width = pl.size(); // sensor_msgs::PointCloud2 output; // pcl::toROSMsg(pl, output); // output.header.frame_id = "livox"; // output.header.stamp = ct; // } int Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) { double group_dis = disA * types[i_cur].range + disB; group_dis = group_dis * group_dis; // i_nex = i_cur; double two_dis; vector disarr; disarr.reserve(20); for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) { if (types[i_nex].range < blind) { curr_direct.setZero(); return 2; } disarr.push_back(types[i_nex].dista); } for (;;) { if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break; if (types[i_nex].range < blind) { curr_direct.setZero(); return 2; } vx = pl[i_nex].x - pl[i_cur].x; vy = pl[i_nex].y - pl[i_cur].y; vz = pl[i_nex].z - pl[i_cur].z; two_dis = vx * vx + vy * vy + vz * vz; if (two_dis >= group_dis) { break; } disarr.push_back(types[i_nex].dista); i_nex++; } double leng_wid = 0; double v1[3], v2[3]; for (uint j = i_cur + 1; j < i_nex; j++) { if ((j >= pl.size()) || (i_cur >= pl.size())) break; v1[0] = pl[j].x - pl[i_cur].x; v1[1] = pl[j].y - pl[i_cur].y; v1[2] = pl[j].z - pl[i_cur].z; v2[0] = v1[1] * vz - vy * v1[2]; v2[1] = v1[2] * vx - v1[0] * vz; v2[2] = v1[0] * vy - vx * v1[1]; double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]; if (lw > leng_wid) { leng_wid = lw; } } if ((two_dis * two_dis / leng_wid) < p2l_ratio) { curr_direct.setZero(); return 0; } uint disarrsize = disarr.size(); for (uint j = 0; j < disarrsize - 1; j++) { for (uint k = j + 1; k < disarrsize; k++) { if (disarr[j] < disarr[k]) { leng_wid = disarr[j]; disarr[j] = disarr[k]; disarr[k] = leng_wid; } } } if (disarr[disarr.size() - 2] < 1e-16) { curr_direct.setZero(); return 0; } if (lidar_type == AVIA) { double dismax_mid = disarr[0] / disarr[disarrsize / 2]; double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2]; if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) { curr_direct.setZero(); return 0; } } else { double dismax_min = disarr[0] / disarr[disarrsize - 2]; if (dismax_min >= limit_maxmin) { curr_direct.setZero(); return 0; } } curr_direct << vx, vy, vz; curr_direct.normalize(); return 1; } bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) { if (nor_dir == 0) { if (types[i - 1].range < blind || types[i - 2].range < blind) { return false; } } else if (nor_dir == 1) { if (types[i + 1].range < blind || types[i + 2].range < blind) { return false; } } double d1 = types[i + nor_dir - 1].dista; double d2 = types[i + 3 * nor_dir - 2].dista; double d; if (d1 < d2) { d = d1; d1 = d2; d2 = d; } d1 = sqrt(d1); d2 = sqrt(d2); if (d1 > edgea * d2 || (d1 - d2) > edgeb) { return false; } return true; }