Showing preview only (600K chars total). Download the full file or copy to clipboard to get everything.
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.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 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.
<signature of Ty Coon>, 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
<p align="center"><img src="./figs/GRIL-Calib-overview.png" width = "500" ></p>
- **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 <container_name> <image_name:tag>
```
**: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 <container_name> <image_name:tag>"
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 <float.h> // FLT_MAX
#include "FusionAhrs.h"
#include "FusionCompass.h"
#include <math.h> // 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 <stdbool.h>
//------------------------------------------------------------------------------
// 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 <math.h> // 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 <math.h> // M_PI, sqrtf, atan2f, asinf
#include <stdbool.h>
#include <stdint.h>
//------------------------------------------------------------------------------
// 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 <math.h> // 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<CalibState> &IMU_states) {
IMU_state_group.assign(IMU_states.begin(), IMU_states.end() - 1);
}
void Gril_Calib::set_Lidar_state(const deque<CalibState> &Lidar_states) {
Lidar_state_group.assign(Lidar_states.begin(), Lidar_states.end() - 1);
}
void Gril_Calib::set_states_2nd_filter(const deque<CalibState> &IMU_states, const deque<CalibState> &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<CalibState> 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<CalibState> &signal_in, deque<CalibState> &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<CalibState> extend_front;
deque<CalibState> 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<CalibState> 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<CalibState> 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<CalibState> &signal_in, deque<CalibState> &signal_out) {
deque<CalibState> sig_out1;
Butter_filt(signal_in, sig_out1); // signal_in에 대해 Butterworth filter를 적용한 결과를 sig_out1에 저장
deque<CalibState> 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<CalibState> &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<CalibState> &IMU_states, deque<Eigen::Quaterniond> &Lidar_wrt_ground_states,
deque<Eigen::Quaterniond> &IMU_wrt_ground_states, deque<V3D> &normal_vector_wrt_lidar_group, deque<double> &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<M3D> es(Hessian_rot);
V3D EigenValue = es.eigenvalues().real();
M3D EigenVec_mat = es.eigenvectors().real();
M3D EigenMatCwise = EigenVec_mat.cwiseProduct(EigenVec_mat);
std::vector<double> EigenMat_1_col{EigenMatCwise(0, 0), EigenMatCwise(1, 0), EigenMatCwise(2, 0)};
std::vector<double> EigenMat_2_col{EigenMatCwise(0, 1), EigenMatCwise(1, 1), EigenMatCwise(2, 1)};
std::vector<double> 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<CalibState> IMU_after_zero_phase;
deque<CalibState> 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<CalibState> IMU_after_2nd_zero_phase;
deque<CalibState> 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<vector<double>> 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 <cmath>
#include <deque>
#include <fstream>
#include <iostream>
#include <condition_variable>
#include <sys/time.h>
#include <algorithm>
#include <csignal>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <eigen_conversions/eigen_msg.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/local_parameterization.h>
#include <ceres/covariance.h>
#include <so3_math.h>
#include "matplotlibcpp.h"
#include <common_lib.h>
#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<typename T>
bool operator()(const T *q, T *residual) const {
Eigen::Matrix<T, 3, 1> IMU_ang_vel_T = IMU_ang_vel.cast<T>();
Eigen::Matrix<T, 3, 1> Lidar_ang_vel_T = Lidar_ang_vel.cast<T>();
Eigen::Quaternion<T> q_LI{q[0], q[1], q[2], q[3]};
Eigen::Matrix<T, 3, 3> R_LI = q_LI.toRotationMatrix(); //Rotation (from LiDAR to IMU)
Eigen::Matrix<T, 3, 1> 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<Angular_Vel_Cost_only_Rot, 3, 4>(
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<typename T>
bool operator()(const T *q, const T *b_g, const T *t, T *residual) const {
//Known parameters used for Residual Construction
Eigen::Matrix<T, 3, 1> IMU_ang_vel_T = IMU_ang_vel.cast<T>();
Eigen::Matrix<T, 3, 1> IMU_ang_acc_T = IMU_ang_acc.cast<T>();
Eigen::Matrix<T, 3, 1> Lidar_ang_vel_T = Lidar_ang_vel.cast<T>();
T deltaT_LI_T{deltaT_LI};
//Unknown Parameters, needed to be estimated
Eigen::Quaternion<T> q_IL{q[0], q[1], q[2], q[3]};
Eigen::Matrix<T, 3, 3> R_IL = q_IL.toRotationMatrix(); //Rotation
Eigen::Matrix<T, 3, 1> 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<T, 3, 1> 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<Angular_Vel_IL_Cost, 3, 4, 3, 1>(
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<typename T>
bool operator()(const T *q, const T *trans, T *residual) const {
// Known parameters used for Residual Construction
Eigen::Quaternion<T> Lidar_wrt_ground_T = Lidar_wrt_ground.cast<T>(); // from ground frame to LiDAR plane frame
Eigen::Matrix<T, 3, 3> R_GL = Lidar_wrt_ground_T.toRotationMatrix(); // Rotation matrix (from Ground to LiDAR)
Eigen::Quaternion<T> IMU_wrt_ground_T = IMU_wrt_ground.cast<T>(); // from ground frame (earth frame) to IMU frame
Eigen::Matrix<T, 3, 3> 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<T> q_IL{q[0], q[1], q[2], q[3]};
Eigen::Matrix<T, 3, 3> R_IL = q_IL.toRotationMatrix(); // Rotation IMU frame to LiDAR frame
Eigen::Matrix<T, 3, 1> T_IL{trans[0], trans[1], trans[2]}; // Translation of I-L (IMU wtr Lidar)
// Plane motion constraint Residual
Eigen::Matrix<T, 3, 3> R_plane = R_IL.transpose() * R_GI.transpose() * R_GL;
Eigen::Matrix<T, 3, 1> e3 = Eigen::Matrix<T, 3, 1>::UnitZ(); // (0,0,1)
Eigen::Matrix<T, 3, 1> resi_plane = R_plane * e3;
// Distance constraint Residual - TODO : Generalized
Eigen::Matrix<T, 3, 1> imu_height_vec = imu_sensor_height_T * e3;
Eigen::Matrix<T, 3, 1> lidar_height_vec = distance_Lidar_wrt_ground_T * e3;
Eigen::Matrix<T, 3, 1> 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<Ground_Plane_Cost_IL, 3, 4, 3>(
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<typename T>
bool operator()(const T *q,const T *b_a, const T *trans, T *residual) const {
// Known parameters used for Residual Construction
Eigen::Matrix<T, 3, 3> R_LL0_T = LidarState.rot_end.cast<T>();
Eigen::Matrix<T, 3, 1> IMU_linear_acc_T = IMU_linear_acc.cast<T>(); // Linear acceleration of IMU
Eigen::Matrix<T, 3, 1> Lidar_linear_acc_T = LidarState.linear_acc.cast<T>(); // lidar linear acceleration
Eigen::Quaternion<T> Lidar_wrt_ground_T = Lidar_wrt_ground.cast<T>(); // from ground frame to LiDAR plane frame
Eigen::Matrix<T, 3, 3> R_GL = Lidar_wrt_ground_T.toRotationMatrix(); // Rotation matrix from Ground frame to LiDAR frame
// Unknown Parameters, needed to be estimated
Eigen::Matrix<T, 3, 1> bias_aL{b_a[0], b_a[1], b_a[2]}; // Bias of Linear acceleration
Eigen::Matrix<T, 3, 1> T_IL{trans[0], trans[1], trans[2]}; // Translation of I-L (IMU wtr Lidar)
Eigen::Quaternion<T> q_IL{q[0], q[1], q[2], q[3]}; // Rotation from IMU frame to LiDAR frame
Eigen::Matrix<T, 3, 3> 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<T, 3, 3> Jacob_trans_T = Jacob_trans.cast<T>();
Eigen::Matrix<T, 3, 1> 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<Linear_acc_Rot_Cost_without_Gravity, 3, 4, 3, 3>(
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<CalibState> &signal_in, deque<CalibState> &signal_out);
void zero_phase_filt(const deque<CalibState> &signal_in, deque<CalibState> &signal_out);
void cut_sequence_tail();
void set_IMU_state(const deque<CalibState> &IMU_states);
void set_Lidar_state(const deque<CalibState> &Lidar_states);
void set_states_2nd_filter(const deque<CalibState> &IMU_states, const deque<CalibState> &Lidar_states);
void solve_Rot_Trans_calib(double &timediff_imu_wrt_lidar, const double &imu_sensor_height);
void normalize_acc(deque<CalibState> &signal_in);
void align_Group(const deque<CalibState> &IMU_states, deque<Eigen::Quaterniond> &Lidar_wrt_ground_states,
deque<Eigen::Quaterniond> &IMU_wrt_ground_states, deque<V3D> &normal_vector_wrt_lidar_group,
deque<double> &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<CalibState> get_IMU_state() {
return IMU_state_group;
}
deque<CalibState> get_Lidar_state() {
return Lidar_state_group;
}
private:
deque<CalibState> IMU_state_group; // LiDAR와 interpolation을 진행한 IMU data 결과를 가지고 있는 groups
deque<CalibState> Lidar_state_group; // LiDAR state
deque<CalibState> IMU_state_group_ALL; // 모든 IMU data (ROS topic)를 가지고 있는 groups
deque<Eigen::Quaterniond> Lidar_wrt_ground_group; // from ground frame (earth frame) to LiDAR plane frame
deque<Eigen::Quaterniond> IMU_wrt_ground_group; // from ground frame (earth frame) to IMU frame
deque<V3D> normal_vector_wrt_lidar_group; // normal vector of LiDAR ground segmentation
deque<double> 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 <sensor_msgs/PointCloud2.h>
#include <ros/ros.h>
#include <jsk_recognition_msgs/PolygonArray.h>
#include <Eigen/Dense>
#include <boost/format.hpp>
#include <numeric>
#include <mutex>
#include <GroundSegmentation/utils.hpp>
#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<pcl::PointXYZI>::Ptr XYZI, pcl::PointCloud<pcl::PointXYZ>::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 <typename PointT>
bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }
template <typename PointT>
struct RevertCandidate
{
int concentric_idx;
int sector_idx;
double ground_flatness;
double line_variable;
Eigen::Vector4f pc_mean;
pcl::PointCloud<PointT> regionwise_ground;
RevertCandidate(int _c_idx, int _s_idx, double _flatness, double _line_var, Eigen::Vector4f _pc_mean, pcl::PointCloud<PointT> _ground)
: concentric_idx(_c_idx), sector_idx(_s_idx), ground_flatness(_flatness), line_variable(_line_var), pc_mean(_pc_mean), regionwise_ground(_ground) {}
};
template <typename PointT>
class PatchWorkpp {
public:
typedef std::vector<pcl::PointCloud<PointT>> Ring;
typedef std::vector<Ring> 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<jsk_recognition_msgs::PolygonArray>("/patchworkpp/plane", 100, true);
pub_revert_pc = node_handle_.advertise<sensor_msgs::PointCloud2>("/patchworkpp/revert_pc", 100, true);
pub_reject_pc = node_handle_.advertise<sensor_msgs::PointCloud2>("/patchworkpp/reject_pc", 100, true);
pub_normal = node_handle_.advertise<sensor_msgs::PointCloud2>("/patchworkpp/normals", 100, true);
pub_noise = node_handle_.advertise<sensor_msgs::PointCloud2>("/patchworkpp/noise", 100, true);
pub_vertical = node_handle_.advertise<sensor_msgs::PointCloud2>("/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<PointT> cloud_in,
pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &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<double> update_flatness_[4];
std::vector<double> update_elevation_[4];
float d_;
VectorXf normal_;
MatrixXf pnormal_;
VectorXf singular_values_;
Eigen::Matrix3f cov_;
Eigen::Vector4f pc_mean_;
// For visualization
bool visualize_;
vector<int> num_sectors_each_zone_;
vector<int> num_rings_each_zone_;
vector<double> sector_sizes_;
vector<double> ring_sizes_;
vector<double> min_ranges_;
vector<double> elevation_thr_;
vector<double> flatness_thr_;
vector<Zone> ConcentricZoneModel_;
jsk_recognition_msgs::PolygonArray poly_list_;
ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical;
pcl::PointCloud<PointT> revert_pc_, reject_pc_, noise_pc_, vertical_pc_;
pcl::PointCloud<PointT> ground_pc_;
pcl::PointCloud<pcl::PointXYZINormal> normals_;
pcl::PointCloud<PointT> 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<Zone> &czm);
void pc2czm(const pcl::PointCloud<PointT> &src, std::vector<Zone> &czm);
void reflected_noise_removal(pcl::PointCloud<PointT> &cloud, pcl::PointCloud<PointT> &cloud_nonground);
void temporal_ground_revert(pcl::PointCloud<PointT> &cloud_ground, pcl::PointCloud<PointT> &cloud_nonground,
std::vector<double> ring_flatness, std::vector<RevertCandidate<PointT>> candidates,
int concentric_idx);
void calc_mean_stdev(std::vector<double> 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<PointT> &ground);
void extract_piecewiseground(
const int zone_idx, const pcl::PointCloud<PointT> &src,
pcl::PointCloud<PointT> &dst,
pcl::PointCloud<PointT> &non_ground_dst);
void extract_initial_seeds(
const int zone_idx, const pcl::PointCloud<PointT> &p_sorted,
pcl::PointCloud<PointT> &init_seeds);
void extract_initial_seeds(
const int zone_idx, const pcl::PointCloud<PointT> &p_sorted,
pcl::PointCloud<PointT> &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<typename PointT> inline
void PatchWorkpp<PointT>::initialize_zone(Zone &z, int num_sectors, int num_rings) {
z.clear();
pcl::PointCloud<PointT> 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<typename PointT> inline
void PatchWorkpp<PointT>::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<typename PointT> inline
void PatchWorkpp<PointT>::flush_patches(vector<Zone> &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<typename PointT> inline
void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground) {
pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_);
// Singular Value Decomposition: SVD
Eigen::JacobiSVD<Eigen::MatrixXf> 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<typename PointT> inline
void PatchWorkpp<PointT>::extract_initial_seeds(
const int zone_idx, const pcl::PointCloud<PointT> &p_sorted,
pcl::PointCloud<PointT> &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.p
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
SYMBOL INDEX (197 symbols across 20 files)
FILE: include/Fusion/FusionAhrs.c
function FusionAhrsInitialise (line 36) | void FusionAhrsInitialise(FusionAhrs *const ahrs) {
function FusionAhrsReset (line 52) | void FusionAhrsReset(FusionAhrs *const ahrs) {
function FusionAhrsSetSettings (line 72) | void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSetti...
function FusionAhrsUpdate (line 100) | void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyrosco...
function FusionAhrsUpdateNoMagnetometer (line 205) | void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const Fusion...
function FusionAhrsUpdateExternalHeading (line 225) | void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const Fusio...
function FusionQuaternion (line 250) | FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) {
function FusionVector (line 260) | FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahr...
function FusionVector (line 278) | FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) {
function FusionAhrsInternalStates (line 303) | FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *c...
function FusionAhrsFlags (line 320) | FusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs) {
function FusionAhrsSetHeading (line 339) | void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) {
FILE: include/Fusion/FusionAhrs.h
type FusionAhrsSettings (line 23) | typedef struct {
type FusionAhrs (line 34) | typedef struct {
type FusionAhrsInternalStates (line 54) | typedef struct {
type FusionAhrsFlags (line 66) | typedef struct {
FILE: include/Fusion/FusionAxes.h
type FusionAxesAlignment (line 24) | typedef enum {
function FusionVector (line 60) | static inline FusionVector FusionAxesSwap(const FusionVector sensor, con...
FILE: include/Fusion/FusionCalibration.h
function FusionVector (line 26) | static inline FusionVector FusionCalibrationInertial(const FusionVector ...
function FusionVector (line 37) | static inline FusionVector FusionCalibrationMagnetic(const FusionVector ...
FILE: include/Fusion/FusionCompass.c
function FusionCompassCalculateHeading (line 23) | float FusionCompassCalculateHeading(const FusionVector accelerometer, co...
FILE: include/Fusion/FusionMath.h
type FusionVector (line 23) | typedef union {
type FusionQuaternion (line 36) | typedef union {
type FusionMatrix (line 51) | typedef union {
type FusionEuler (line 71) | typedef union {
function FusionDegreesToRadians (line 127) | static inline float FusionDegreesToRadians(const float degrees) {
function FusionRadiansToDegrees (line 136) | static inline float FusionRadiansToDegrees(const float radians) {
function FusionAsin (line 148) | static inline float FusionAsin(const float value) {
function FusionFastInverseSqrt (line 169) | static inline float FusionFastInverseSqrt(const float x) {
function FusionVectorIsZero (line 191) | static inline bool FusionVectorIsZero(const FusionVector vector) {
function FusionVector (line 201) | static inline FusionVector FusionVectorAdd(const FusionVector vectorA, c...
function FusionVector (line 215) | static inline FusionVector FusionVectorSubtract(const FusionVector vecto...
function FusionVectorSum (line 228) | static inline float FusionVectorSum(const FusionVector vector) {
function FusionVector (line 238) | static inline FusionVector FusionVectorMultiplyScalar(const FusionVector...
function FusionVector (line 252) | static inline FusionVector FusionVectorHadamardProduct(const FusionVecto...
function FusionVector (line 266) | static inline FusionVector FusionVectorCrossProduct(const FusionVector v...
function FusionVectorMagnitudeSquared (line 283) | static inline float FusionVectorMagnitudeSquared(const FusionVector vect...
function FusionVectorMagnitude (line 292) | static inline float FusionVectorMagnitude(const FusionVector vector) {
function FusionVector (line 301) | static inline FusionVector FusionVectorNormalise(const FusionVector vect...
function FusionQuaternion (line 319) | static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternio...
function FusionQuaternion (line 334) | static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuat...
function FusionQuaternion (line 356) | static inline FusionQuaternion FusionQuaternionMultiplyVector(const Fusi...
function FusionQuaternion (line 374) | static inline FusionQuaternion FusionQuaternionNormalise(const FusionQua...
function FusionVector (line 399) | static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix...
function FusionMatrix (line 417) | static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaterni...
function FusionEuler (line 445) | static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion...
FILE: include/Fusion/FusionOffset.c
function FusionOffsetInitialise (line 40) | void FusionOffsetInitialise(FusionOffset *const offset, const unsigned i...
function FusionVector (line 54) | FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector...
FILE: include/Fusion/FusionOffset.h
type FusionOffset (line 23) | typedef struct {
FILE: include/Gril_Calib/Gril_Calib.h
type Vector3d (line 34) | typedef Vector3d V3D;
type Matrix3d (line 35) | typedef Matrix3d M3D;
type Eigen (line 36) | typedef Eigen::Quaterniond QD;
type CalibState (line 46) | struct CalibState {
type Angular_Vel_Cost_only_Rot (line 109) | struct Angular_Vel_Cost_only_Rot {
function ceres (line 128) | static ceres::CostFunction *Create(const V3D IMU_ang_vel_, const V3D Lid...
type Angular_Vel_IL_Cost (line 137) | struct Angular_Vel_IL_Cost {
function T (line 156) | T td{t[0]}; //Time lag (IMU ...
function ceres (line 166) | static ceres::CostFunction *
type Ground_Plane_Cost_IL (line 179) | struct Ground_Plane_Cost_IL {
function ceres (line 223) | static ceres::CostFunction *
type Linear_acc_Rot_Cost_without_Gravity (line 238) | struct Linear_acc_Rot_Cost_without_Gravity {
function ceres (line 278) | static ceres::CostFunction *Create(const CalibState LidarState_, const V...
function class (line 289) | class Gril_Calib {
FILE: include/GroundSegmentation/patchworkpp.hpp
function xyzi2xyz (line 43) | void xyzi2xyz(pcl::PointCloud<pcl::PointXYZI>::Ptr XYZI, pcl::PointCloud...
function point_z_cmp (line 56) | bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }
type RevertCandidate (line 59) | struct RevertCandidate
method RevertCandidate (line 68) | RevertCandidate(int _c_idx, int _s_idx, double _flatness, double _line...
class PatchWorkpp (line 73) | class PatchWorkpp {
method PatchWorkpp (line 79) | PatchWorkpp() {}
method PatchWorkpp (line 81) | PatchWorkpp(ros::NodeHandle *nh) : node_handle_(*nh) {
FILE: include/GroundSegmentation/utils.hpp
type PointXYZILID (line 53) | struct PointXYZILID
function PointXYZILID2XYZI (line 72) | void PointXYZILID2XYZI(pcl::PointCloud<PointXYZILID>& src,
function count_num_ground (line 89) | int count_num_ground(const pcl::PointCloud<PointXYZILID>& pc){
function count_num_ground_without_vegetation (line 107) | int count_num_ground_without_vegetation(const pcl::PointCloud<PointXYZIL...
function set_initial_gt_counts (line 123) | std::map<int, int> set_initial_gt_counts(std::vector<int>& gt_classes){
function count_num_each_class (line 131) | std::map<int, int> count_num_each_class(const pcl::PointCloud<PointXYZIL...
function count_num_outliers (line 149) | int count_num_outliers(const pcl::PointCloud<PointXYZILID>& pc){
function discern_ground (line 164) | void discern_ground(const pcl::PointCloud<PointXYZILID>& src, pcl::Point...
function discern_ground_without_vegetation (line 183) | void discern_ground_without_vegetation(const pcl::PointCloud<PointXYZILI...
function calculate_precision_recall (line 199) | void calculate_precision_recall(const pcl::PointCloud<PointXYZILID>& pc_...
function calculate_precision_recall_without_vegetation (line 218) | void calculate_precision_recall_without_vegetation(const pcl::PointCloud...
function save_all_labels (line 243) | int save_all_labels(const pcl::PointCloud<PointXYZILID>& pc, string ABS_...
function save_all_accuracy (line 298) | void save_all_accuracy(const pcl::PointCloud<PointXYZILID>& pc_curr,
function pc2pcdfile (line 330) | void pc2pcdfile(const pcl::PointCloud<PointXYZILID>& TP, const pcl::Poin...
FILE: include/IMU_Processing.hpp
function time_list (line 34) | const bool time_list(PointType &x, PointType &y) {return (x.curvature < ...
class ImuProcess (line 37) | class ImuProcess
FILE: include/common_lib.h
type gril_calib (line 42) | typedef gril_calib::Pose6D Pose6D;
type pcl (line 43) | typedef pcl::PointXYZINormal PointType;
type pcl (line 44) | typedef pcl::PointXYZRGB PointTypeRGB;
type pcl (line 45) | typedef pcl::PointCloud<PointType> PointCloudXYZI;
type pcl (line 46) | typedef pcl::PointCloud<PointTypeRGB> PointCloudXYZRGB;
type vector (line 47) | typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVec...
type Vector3d (line 48) | typedef Vector3d V3D;
type Matrix3d (line 49) | typedef Matrix3d M3D;
type Vector3f (line 50) | typedef Vector3f V3F;
type Vector2d (line 51) | typedef Vector2d V2D;
type Matrix2d (line 52) | typedef Matrix2d M2D;
type LID_TYPE (line 63) | enum LID_TYPE{AVIA = 1, VELO, OUSTER, L515, PANDAR, VELO_NCLT, VELO_with...
type StatesGroup (line 76) | struct StatesGroup
function resetpose (line 161) | void resetpose()
function Eigen (line 247) | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R){
type typename (line 266) | typedef typename Derived::Scalar Scalar_t;
FILE: include/ikd-Tree/ikd_Tree.cpp
function BoxPointType (line 90) | BoxPointType KD_TREE::tree_range(){
type timespec (line 636) | struct timespec
type timespec (line 683) | struct timespec
type timespec (line 740) | struct timespec
type timespec (line 786) | struct timespec
function PointType_CMP (line 1314) | PointType_CMP MANUAL_HEAP::top(){
function Operation_Logger_Type (line 1382) | Operation_Logger_Type MANUAL_Q::front(){
function Operation_Logger_Type (line 1386) | Operation_Logger_Type MANUAL_Q::back(){
FILE: include/ikd-Tree/ikd_Tree.h
type pcl (line 21) | typedef pcl::PointXYZINormal PointType;
type vector (line 22) | typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVec...
type KD_TREE_NODE (line 26) | struct KD_TREE_NODE
type PointType_CMP (line 50) | struct PointType_CMP{
function operator (line 57) | bool operator < (const PointType_CMP &a)const{
type BoxPointType (line 63) | struct BoxPointType{
type operation_set (line 68) | enum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSA...
type delete_point_storage_set (line 70) | enum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THRE...
type Operation_Logger_Type (line 72) | struct Operation_Logger_Type{
function class (line 79) | class MANUAL_Q{
function class (line 94) | class MANUAL_HEAP
function class (line 113) | class KD_TREE
FILE: include/matplotlibcpp.h
function namespace (line 42) | namespace matplotlibcpp {
function backend (line 265) | inline void backend(const std::string& name)
function annotate (line 270) | inline bool annotate(std::string annotation, double x, double y)
function namespace (line 296) | namespace detail {
function namespace (line 781) | namespace detail {
function fignum_exists (line 1520) | inline bool fignum_exists(long number)
function figure_size (line 1536) | inline void figure_size(size_t w, size_t h)
function legend (line 1558) | inline void legend()
function legend (line 1568) | inline void legend(const std::map<std::string, std::string>& keywords)
function xticks (line 1708) | void xticks(const std::vector<Numeric> &ticks, const std::map<std::strin...
function yticks (line 1757) | void yticks(const std::vector<Numeric> &ticks, const std::map<std::strin...
function margins (line 1762) | void margins(Numeric margin)
function margins (line 1777) | void margins(Numeric margin_x, Numeric margin_y)
function subplot (line 1820) | inline void subplot(long nrows, long ncols, long plot_number)
function axis (line 1906) | inline void axis(const std::string &axisstr)
function grid (line 2066) | inline void grid(bool flag)
function close (line 2108) | inline void close()
function xkcd (line 2121) | inline void xkcd() {
function draw (line 2138) | inline void draw()
function pause (line 2152) | void pause(Numeric interval)
function save (line 2166) | inline void save(const std::string& filename)
function clf (line 2183) | inline void clf() {
function cla (line 2195) | inline void cla() {
function ion (line 2207) | inline void ion() {
function tight_layout (line 2256) | inline void tight_layout() {
function namespace (line 2270) | namespace detail {
type Fallback (line 2287) | struct Fallback { void operator()(); }
type Derived (line 2288) | struct Derived
type dtype (line 2300) | typedef decltype(&Fallback::operator()) dtype;
type typename (line 2308) | typedef typename is_callable_impl<std::is_class<T>::value, T>::type type;
function false_type (line 2315) | struct plot_impl<std::false_type>
function true_type (line 2354) | struct plot_impl<std::true_type>
function class (line 2400) | class Plot
FILE: include/preprocess.h
type Feature (line 14) | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, Z...
type Surround (line 15) | enum Surround{Prev, Next}
type E_jump (line 16) | enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}
type orgtype (line 20) | struct orgtype
function Point (line 39) | struct EIGEN_ALIGN16 Point {
function Point (line 57) | struct EIGEN_ALIGN16 Point {
function Point (line 75) | struct EIGEN_ALIGN16 Point {
function Point (line 101) | struct EIGEN_ALIGN16 Point {
FILE: include/scope_timer.hpp
class TimeConsuming (line 16) | class TimeConsuming {
method TimeConsuming (line 20) | TimeConsuming(string msg, int repeat_time) {
method TimeConsuming (line 27) | TimeConsuming(string msg) {
method set_enbale (line 53) | void set_enbale(bool enable){
method start (line 57) | void start() {
method stop (line 61) | void stop() {
FILE: src/laserMapping.cpp
function cloud2msg (line 192) | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, std::string...
function calc_dist (line 199) | float calc_dist(PointType p1, PointType p2) {
function calcBodyVar (line 204) | void calcBodyVar(Eigen::Vector3d &pb, const float range_inc,
function SigHandle (line 229) | void SigHandle(int sig) {
function dump_lio_state_to_log (line 239) | inline void dump_lio_state_to_log(FILE *fp) {
function pointBodyToWorld (line 255) | void pointBodyToWorld(PointType const *const pi, PointType *const po) {
function pointBodyToWorld (line 269) | void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po) {
function RGBpointBodyToWorld (line 277) | void RGBpointBodyToWorld(PointType const *const pi, PointTypeRGB *const ...
function points_cache_collect (line 295) | void points_cache_collect() {
function lasermap_fov_segment (line 306) | void lasermap_fov_segment() {
function livox_pcl_cbk (line 363) | void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
function standard_pcl_cbk (line 402) | void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) {
function imu_cbk (line 454) | void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in, const ros::Publis...
function sync_packages (line 506) | bool sync_packages(MeasureGroup &meas) {
function map_incremental (line 549) | void map_incremental() {
function publish_frame_world (line 596) | void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes) {
function publish_frame_body (line 651) | void publish_frame_body(const ros::Publisher &pubLaserCloudFullRes_body) {
function publish_effect_world (line 660) | void publish_effect_world(const ros::Publisher &pubLaserCloudEffect) {
function publish_map (line 673) | void publish_map(const ros::Publisher &pubLaserCloudMap) {
function set_posestamp (line 682) | void set_posestamp(T &out) {
function publish_odometry (line 700) | void publish_odometry(const ros::Publisher &pubOdomAftMapped) {
function publish_path (line 722) | void publish_path(const ros::Publisher pubPath) {
function fileout_calib_result (line 735) | void fileout_calib_result() {
function printProgress (line 754) | void printProgress(double percentage) {
function saveTrajectory (line 775) | void saveTrajectory(const std::string &traj_file) {
function get_quat_LiDAR_plane_to_gravity (line 806) | void get_quat_LiDAR_plane_to_gravity(Eigen::Quaterniond &lidar_q, V3D& n...
function main (line 827) | int main(int argc, char **argv) {
FILE: src/preprocess.cpp
function time_list_cut_frame (line 6) | const bool time_list_cut_frame(PointType &x, PointType &y) {
Condensed preview — 64 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (610K chars).
[
{
"path": "CMakeLists.txt",
"chars": 2465,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(gril_calib)\n\nSET(CMAKE_BUILD_TYPE \"Release\")\n\nADD_COMPILE_OPTIONS(-std=c++"
},
{
"path": "LI-Init-LICENSE",
"chars": 18092,
"preview": " GNU GENERAL PUBLIC LICENSE\n Version 2, June 1991\n\n Copyright (C) 1989, 1991 Fr"
},
{
"path": "README.md",
"chars": 5504,
"preview": "# GRIL-Calib [ROS1 / ROS2]\nOfficial implementation of our paper **\"GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrins"
},
{
"path": "config/hilti/Basement_3.yaml",
"chars": 4467,
"preview": "# rosbag play -s 85 -u 230 Basement_3.bag \n\ncommon:\n lid_topic: \"/os_cloud_node/points\"\n imu_topic: \"/alphasense"
},
{
"path": "config/hilti/Basement_4.yaml",
"chars": 4480,
"preview": "# rosbag play -s 75 -u 150 Basement_4.bag \n\ncommon:\n lid_topic: \"/os_cloud_node/points\"\n imu_topic: \"/alphasense"
},
{
"path": "config/m2dgr/velodyne_m2dgr_gate01.yaml",
"chars": 4757,
"preview": "# rosbag play -u 140 gate_01.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreproce"
},
{
"path": "config/m2dgr/velodyne_m2dgr_hall04.yaml",
"chars": 4768,
"preview": "# rosbag play -u 160 hall_04.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreproce"
},
{
"path": "config/m2dgr/velodyne_m2dgr_lift04.yaml",
"chars": 4798,
"preview": "# rosbag play -u 80 lift_04.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\npreproces"
},
{
"path": "config/m2dgr/velodyne_m2dgr_rotation02.yaml",
"chars": 4831,
"preview": "# rosbag play -u 120 rotation_02.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\nprep"
},
{
"path": "config/m2dgr/velodyne_m2dgr_street08.yaml",
"chars": 4781,
"preview": "# rosbag play -s 20 -u 110 street_08.bag\n\ncommon:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/handsfree/imu\" \n\n"
},
{
"path": "config/ouster64.yaml",
"chars": 4426,
"preview": "common:\n lid_topic: \"/os1_cloud_node/points\"\n imu_topic: \"/os1_cloud_node/imu\"\n\npreprocess:\n lidar_type: 3 "
},
{
"path": "config/s3e/velodyne_s3e_bob_lab01.yaml",
"chars": 4784,
"preview": "# rosbag play -s 65 -u 145 S3E_Laboratory_1.bag\n\ncommon:\n lid_topic: \"/Bob/velodyne_points\"\n imu_topic: \"/Bob/im"
},
{
"path": "config/s3e/velodyne_s3e_bob_lab02.yaml",
"chars": 4789,
"preview": "# rosbag play -s 220 -u 120 S3E_Laboratory_2.bag\n\ncommon:\n lid_topic: \"/Bob/velodyne_points\"\n imu_topic: \"/Bob/i"
},
{
"path": "config/velodyne16.yaml",
"chars": 4718,
"preview": "common:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/imu/data\"\n\npreprocess:\n lidar_type: 2 # V"
},
{
"path": "docker/Dockerfile",
"chars": 1530,
"preview": "# Author: Tae Young Kim\n# email: tyoung96@yonsei.ac.kr\n\nFROM ros:melodic\n\n# Install PCL & Eigen & essential libraries\nRU"
},
{
"path": "docker/container_run.sh",
"chars": 1160,
"preview": "#!/bin/bash\n# Author : Taeyoung Kim (https://github.com/Taeyoung96)\n\n# Set the project directory (PROJECT_DIR) as the pa"
},
{
"path": "docker/ros_entrypoint.sh",
"chars": 242,
"preview": "#!/bin/bash\n \nset -e\n\n# Ros build\nsource \"/opt/ros/melodic/setup.bash\"\nsource \"/root/livox_ws/devel/setup.bash\"\n\n\n# Libr"
},
{
"path": "include/Fusion/CMakeLists.txt",
"chars": 159,
"preview": "file(GLOB_RECURSE files \"*.c\")\n\nadd_library(Fusion ${files})\n\nif(UNIX AND NOT APPLE)\n target_link_libraries(Fusion m)"
},
{
"path": "include/Fusion/Fusion.h",
"chars": 636,
"preview": "/**\n * @file Fusion.h\n * @author Seb Madgwick\n * @brief Main header file for the Fusion library. This is the only file "
},
{
"path": "include/Fusion/FusionAhrs.c",
"chars": 14969,
"preview": "/**\n * @file FusionAhrs.c\n * @author Seb Madgwick\n * @brief AHRS algorithm to combine gyroscope, accelerometer, and magn"
},
{
"path": "include/Fusion/FusionAhrs.h",
"chars": 3201,
"preview": "/**\n * @file FusionAhrs.h\n * @author Seb Madgwick\n * @brief AHRS algorithm to combine gyroscope, accelerometer, and magn"
},
{
"path": "include/Fusion/FusionAxes.h",
"chars": 6962,
"preview": "/**\n * @file FusionAxes.h\n * @author Seb Madgwick\n * @brief Swaps sensor axes for alignment with the body axes.\n */\n\n#if"
},
{
"path": "include/Fusion/FusionCalibration.h",
"chars": 1582,
"preview": "/**\n * @file FusionCalibration.h\n * @author Seb Madgwick\n * @brief Gyroscope, accelerometer, and magnetometer calibratio"
},
{
"path": "include/Fusion/FusionCompass.c",
"chars": 1402,
"preview": "/**\n * @file FusionCompass.c\n * @author Seb Madgwick\n * @brief Tilt-compensated compass to calculate an heading relative"
},
{
"path": "include/Fusion/FusionCompass.h",
"chars": 684,
"preview": "/**\n * @file FusionCompass.h\n * @author Seb Madgwick\n * @brief Tilt-compensated compass to calculate an heading relative"
},
{
"path": "include/Fusion/FusionMath.h",
"chars": 13803,
"preview": "/**\n * @file FusionMath.h\n * @author Seb Madgwick\n * @brief Math library.\n */\n\n#ifndef FUSION_MATH_H\n#define FUSION_MATH"
},
{
"path": "include/Fusion/FusionOffset.c",
"chars": 2371,
"preview": "/**\n * @file FusionOffset.c\n * @author Seb Madgwick\n * @brief Gyroscope offset correction algorithm for run-time calibra"
},
{
"path": "include/Fusion/FusionOffset.h",
"chars": 1099,
"preview": "/**\n * @file FusionOffset.h\n * @author Seb Madgwick\n * @brief Gyroscope offset correction algorithm for run-time calibra"
},
{
"path": "include/Gril_Calib/Gril_Calib.cpp",
"chars": 35241,
"preview": "#include \"Gril_Calib.h\"\n\n/*\nDescription: Gril-Calib (Heavily adapted from LI-Init by Fangcheng Zhu)\nModifier : Taeyoung "
},
{
"path": "include/Gril_Calib/Gril_Calib.h",
"chars": 17741,
"preview": "#ifndef Gril_Calib_H\n#define Gril_Calib_H\n\n#include <cmath>\n#include <deque>\n#include <fstream>\n#include <iostream>\n#inc"
},
{
"path": "include/GroundSegmentation/patchworkpp.hpp",
"chars": 39767,
"preview": "/**\n * @file patchworkpp.hpp\n * @author Seungjae Lee\n * @brief \n * @version 0.1\n * @date 2022-07-20\n * \n * @copyright Co"
},
{
"path": "include/GroundSegmentation/utils.hpp",
"chars": 12466,
"preview": "#ifndef COMMON_H\n#define COMMON_H\n\n#include \"math.h\"\n#include <sstream>\n#include <vector>\n#include <map>\n#include <iostr"
},
{
"path": "include/IMU_Processing.hpp",
"chars": 15858,
"preview": "#ifndef _IMU_PROCESSING_HPP\n#define _IMU_PROCESSING_HPP\n\n#include <cmath>\n#include <math.h>\n#include <deque>\n#include <m"
},
{
"path": "include/color.h",
"chars": 1114,
"preview": "#ifndef COLOR_H\n#define COLOR_H\n\n#define RESET \"\\033[0m\"\n#define BLACK \"\\033[30m\" /* Black */\n#d"
},
{
"path": "include/common_lib.h",
"chars": 10482,
"preview": "#ifndef COMMON_LIB_H\n#define COMMON_LIB_H\n\n#include <so3_math.h>\n#include <Eigen/Eigen>\n#include <pcl/point_types.h>\n#in"
},
{
"path": "include/ikd-Tree/README.md",
"chars": 73,
"preview": "# ikd-Tree\nikd-Tree is an incremental k-d tree for robotic applications.\n"
},
{
"path": "include/ikd-Tree/ikd_Tree.cpp",
"chars": 65683,
"preview": "#include \"ikd_Tree.h\"\n\n/*\nDescription: ikd-Tree: an incremental k-d tree for robotic applications \nAuthor: Yixi Cai\nemai"
},
{
"path": "include/ikd-Tree/ikd_Tree.h",
"chars": 6571,
"preview": "#pragma once\n#include <pcl/point_types.h>\n#include <Eigen/StdVector>\n#include <Eigen/Geometry>\n#include <stdio.h>\n#inclu"
},
{
"path": "include/matplotlibcpp.h",
"chars": 81831,
"preview": "#pragma once\n\n// Python headers must be included before any system headers, since\n// they define _POSIX_C_SOURCE\n#includ"
},
{
"path": "include/preprocess.h",
"chars": 5292,
"preview": "#ifndef PREPROCESS_H\n#define PREPROCESS_H\n\n#include \"common_lib.h\"\n#include <ros/ros.h>\n#include <pcl_conversions/pcl_co"
},
{
"path": "include/scope_timer.hpp",
"chars": 2906,
"preview": "//\n// Created by yunfan on 2021/3/19.\n// Version: 1.0.0\n//\n\n\n#ifndef SCROPE_TIMER_HPP//SRC_POLY_VISUAL_UTILS_HPP\n#define"
},
{
"path": "include/so3_math.h",
"chars": 3987,
"preview": "#ifndef SO3_MATH_H\n#define SO3_MATH_H\n\n#include <math.h>\n#include <Eigen/Core>\n\n#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1]"
},
{
"path": "launch/hilti/hilti_basement_3.launch",
"chars": 664,
"preview": "<launch>\n <!-- Launch file for ouster Ouster LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command"
},
{
"path": "launch/hilti/hilti_basement_4.launch",
"chars": 664,
"preview": "<launch>\n <!-- Launch file for ouster Ouster LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command"
},
{
"path": "launch/m2dgr/m2dgr_gate01.launch",
"chars": 880,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <!-- Image conversion --"
},
{
"path": "launch/m2dgr/m2dgr_hall04.launch",
"chars": 880,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <!-- Image conversion --"
},
{
"path": "launch/m2dgr/m2dgr_lift04.launch",
"chars": 880,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <!-- Image conversion --"
},
{
"path": "launch/m2dgr/m2dgr_rotation02.launch",
"chars": 884,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <!-- Image conversion --"
},
{
"path": "launch/m2dgr/m2dgr_street08.launch",
"chars": 881,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <!-- Image conversion --"
},
{
"path": "launch/ouster.launch",
"chars": 659,
"preview": "<launch>\n <!-- Launch file for ouster Ouster LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command"
},
{
"path": "launch/s3e/s3e_bob_lab01.launch",
"chars": 863,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <!-- Image conversion --"
},
{
"path": "launch/s3e/s3e_bob_lab02.launch",
"chars": 863,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <!-- Image conversion --"
},
{
"path": "launch/velodyne.launch",
"chars": 650,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command=\"load\""
},
{
"path": "msg/Pose6D.msg",
"chars": 597,
"preview": "# the preintegrated Lidar states at the time of IMU measurements in a frame\nfloat64 offset_time # the offset time of IM"
},
{
"path": "msg/States.msg",
"chars": 569,
"preview": "Header header # timestamp of the first lidar in a frame\nfloat64[] rot_end # the estimated attitude (rotati"
},
{
"path": "package.xml",
"chars": 1467,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>gril_calib</name>\n <version>0.0.0</version>\n\n <description>\n This is a modi"
},
{
"path": "result/traj.txt",
"chars": 0,
"preview": ""
},
{
"path": "rviz_cfg/.gitignore",
"chars": 0,
"preview": ""
},
{
"path": "rviz_cfg/hilti.rviz",
"chars": 10905,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rviz_cfg/m2dgr.rviz",
"chars": 12210,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rviz_cfg/s3e.rviz",
"chars": 12614,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 126\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rviz_cfg/spinning.rviz",
"chars": 11796,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "src/laserMapping.cpp",
"chars": 61250,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// J. Zhang and S. Singh. "
},
{
"path": "src/preprocess.cpp",
"chars": 41982,
"preview": "#include \"preprocess.h\"\n\n#define RETURN0 0x00\n#define RETURN0AND1 0x10\n\nconst bool time_list_cut_frame(PointType &x,"
}
]
About this extraction
This page contains the full source code of the Taeyoung96/GRIL-Calib GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 64 files (574.8 KB), approximately 160.4k tokens, and a symbol index with 197 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.