admin/Point-LIO_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

point-lio with grid map

Browse Source
This commit is contained in:
Joanna-HE
2024-01-18 08:54:06 -05:00
commit d4919a8c6a
70 changed files with 27659 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
CMakeLists.txt Executable file
View File

@@ -0,0 +1,86 @@
cmake_minimum_required(VERSION 2.8.3)
project(point_lio)
SET(CMAKE_BUILD_TYPE "Debug")
ADD_COMPILE_OPTIONS(-std=c++14 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
include(ProcessorCount)
ProcessorCount(N)
message("Processer number: ${N}")
if(N GREATER 5)
add_definitions(-DMP_EN)
add_definitions(-DMP_PROC_NUM=4)
message("core for MP: 3")
elseif(N GREATER 3)
math(EXPR PROC_NUM "${N} - 2")
add_definitions(-DMP_EN)
add_definitions(-DMP_PROC_NUM="${PROC_NUM}")
message("core for MP: ${PROC_NUM}")
else()
add_definitions(-DMP_PROC_NUM=1)
endif()
else()
add_definitions(-DMP_PROC_NUM=1)
endif()
find_package(OpenMP QUIET)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
find_package(PythonLibs REQUIRED)
find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
std_msgs
pcl_ros
tf
livox_ros_driver
message_generation
eigen_conversions
)
find_package(Eigen3 REQUIRED)
find_package(PCL 1.8 REQUIRED)
message(Eigen: ${EIGEN3_INCLUDE_DIR})
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
${PYTHON_INCLUDE_DIRS}
include)
link_directories(
include
${PCL_LIBRARY_DIRS}
)
add_executable(pointlio_mapping src/laserMapping.cpp
src/li_initialization.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp
src/IMU_Processing.cpp)
target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})

34
LICENSE Executable file
View File

@@ -0,0 +1,34 @@
This is an advanced implementation of the algorithm described in the following paper:
J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
Modifier: livox dev@livoxtech.com
Copyright 2013, Ji Zhang, Carnegie Mellon University
Further contributions copyright (c) 2016, Southwest Research Institute
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

0
Log/IMU_before_filter.txt Normal file
View File

0
Log/IMU_meas.txt Normal file
View File

0
Log/LiDAR_meas.txt Normal file
View File

0
Log/Lidar_before_filter.txt Normal file
View File

0
Log/Lidar_omg_after_rot.txt Normal file
View File

0
Log/acc_cost.txt Normal file
View File

1
Log/guide.md Executable file
View File

@@ -0,0 +1 @@
Here saved the debug records which can be drew by the ../log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h.

0
Log/imu.txt Executable file
View File

0
Log/imu_pbp.txt Normal file
View File

0
Log/mat_out.txt Normal file
View File

46
Log/plot.py Executable file
View File

@@ -0,0 +1,46 @@
# import matplotlib
# matplotlib.use('Agg')
import numpy as np
import matplotlib.pyplot as plt
a_out=np.loadtxt('mat_out.txt')
#######for normal#######
fig, axs = plt.subplots(3,2)
lab_out = ['', 'out-x', 'out-y', 'out-z']
plot_ind = range(7,10)
time=a_out[:,0]
axs[0,0].set_title('Attitude')
axs[1,0].set_title('Translation')
axs[2,0].set_title('Velocity')
axs[0,1].set_title('bg')
axs[1,1].set_title('ba')
axs[2,1].set_title('Gravity')
for i in range(1,4):
for j in range(6):
axs[j%3, j//3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
for j in range(6):
axs[j%3, j//3].grid()
axs[j%3, j//3].legend()
plt.grid()
#######for normal#######
#### Draw IMU data
#fig, axs = plt.subplots(2)
#imu=np.loadtxt('imu_pbp.txt')
#time=imu[:,0]
#axs[0].set_title('Gyroscope')
#axs[1].set_title('Accelerameter')
#lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
#lab_2 = ['acc-x', 'acc-y', 'acc-z']
#for i in range(3):
#if i==1:
# axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
# axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
#for i in range(2):
#axs[i].set_xlim(386,389)
# axs[i].grid()
# axs[i].legend()
#plt.grid()
plt.show()

125
Log/plot_imu.py Executable file
View File

@@ -0,0 +1,125 @@
# import matplotlib
# matplotlib.use('Agg')
import numpy as np
import matplotlib.pyplot as plt
#### Draw IMU data
fig, axs = plt.subplots(2)
imu=np.loadtxt('imu_pbp.txt')
time=imu[:,0]
axs[0].set_title('Gyroscope')
axs[1].set_title('Accelerameter')
lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
lab_2 = ['acc-x', 'acc-y', 'acc-z']
for i in range(3):
#if i==1:
axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
for i in range(2):
#axs[i].set_xlim(386,389)
axs[i].grid()
axs[i].legend()
plt.grid()
#fig, axs = plt.subplots(5)
#axs[0].set_title('miss')
#axs[1].set_title('miss')
#axs[2].set_title('miss')
#axs[3].set_title('miss')
#axs[4].set_title('miss')
#len_time1 = np.arange(0,1977)
#len_time2 = np.arange(1977, 3954)
#len_time3 = np.arange(3954,5931)
#len_time4 = np.arange(5931,7908)
#len_time5 = np.arange(7908,9885)
#if i==1:
#axs[0].plot(len_time1, time[0:1977],'.-', label='check')
#axs[1].plot(len_time2, time[1977:3954],'.-', label='check')
#axs[2].plot(len_time3, time[3954:5931],'.-', label='check')
#axs[3].plot(len_time4, time[5931:7908],'.-', label='check')
#axs[4].plot(len_time5, time[7908:9885],'.-', label='check')
#axs[i].set_xlim(386,389)
#axs[0].grid()
#axs[0].legend()
#axs[1].grid()
#axs[1].legend()
#axs[2].grid()
#axs[2].legend()
#axs[3].grid()
#axs[3].legend()
#axs[4].grid()
#axs[4].legend()
#plt.grid()
#fig, axs = plt.subplots(5)
#axs[0].set_title('miss')
#axs[1].set_title('miss')
#axs[2].set_title('miss')
#axs[3].set_title('miss')
#axs[4].set_title('miss')
#len_time1 = np.arange(9885,9885+1977)
#len_time2 = np.arange(9885+1977,9885+3954)
#len_time3 = np.arange(9885+3954,9885+5931)
#len_time4 = np.arange(9885+5931,9885+7908)
#len_time5 = np.arange(9885+7908,9885+9885)
#if i==1:
#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check')
#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check')
#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check')
#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check')
#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check')
#axs[i].set_xlim(386,389)
#axs[0].grid()
#axs[0].legend()
#axs[1].grid()
#axs[1].legend()
#axs[2].grid()
#axs[2].legend()
#axs[3].grid()
#axs[3].legend()
#axs[4].grid()
#axs[4].legend()
#plt.grid()
# #### Draw time calculation
# plt.figure(3)
# fig = plt.figure()
# font1 = {'family' : 'Times New Roman',
# 'weight' : 'normal',
# 'size' : 12,
# }
# c="red"
# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
# # n = a_out[:,1].size
# # time_mean = a_out[:,1].mean()
# # time_se = a_out[:,1].std() / np.sqrt(n)
# # time_err = a_out[:,1] - time_mean
# # feat_mean = a_out[:,2].mean()
# # feat_err = a_out[:,2] - feat_mean
# # feat_se = a_out[:,2].std() / np.sqrt(n)
# ax1 = fig.add_subplot(111)
# ax1.set_ylabel('Effective Feature Numbers',font1)
# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
# ax1.set_ylim([0, 3000])
# ax2 = ax1.twinx()
# ax2.spines['right'].set_color('red')
# ax2.set_ylabel('Compute Time (ms)',font1)
# ax2.yaxis.label.set_color('red')
# ax2.tick_params(axis='y', colors='red')
# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.set_xlim([0.5, 3.5])
# ax2.set_ylim([0, 100])
# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
# # # print(time_se)
# # # print(a_out3[:,2])
# plt.grid()
# plt.savefig("time.pdf", dpi=1200)
plt.show()

178
Log/plot_out.py Executable file
View File

@@ -0,0 +1,178 @@
# import matplotlib
# matplotlib.use('Agg')
import numpy as np
import matplotlib.pyplot as plt
# a_pre=np.loadtxt('mat_pre.txt')
a_out=np.loadtxt('mat_out.txt')
if((a_out.shape[1] != 19) & (a_out.shape[1] != 20)):
######for ikfom
fig, axs = plt.subplots(4,2)
#lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
lab_out = ['', 'out-x', 'out-y', 'out-z']
plot_ind = range(7,10)
time=a_out[:,0]
axs[0,0].set_title('Attitude')
axs[1,0].set_title('Translation')
axs[2,0].set_title('Velocity')
axs[3,0].set_title('Angular velocity')
axs[0,1].set_title('Acceleration')
axs[1,1].set_title('Gravity')
axs[2,1].set_title('bg')
axs[3,1].set_title('ba')
for i in range(1,4):
for j in range(8):
#axs[j%4, j//4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i])
axs[j%4, j//4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
for j in range(8):
# axs[j].set_xlim(386,389)
axs[j%4, j//4].grid()
axs[j%4, j//4].legend()
plt.grid()
######for ikfom#######
else:
#######for normal#######
fig, axs = plt.subplots(3,2)
lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
lab_out = ['', 'out-x', 'out-y', 'out-z']
plot_ind = range(7,10)
time=a_out[:,0]
time1 = a_pre[:,0]
axs[0,0].set_title('Attitude')
axs[1,0].set_title('Translation')
axs[2,0].set_title('Velocity')
axs[0,1].set_title('bg')
axs[1,1].set_title('ba')
axs[2,1].set_title('Gravity')
for i in range(1,4):
for j in range(6):
axs[j%3, j/3].plot(time1, a_pre[:,i+j*3],'.-', label=lab_pre[i])
axs[j%3, j/3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
for j in range(6):
# axs[j].set_xlim(386,389)
axs[j%3, j//3].grid()
axs[j%3, j//3].legend()
plt.grid()
#######for normal#######
#### Draw IMU data
fig, axs = plt.subplots(2)
imu=np.loadtxt('imu_pbp.txt')
time=imu[:,0]
axs[0].set_title('Gyroscope')
axs[1].set_title('Accelerameter')
lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
lab_2 = ['acc-x', 'acc-y', 'acc-z']
for i in range(3):
#if i==1:
axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
for i in range(2):
#axs[i].set_xlim(386,389)
axs[i].grid()
axs[i].legend()
plt.grid()
#fig, axs = plt.subplots(5)
#axs[0].set_title('miss')
#axs[1].set_title('miss')
#axs[2].set_title('miss')
#axs[3].set_title('miss')
#axs[4].set_title('miss')
#len_time1 = np.arange(0,1977)
#len_time2 = np.arange(1977, 3954)
#len_time3 = np.arange(3954,5931)
#len_time4 = np.arange(5931,7908)
#len_time5 = np.arange(7908,9885)
#if i==1:
#axs[0].plot(len_time1, time[0:1977],'.-', label='check')
#axs[1].plot(len_time2, time[1977:3954],'.-', label='check')
#axs[2].plot(len_time3, time[3954:5931],'.-', label='check')
#axs[3].plot(len_time4, time[5931:7908],'.-', label='check')
#axs[4].plot(len_time5, time[7908:9885],'.-', label='check')
#axs[i].set_xlim(386,389)
#axs[0].grid()
#axs[0].legend()
#axs[1].grid()
#axs[1].legend()
#axs[2].grid()
#axs[2].legend()
#axs[3].grid()
#axs[3].legend()
#axs[4].grid()
#axs[4].legend()
#plt.grid()
#fig, axs = plt.subplots(5)
#axs[0].set_title('miss')
#axs[1].set_title('miss')
#axs[2].set_title('miss')
#axs[3].set_title('miss')
#axs[4].set_title('miss')
#len_time1 = np.arange(9885,9885+1977)
#len_time2 = np.arange(9885+1977,9885+3954)
#len_time3 = np.arange(9885+3954,9885+5931)
#len_time4 = np.arange(9885+5931,9885+7908)
#len_time5 = np.arange(9885+7908,9885+9885)
#if i==1:
#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check')
#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check')
#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check')
#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check')
#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check')
#axs[i].set_xlim(386,389)
#axs[0].grid()
#axs[0].legend()
#axs[1].grid()
#axs[1].legend()
#axs[2].grid()
#axs[2].legend()
#axs[3].grid()
#axs[3].legend()
#axs[4].grid()
#axs[4].legend()
#plt.grid()
# #### Draw time calculation
# plt.figure(3)
# fig = plt.figure()
# font1 = {'family' : 'Times New Roman',
# 'weight' : 'normal',
# 'size' : 12,
# }
# c="red"
# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
# # n = a_out[:,1].size
# # time_mean = a_out[:,1].mean()
# # time_se = a_out[:,1].std() / np.sqrt(n)
# # time_err = a_out[:,1] - time_mean
# # feat_mean = a_out[:,2].mean()
# # feat_err = a_out[:,2] - feat_mean
# # feat_se = a_out[:,2].std() / np.sqrt(n)
# ax1 = fig.add_subplot(111)
# ax1.set_ylabel('Effective Feature Numbers',font1)
# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
# ax1.set_ylim([0, 3000])
# ax2 = ax1.twinx()
# ax2.spines['right'].set_color('red')
# ax2.set_ylabel('Compute Time (ms)',font1)
# ax2.yaxis.label.set_color('red')
# ax2.tick_params(axis='y', colors='red')
# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.set_xlim([0.5, 3.5])
# ax2.set_ylim([0, 100])
# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
# # # print(time_se)
# # # print(a_out3[:,2])
# plt.grid()
# plt.savefig("time.pdf", dpi=1200)
plt.show()

38
Log/plot_rtk.py Executable file
View File

@@ -0,0 +1,38 @@
import numpy as np
import matplotlib.pyplot as plt
a_grdt=np.loadtxt('pos_rtk.txt')
a_out=np.loadtxt('mat_out.txt')
#######for normal#######
fig, axs = plt.subplots(3,1)
lab_grdt = ['', 'gt-x', 'gt-y', 'gt-z']
lab_out = ['', 'out-x', 'out-y', 'out-z']
plot_ind = range(7,10)
time=a_out[:,0]
time1 = a_grdt[:,0]
axs[0].set_title('East [m]')
axs[1].set_title('North [m]')
axs[2].set_title('Up [m]')
for i in range(1,4):
axs[i-1].plot(time1, a_grdt[:,i],'.-', label=lab_grdt[i])
axs[i-1].plot(time, a_out[:,i+3],'.-', label=lab_out[i])
for j in range(1,4):
axs[j-1].grid()
axs[j-1].legend()
plt.grid()
#######for normal#######
fig, axs = plt.subplots(2,1)
axs[0].set_title('clock drift')
lab_1 = ['dt-g', 'dt-r', 'dt-e', 'dt-c']
for i in range(3):
#if i==1:
axs[0].plot(time, a_out[:,i+13],'.-', label=lab_1[i])
axs[0].plot(time, a_out[:,19],'.-', label=lab_1[3])
for i in range(1):
#axs[i].set_xlim(386,389)
axs[i].grid()
axs[i].legend()
plt.grid()
plt.show()

0
Log/pos_log.txt Normal file
View File

13978
Log/pos_rtk.txt Normal file
View File

File diff suppressed because it is too large Load Diff

174
README.md Normal file
View File

@@ -0,0 +1,174 @@
# Point-LIO
## Point-LIO: Robust High-Bandwidth Lidar-Inertial Odometry
## 1. Introduction
<div align="center">
<div align="center">
<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/toc4.png" width = 75% >
</div>
<font color=#a0a0a0 size=2>The framework and key points of the Point-LIO.</font>
</div>
**Point-LIO** is a robust and high-bandwidth LiDAR-inertial odometry with the capability to estimate extremely aggressive robotic motions. Point-LIO has two key novelties that enable a high-bandwidth LiDAR-inertial odometry (LIO). The first one is a point-by-point LIO framework, where the state is updated at each LiDAR point measurement without accumulating them into a frame. This point-by-point update allows an extremely high-frequency odometry output, significantly increases the odometry bandwidth, and also fundamentally removes the artificial in-frame motion distortion in aggressive motions. The second main novelty is a stochastic process-augmented kinematic model which models the IMU measurements as an output, instead of input as in existing filter-based odometry or SLAM systems, of the model. This new modeling method enables accurate localization and reliable mapping for aggressive motions even when IMU measurements are saturated. In experiments, Point-LIO is able to provide accurate, high-frequency odometry (4-8 kHz) and reliable mapping under severe vibrations and aggressive motions with high angular velocity (75 rad s^{-1}) beyond the IMU measuring ranges. And Point-LIO is computationally efficient, robust, versatile on public datasets with general motions. As an odometry, Point-LIO could be used in various autonomous tasks, such as trajectory planning, control, and perception, especially in cases involving very fast ego-motions (e.g., in the presence of severe vibration and high angular or linear velocity) or requiring high-rate odometry output and mapping (e.g., for high-rate feedback control and perception).
## **1.1. Developers:**
The codes of this repo are contributed by:
[Dongjiao He (贺东娇)](https://github.com/Joanna-HE) and [Wei Xu (徐威)](https://github.com/XW-HKU)
## **1.2. Related paper**
Our paper is accepted to Advanced Intelligent Systems(AIS), and is in the process of production. The preprint version is attached here, [Point-LIO_preprint.pdf](https://github.com/hku-mars/Point-LIO/files/10989136/Point-LIO_preprint.pdf)
## **1.3. Related video**
Our accompany video is available on **YouTube**.
<div align="center">
<a href="https://youtu.be/oS83xUs42Uw" target="_blank"><img src="https://github.com/hku-mars/Point-LIO/raw/master/image/final.png" width=60% /></a>
</div>
## 2. What can Point-LIO do?
### 2.1 Simultaneous LiDAR localization and mapping (SLAM) without motion distortion
### 2.2 Produce high odometry output frequence and high bandwidth
### 2.3 SLAM with aggressive motions even the IMU is saturated
# **3. Prerequisites**
## **3.1 Ubuntu and [ROS](https://www.ros.org/)**
We tested our code on Ubuntu18.04 with ros melodic and Ubuntu20.04 with noetic. Additional ROS package is required:
```
sudo apt-get install ros-xxx-pcl-conversions
```
## **3.2 Eigen**
Following the official [Eigen installation](eigen.tuxfamily.org/index.php?title=Main_Page), or directly install Eigen by:
```
sudo apt-get install libeigen3-dev
```
## **3.3 livox_ros_driver**
Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).
*Remarks:*
- Since the Point-LIO supports Livox serials LiDAR, so the **livox_ros_driver** must be installed and **sourced** before run any Point-LIO luanch file.
- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document).
## 4. Build
Clone the repository and catkin_make:
```
cd ~/$A_ROS_DIR$/src
git clone https://github.com/hku-mars/Point-LIO.git
cd Point-LIO
git submodule update --init
cd ../..
catkin_make
source devel/setup.bash
```
- Remember to source the livox_ros_driver before build (follow 3.3 **livox_ros_driver**)
- If you want to use a custom build of PCL, add the following line to ~/.bashrc
```export PCL_ROOT={CUSTOM_PCL_PATH}```
## 5. Directly run
Important notes:
A. Please make sure the IMU and LiDAR are **Synchronized**, that's important.
B. Please obtain the saturation values of your used IMU (i.e., accelerator and gyroscope), and the units of the accelerator of your used IMU, then modify the .yaml file according to those settings. That's improtant.
C. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important because Point-LIO processes at the sampling time of each LiDAR point.
D. We recommend to set the **extrinsic_est_en** to false if the extrinsic is give. As for the extrinsic initiallization, please refer to our recent work: [**Robust and Online LiDAR-inertial Initialization**](https://github.com/hku-mars/LiDAR_IMU_Init).
E. If a high odometry output frequency without downsample is required, set ``` publish_odometry_without_downsample ``` as true. Then the warning message of tf "TF_REPEATED_DATA" will pop up in the terminal window, because the time interval between two publish odometery is too small. The following command could be used to suppress this warning to a smaller frequency:
in your catkin_ws/src,
git clone --branch throttle-tf-repeated-data-error git@github.com:BadgerTechnologies/geometry2.git
Then rebuild, source setup.bash, run and then it should be reduced down to once every 10 seconds. If 10 seconds is still too much log output then change the ros::Duration(10.0) to 10000 seconds or whatever you like.
### 5.1 For Avia
Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
```
cd ~/$Point_LIO_ROS_DIR$
source devel/setup.bash
roslaunch point_lio mapping_avia.launch
roslaunch livox_ros_driver livox_lidar_msg.launch
```
- For livox serials, Point-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for Point-LIO. ``` livox_lidar.launch ``` can not produce it right now.
- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage.
### 5.2 For Livox serials with external IMU
mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:
Edit ``` config/avia.yaml ``` to set the below parameters:
1. LiDAR point cloud topic name: ``` lid_topic ```
2. IMU topic name: ``` imu_topic ```
3. Translational extrinsic: ``` extrinsic_T ```
4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
- The extrinsic parameters in Point-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual.
5. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro```
6. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ```
### 5.3 For Velodyne or Ouster (Velodyne as an example)
Step A: Setup before run
Edit ``` config/velodyne.yaml ``` to set the below parameters:
1. LiDAR point cloud topic name: ``` lid_topic ```
2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
3. Set the parameter ```timestamp_unit``` based on the unit of **time** (Velodyne) or **t** (Ouster) field in PoindCloud2 rostopic
4. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ```
5. Translational extrinsic: ``` extrinsic_T ```
6. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
- The extrinsic parameters in Point-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame).
7. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro```
8. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ```
Step B: Run below
```
cd ~/$Point_LIO_ROS_DIR$
source devel/setup.bash
roslaunch point_lio mapping_velody16.launch
```
Step C: Run LiDAR's ros driver or play rosbag.
### 5.4 PCD file save
Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` Point-LIO/PCD/scans.pcd ``` after the Point-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds.
*Tips for pcl_viewer:*
- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running.
```
1 is all random
2 is X values
3 is Y values
4 is Z values
5 is intensity
```
# **6. Examples**
## **6.1. Example-1: SLAM on datasets with aggressive motions where IMU is saturated**
<div align="center">
<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example1.gif" width="40%" />
<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example2.gif" width="54%" />
</div>
## **6.2. Example-2: Application on FPV and PULSAR**
<div align="center">
<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example3.gif" width="58%" />
<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example4.gif" width="35%" />
</div>
PULSAR is a self-rotating actuated by only one motor, [PULSAR](https://github.com/hku-mars/PULSAR)
## 7. Contact us
If you have any questions about this work, please feel free to contact me <hdj65822ATconnect.hku.hk> and Dr. Fu Zhang <fuzhangAThku.hk> via email.

54
config/avia.yaml Executable file
View File

@@ -0,0 +1,54 @@
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 1 # 4
scan_line: 6 # 32
timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 0.50
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.005 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 1.0 # 9.810 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001; 0.01
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 0.1
imu_meas_omg_cov: 0.1 #0.01 # 0.1
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # [0.0, 0.0, -9.787561] # gvins #
gravity_init: [0.0, 0.0, -9.810] # preknown gravity for unstationary start
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # 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.

53
config/horizon.yaml Executable file
View File

@@ -0,0 +1,53 @@
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 1
scan_line: 6
timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 4.0
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.005 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.01 #0.1 # 2
imu_meas_omg_cov: 0.01 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state
extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # 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.

53
config/ouster64.yaml Executable file
View File

@@ -0,0 +1,53 @@
common:
lid_topic: "/os_cloud_node/points"
imu_topic: "/os_cloud_node/imu"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 3 # 2 #velodyne # 1 Livox Avia LiDAR
scan_line: 64 # 32 #velodyne 6 avia
timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 4.0
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.01 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 2
imu_meas_omg_cov: 0.1 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state
extrinsic_T: [0.0, 0.0, 0.0]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # 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.

59
config/velody16.yaml Executable file
View File

@@ -0,0 +1,59 @@
common:
lid_topic: "/velodyne_points"
imu_topic: "/imu/data"
con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined
cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
time_diff_lidar_to_imu: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
preprocess:
lidar_type: 2
scan_line: 32
timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 2.0
mapping:
imu_en: true
extrinsic_est_en: false # for aggressive motion, set this variable false
imu_time_inte: 0.01 # = 1 / frequency of IMU
lidar_time_inte: 0.1
satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
lidar_meas_cov: 0.01 # 0.001
acc_cov_output: 500
gyr_cov_output: 1000
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
imu_meas_acc_cov: 0.1 #0.1 # 2
imu_meas_omg_cov: 0.1 #0.1 # 2
gyr_cov_input: 0.01 # for IMU as input model
acc_cov_input: 0.1 # for IMU as input model
plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
match_s: 81
ivox_grid_resolution: 2.0
gravity: [0.0, 0.0, -9.810] # [-0.30, 0.880, -9.76] # liosam [0.0, 9.810, 0.0] # # preknown gravity, use when imu_en is false or start from a non-stationary state
extrinsic_T: [ 0, 0, 0.28] # ulhk # [-0.5, 1.4, 1.5] # utbm
# extrinsic_R: [ 0, 1, 0,
# -1, 0, 0,
# 0, 0, 1 ] # ulhk 5 6
# extrinsic_R: [ 0, -1, 0,
# 1, 0, 0,
# 0, 0, 1 ] # utbm 1, 2
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ] # ulhk 4 utbm 3
odometry:
publish_odometry_without_downsample: false
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
scan_bodyframe_pub_en: false # 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.

18
include/.vscode/c_cpp_properties.json vendored Executable file
View File

@@ -0,0 +1,18 @@
{
"configurations": [
{
"browse": {
"databaseFilename": "",
"limitSymbolsToIncludedHeaders": true
},
"includePath": [
"/home/ecstasy/catkin_ws/devel/include/**",
"/opt/ros/melodic/include/**",
"/home/ecstasy/catkin_ws/src/FAST_LIO/include/**",
"/usr/include/**"
],
"name": "ROS"
}
],
"version": 4
}

6
include/.vscode/settings.json vendored Executable file
View File

@@ -0,0 +1,6 @@
{
"python.autoComplete.extraPaths": [
"/home/ecstasy/catkin_ws/devel/lib/python2.7/dist-packages",
"/opt/ros/melodic/lib/python2.7/dist-packages"
]
}

32
include/IKFoM/.gitignore vendored Normal file
View File

@@ -0,0 +1,32 @@
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app

5
include/IKFoM/.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,5 @@
{
"files.associations": {
"complex": "cpp"
}
}

62
include/IKFoM/IKFoM_toolkit/.vscode/settings.json vendored Executable file
View File

@@ -0,0 +1,62 @@
{
"files.associations": {
"iosfwd": "cpp",
"core": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"array": "cpp",
"atomic": "cpp",
"strstream": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"complex": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"list": "cpp",
"unordered_map": "cpp",
"vector": "cpp",
"exception": "cpp",
"fstream": "cpp",
"functional": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"new": "cpp",
"ostream": "cpp",
"numeric": "cpp",
"ratio": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"system_error": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"typeinfo": "cpp",
"algorithm": "cpp",
"iterator": "cpp",
"map": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"optional": "cpp",
"random": "cpp",
"set": "cpp",
"string": "cpp",
"string_view": "cpp"
}
}

5
include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json vendored Executable file
View File

@@ -0,0 +1,5 @@
{
"files.associations": {
"core": "cpp"
}
}

344
include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp Executable file
View File

@@ -0,0 +1,344 @@
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Author: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ESEKFOM_EKF_HPP
#define ESEKFOM_EKF_HPP
#include <vector>
#include <cstdlib>
#include <boost/bind.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Sparse>
#include "../mtk/types/vect.hpp"
#include "../mtk/types/SOn.hpp"
#include "../mtk/types/S2.hpp"
#include "../mtk/types/SEn.hpp"
#include "../mtk/startIdx.hpp"
#include "../mtk/build_manifold.hpp"
#include "util.hpp"
namespace esekfom {
using namespace Eigen;
template<typename T>
struct dyn_share_modified
{
bool valid;
bool converge;
T M_Noise;
Eigen::Matrix<T, Eigen::Dynamic, 1> z;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
Eigen::Matrix<T, 6, 1> z_IMU;
Eigen::Matrix<T, 6, 1> R_IMU;
bool satu_check[6];
};
template<typename state, int process_noise_dof, typename input = state, typename measurement=state, int measurement_noise_dof=0>
class esekf{
typedef esekf self;
enum{
n = state::DOF, m = state::DIM, l = measurement::DOF
};
public:
typedef typename state::scalar scalar_type;
typedef Matrix<scalar_type, n, n> cov;
typedef Matrix<scalar_type, m, n> cov_;
typedef SparseMatrix<scalar_type> spMt;
typedef Matrix<scalar_type, n, 1> vectorized_state;
typedef Matrix<scalar_type, m, 1> flatted_state;
typedef flatted_state processModel(state &, const input &);
typedef Eigen::Matrix<scalar_type, m, n> processMatrix1(state &, const input &);
typedef Eigen::Matrix<scalar_type, m, process_noise_dof> processMatrix2(state &, const input &);
typedef Eigen::Matrix<scalar_type, process_noise_dof, process_noise_dof> processnoisecovariance;
typedef void measurementModel_dyn_share_modified_cov(state &, Eigen::Matrix3d, Eigen::Matrix3d, dyn_share_modified<scalar_type> &);
typedef void measurementModel_dyn_share_modified(state &, dyn_share_modified<scalar_type> &);
typedef Eigen::Matrix<scalar_type ,l, n> measurementMatrix1(state &);
typedef Eigen::Matrix<scalar_type , Eigen::Dynamic, n> measurementMatrix1_dyn(state &);
typedef Eigen::Matrix<scalar_type ,l, measurement_noise_dof> measurementMatrix2(state &);
typedef Eigen::Matrix<scalar_type ,Eigen::Dynamic, Eigen::Dynamic> measurementMatrix2_dyn(state &);
typedef Eigen::Matrix<scalar_type, measurement_noise_dof, measurement_noise_dof> measurementnoisecovariance;
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> measurementnoisecovariance_dyn;
esekf(const state &x = state(),
const cov &P = cov::Identity()): x_(x), P_(P){};
void init_dyn_share_modified_2h(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified_cov h_dyn_share_in1)
{
f = f_in;
f_x = f_x_in;
// f_w = f_w_in;
h_dyn_share_modified_1 = h_dyn_share_in1;
// h_dyn_share_modified_3 = h_dyn_share_in3;
maximum_iter = 1;
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
x_.build_SEN_state();
}
void init_dyn_share_modified_3h(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified_cov h_dyn_share_in1, measurementModel_dyn_share_modified h_dyn_share_in2)
{
f = f_in;
f_x = f_x_in;
// f_w = f_w_in;
h_dyn_share_modified_1 = h_dyn_share_in1;
h_dyn_share_modified_2 = h_dyn_share_in2;
// h_dyn_share_modified_3 = h_dyn_share_in3;
maximum_iter = 1;
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
x_.build_SEN_state();
}
// iterated error state EKF propogation
void predict(double &dt, processnoisecovariance &Q, const input &i_in, bool predict_state, bool prop_cov){
if (predict_state)
{
flatted_state f_ = f(x_, i_in);
x_.oplus(f_, dt);
}
if (prop_cov)
{
flatted_state f_ = f(x_, i_in);
// state x_before = x_;
cov_ f_x_ = f_x(x_, i_in);
cov f_x_final;
F_x1 = cov::Identity();
for (std::vector<std::pair<std::pair<int, int>, int> >::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) {
int idx = (*it).first.first;
int dim = (*it).first.second;
int dof = (*it).second;
for(int i = 0; i < n; i++){
for(int j=0; j<dof; j++)
{f_x_final(idx+j, i) = f_x_(dim+j, i);}
}
}
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
int idx = (*it).first;
int dim = (*it).second;
for(int i = 0; i < 3; i++){
seg_SO3(i) = -1 * f_(dim + i) * dt;
}
// MTK::SO3<scalar_type> res;
// res.w() = MTK::exp<scalar_type, 3>(res.vec(), seg_SO3, scalar_type(1/2));
F_x1.template block<3, 3>(idx, idx) = MTK::SO3<scalar_type>::exp(seg_SO3); // res.normalized().toRotationMatrix();
res_temp_SO3 = MTK::A_matrix(seg_SO3);
for(int i = 0; i < n; i++){
f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i));
}
}
F_x1 += f_x_final * dt;
P_ = F_x1 * P_ * (F_x1).transpose() + Q * (dt * dt);
}
}
bool update_iterated_dyn_share_modified() {
dyn_share_modified<scalar_type> dyn_share;
state x_propagated = x_;
int dof_Measurement;
double m_noise;
for(int i=0; i<maximum_iter; i++)
{
dyn_share.valid = true;
h_dyn_share_modified_1(x_, P_.template block<3, 3>(0, 0), P_. template block<3, 3>(3, 3), dyn_share);
if(! dyn_share.valid)
{
return false;
// continue;
}
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> z = dyn_share.z;
// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
dof_Measurement = h_x.rows();
m_noise = dyn_share.M_Noise;
// dof_Measurement_noise = dyn_share.R.rows();
// vectorized_state dx, dx_new;
// x_.boxminus(dx, x_propagated);
// dx_new = dx;
// P_ = P_propagated;
Matrix<scalar_type, n, Eigen::Dynamic> PHT;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> HPHT;
Matrix<scalar_type, n, Eigen::Dynamic> K_;
if(n > dof_Measurement)
{
PHT = P_. template block<n, 12>(0, 0) * h_x.transpose();
HPHT = h_x * PHT.topRows(12);
for (int m = 0; m < dof_Measurement; m++)
{
HPHT(m, m) += m_noise;
}
K_= PHT*HPHT.inverse();
}
else
{
Matrix<scalar_type, 12, 12> HTH = m_noise * h_x.transpose() * h_x;
Matrix<scalar_type, n, n> P_inv = P_.inverse();
P_inv.template block<12, 12>(0, 0) += HTH;
P_inv = P_inv.inverse();
K_ = P_inv.template block<n, 12>(0, 0) * h_x.transpose() * m_noise;
}
Matrix<scalar_type, n, 1> dx_ = K_ * z; // - h) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
// state x_before = x_;
x_.boxplus(dx_);
{
P_ = P_ - K_*h_x*P_. template block<12, n>(0, 0);
}
}
return true;
}
void update_iterated_dyn_share_IMU() {
dyn_share_modified<scalar_type> dyn_share;
for(int i=0; i<maximum_iter; i++)
{
dyn_share.valid = true;
h_dyn_share_modified_2(x_, dyn_share);
Matrix<scalar_type, 6, 1> z = dyn_share.z_IMU;
Matrix<double, 30, 6> PHT;
Matrix<double, 6, 30> HP;
Matrix<double, 6, 6> HPHT;
PHT.setZero();
HP.setZero();
HPHT.setZero();
for (int l_ = 0; l_ < 6; l_++)
{
if (!dyn_share.satu_check[l_])
{
PHT.col(l_) = P_.col(15+l_) + P_.col(24+l_);
HP.row(l_) = P_.row(15+l_) + P_.row(24+l_);
}
}
for (int l_ = 0; l_ < 6; l_++)
{
if (!dyn_share.satu_check[l_])
{
HPHT.col(l_) = HP.col(15+l_) + HP.col(24+l_);
}
HPHT(l_, l_) += dyn_share.R_IMU(l_); //, l);
}
Eigen::Matrix<double, 30, 6> K = PHT * HPHT.inverse();
Matrix<scalar_type, n, 1> dx_ = K * z;
P_ -= K * HP;
x_.boxplus(dx_);
}
return;
}
void change_x(state &input_state)
{
x_ = input_state;
if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size())&&(!x_.SEN_state.size()))
{
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
x_.build_SEN_state();
}
}
void change_P(cov &input_cov)
{
P_ = input_cov;
}
const state& get_x() const {
return x_;
}
const cov& get_P() const {
return P_;
}
cov P_;
state x_;
private:
measurement m_;
spMt l_;
spMt f_x_1;
spMt f_x_2;
cov F_x1 = cov::Identity();
cov F_x2 = cov::Identity();
cov L_ = cov::Identity();
processModel *f;
processMatrix1 *f_x;
processMatrix2 *f_w;
measurementMatrix1 *h_x;
measurementMatrix2 *h_v;
measurementMatrix1_dyn *h_x_dyn;
measurementMatrix2_dyn *h_v_dyn;
measurementModel_dyn_share_modified_cov *h_dyn_share_modified_1;
measurementModel_dyn_share_modified *h_dyn_share_modified_2;
measurementModel_dyn_share_modified *h_dyn_share_modified_3;
int maximum_iter = 0;
scalar_type limit[n];
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace esekfom
#endif // ESEKFOM_EKF_HPP

82
include/IKFoM/IKFoM_toolkit/esekfom/util.hpp Executable file
View File

@@ -0,0 +1,82 @@
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Author: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef __MEKFOM_UTIL_HPP__
#define __MEKFOM_UTIL_HPP__
#include <Eigen/Core>
#include "../mtk/src/mtkmath.hpp"
namespace esekfom {
template <typename T1, typename T2>
class is_same {
public:
operator bool() {
return false;
}
};
template<typename T1>
class is_same<T1, T1> {
public:
operator bool() {
return true;
}
};
template <typename T>
class is_double {
public:
operator bool() {
return false;
}
};
template<>
class is_double<double> {
public:
operator bool() {
return true;
}
};
template<typename T>
static T
id(const T &x)
{
return x;
}
} // namespace esekfom
#endif // __MEKFOM_UTIL_HPP__

248
include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp Executable file
View File

@@ -0,0 +1,248 @@
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/build_manifold.hpp
* @brief Macro to automatically construct compound manifolds.
*
*/
#ifndef MTK_AUTOCONSTRUCT_HPP_
#define MTK_AUTOCONSTRUCT_HPP_
#include <vector>
#include <boost/preprocessor/seq.hpp>
#include <boost/preprocessor/cat.hpp>
#include <Eigen/Core>
#include "src/SubManifold.hpp"
#include "startIdx.hpp"
#ifndef PARSED_BY_DOXYGEN
//////// internals //////
#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple
#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))
#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)
#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type()
#define MTK_CONSTRUCTOR_COPY( type, id) id(id)
#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale);
#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale);
#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id);
#define MTK_HAT( type, id) if(id.IDX == idx){id.hat(vec, res);}
#define MTK_JACOB_RIGHT_INV( type, id) if(id.IDX == idx){id.Jacob_right_inv(vec, res);}
#define MTK_JACOB_RIGHT( type, id) if(id.IDX == idx){id.Jacob_right(vec, res);}
#define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);}
#define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);}
#define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);}
#define MTK_OSTREAM( type, id) << __var.id << " "
#define MTK_ISTREAM( type, id) >> __var.id
#define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));}
#define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));}
#define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));}
#define MTK_SEN_state( type, id) if(id.TYP == 4){(SEN_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));}
#define MTK_SUBVARLIST(seq, S2state, SO3state, SENstate) \
BOOST_PP_FOR_1( \
( \
BOOST_PP_SEQ_SIZE(seq), \
BOOST_PP_SEQ_HEAD(seq), \
BOOST_PP_SEQ_TAIL(seq) (~), \
0,\
0,\
S2state,\
SO3state,\
SENstate ),\
MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT)
#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
MTK::SubManifold<type, dof, dim> id;
#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state, SENstate) \
MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
enum {DOF = type::DOF + dof}; \
enum {DIM = type::DIM+dim}; \
typedef type::scalar scalar;
#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state
#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state, SENstate) \
MTK_APPLY_MACRO_ON_TUPLE(~, \
BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \
( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))
#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state
//! this used to be BOOST_PP_TUPLE_ELEM_4_0:
#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g, h) a
#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state
#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate) ( \
BOOST_PP_DEC(len), \
BOOST_PP_SEQ_HEAD(seq), \
BOOST_PP_SEQ_TAIL(seq), \
dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\
dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\
S2state,\
SO3state,\
SENstate )
#endif /* not PARSED_BY_DOXYGEN */
/**
* Construct a manifold.
* @param name is the class-name of the manifold,
* @param entries is the list of sub manifolds
*
* Entries must be given in a list like this:
* @code
* typedef MTK::trafo<MTK::SO3<double> > Pose;
* typedef MTK::vect<double, 3> Vec3;
* MTK_BUILD_MANIFOLD(imu_state,
* ((Pose, pose))
* ((Vec3, vel))
* ((Vec3, acc_bias))
* )
* @endcode
* Whitespace is optional, but the double parentheses are necessary.
* Construction is done entirely in preprocessor.
* After construction @a name is also a manifold. Its members can be
* accessed by names given in @a entries.
*
* @note Variable types are not allowed to have commas, thus types like
* @c vect<double, 3> need to be typedef'ed ahead.
*/
#define MTK_BUILD_MANIFOLD(name, entries) \
struct name { \
typedef name self; \
std::vector<std::pair<int, int> > S2_state;\
std::vector<std::pair<int, int> > SO3_state;\
std::vector<std::pair<std::pair<int, int>, int> > vect_state;\
std::vector<std::pair<std::pair<int, int>, int> > SEN_state;\
MTK_SUBVARLIST(entries, S2_state, SO3_state, SEN_state) \
name ( \
MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \
) : \
MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\
int getDOF() const { return DOF; } \
void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
MTK_TRANSFORM(MTK_BOXPLUS, entries)\
} \
void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) { \
MTK_TRANSFORM(MTK_OPLUS, entries)\
} \
void boxminus(MTK::vectview<scalar,DOF> __res, const name& __oth) const { \
MTK_TRANSFORM(MTK_BOXMINUS, entries)\
} \
friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \
return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \
} \
void build_S2_state(){\
MTK_TRANSFORM(MTK_S2_state, entries)\
}\
void build_vect_state(){\
MTK_TRANSFORM(MTK_vect_state, entries)\
}\
void build_SO3_state(){\
MTK_TRANSFORM(MTK_SO3_state, entries)\
}\
void build_SEN_state(){\
MTK_TRANSFORM(MTK_SEN_state, entries)\
}\
void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
MTK_TRANSFORM(MTK_HAT, entries)\
}\
void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
MTK_TRANSFORM(MTK_JACOB_RIGHT_INV, entries)\
}\
void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
MTK_TRANSFORM(MTK_JACOB_RIGHT, entries)\
}\
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {\
MTK_TRANSFORM(MTK_S2_hat, entries)\
}\
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) {\
MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\
}\
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, Eigen::Matrix<scalar, 2, 1> dx, int idx) {\
MTK_TRANSFORM(MTK_S2_Mx, entries)\
}\
friend std::istream& operator>>(std::istream& __is, name& __var){ \
return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \
} \
};
#endif /*MTK_AUTOCONSTRUCT_HPP_*/

123
include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp Executable file
View File

@@ -0,0 +1,123 @@
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/src/SubManifold.hpp
* @brief Defines the SubManifold class
*/
#ifndef SUBMANIFOLD_HPP_
#define SUBMANIFOLD_HPP_
#include "vectview.hpp"
namespace MTK {
/**
* @ingroup SubManifolds
* Helper class for compound manifolds.
* This class wraps a manifold T and provides an enum IDX refering to the
* index of the SubManifold within the compound manifold.
*
* Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds".
*
* @tparam T The manifold type of the sub-type
* @tparam idx The index of the sub-type within the compound manifold
*/
template<class T, int idx, int dim>
struct SubManifold : public T
{
enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };
//! manifold type
typedef T type;
//! Construct from derived type
template<class X>
explicit
SubManifold(const X& t) : T(t) {};
//! Construct from internal type
//explicit
SubManifold(const T& t) : T(t) {};
//! inherit assignment operator
using T::operator=;
};
} // namespace MTK
#endif /* SUBMANIFOLD_HPP_ */

294
include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp Executable file
View File

@@ -0,0 +1,294 @@
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/src/mtkmath.hpp
* @brief several math utility functions.
*/
#ifndef MTKMATH_H_
#define MTKMATH_H_
#include <cmath>
#include <boost/math/tools/precision.hpp>
#include "../types/vect.hpp"
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
namespace MTK {
namespace internal {
template<class Manifold>
struct traits {
typedef typename Manifold::scalar scalar;
enum {DOF = Manifold::DOF};
typedef vect<DOF, scalar> vectorized_type;
typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
};
template<>
struct traits<float> : traits<Scalar<float> > {};
template<>
struct traits<double> : traits<Scalar<double> > {};
} // namespace internal
/**
* \defgroup MTKMath Mathematical helper functions
*/
//@{
//! constant @f$ \pi @f$
const double pi = M_PI;
template<class scalar> inline scalar tolerance();
template<> inline float tolerance<float >() { return 1e-5f; }
template<> inline double tolerance<double>() { return 1e-11; }
/**
* normalize @a x to @f$[-bound, bound] @f$.
*
* result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
*/
template<class scalar>
inline scalar normalize(scalar x, scalar bound){ //not used
if(std::fabs(x) <= bound) return x;
int r = (int)(x *(scalar(1.0)/ bound));
return x - ((r + (r>>31) + 1) & ~1)*bound;
}
/**
* Calculate cosine and sinc of sqrt(x2).
* @param x2 the squared angle must be non-negative
* @return a pair containing cos and sinc of sqrt(x2)
*/
template<class scalar>
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
using std::sqrt;
using std::cos;
using std::sin;
static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
static scalar const taylor_2_bound = sqrt(taylor_0_bound);
static scalar const taylor_n_bound = sqrt(taylor_2_bound);
assert(x2>=0 && "argument must be non-negative and must not be nan/-nan");
// FIXME check if bigger bounds are possible
if(x2>=taylor_n_bound) {
// slow fall-back solution
scalar x = sqrt(x2);
return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
}
// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
// TODO Find optimal coefficients using Remez algorithm
static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
scalar cosi = 1., sinc=1;
scalar term = -1/2. * x2;
for(int i=0; i<3; ++i) {
cosi += term;
term *= inv[2*i];
sinc += term;
term *= -inv[2*i+1] * x2;
}
return std::make_pair(cosi, sinc);
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
Eigen::Matrix<typename Base::scalar, 3, 3> res;
res << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
return res;
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v){
Eigen::Matrix<typename Base::scalar, 3, 3> res;
if(v.norm() > MTK::tolerance<typename Base::scalar>())
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
}
else
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
}
return res;
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v){
Eigen::Matrix<typename Base::scalar, 3, 3> res;
if(v.norm() > MTK::tolerance<typename Base::scalar>())
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() - 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
}
else
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
}
return res;
}
template<typename scalar>
Eigen::Matrix<scalar, 2, 3> S2_w_expw_( Eigen::Matrix<scalar, 2, 1> v, scalar length)
{
Eigen::Matrix<scalar, 2, 3> res;
scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]);
if(norm < MTK::tolerance<scalar>()){
res = Eigen::Matrix<scalar, 2, 3>::Zero();
res(0, 1) = 1;
res(1, 2) = 1;
res /= length;
}
else{
res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0,
-v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm);
res /= length;
}
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){
Eigen::Matrix<typename Base::scalar, 3, 3> res;
double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
double norm = std::sqrt(squaredNorm);
if(norm < MTK::tolerance<typename Base::scalar>()){
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
}
else{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);
}
return res;
}
template<class scalar, int n>
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
scalar norm2 = vec.squaredNorm();
std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
scalar mult = cos_sinc.second * scale;
result = mult * vec;
return cos_sinc.first;
}
/**
* Inverse function to @c exp.
*
* @param result @c vectview to the result
* @param w scalar part of input
* @param vec vector part of input
* @param scale scale result by this value
* @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
*/
template<class scalar, int n>
void log(vectview<scalar, n> result,
const scalar &w, const vectview<const scalar, n> vec,
const scalar &scale, bool plus_minus_periodicity)
{
// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
scalar nv = vec.norm();
if(nv < tolerance<scalar>()) {
if(!plus_minus_periodicity && w < 0) {
// find the maximal entry:
int i;
nv = vec.cwiseAbs().maxCoeff(&i);
result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
return;
}
nv = tolerance<scalar>();
}
scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );
result = s * vec;
}
} // namespace MTK
#endif /* MTKMATH_H_ */

168
include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp Executable file
View File

@@ -0,0 +1,168 @@
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/src/vectview.hpp
* @brief Wrapper class around a pointer used as interface for plain vectors.
*/
#ifndef VECTVIEW_HPP_
#define VECTVIEW_HPP_
#include <Eigen/Core>
namespace MTK {
/**
* A view to a vector.
* Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.
* The dimension of the vector is given as template parameter and type-checked when used in expressions.
* Data has to be modifiable.
*
* @tparam scalar Scalar type of the vector.
* @tparam dim Dimension of the vector.
*
* @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct
*/
namespace internal {
template<class Base, class T1, class T2>
struct CovBlock {
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> ConstType;
};
template<class Base, class T1, class T2>
struct CovBlock_ {
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> ConstType;
};
template<typename Base1, typename Base2, typename T1, typename T2>
struct CrossCovBlock {
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> ConstType;
};
template<typename Base1, typename Base2, typename T1, typename T2>
struct CrossCovBlock_ {
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> ConstType;
};
template<class scalar, int dim>
struct VectviewBase {
typedef Eigen::Matrix<scalar, dim, 1> matrix_type;
typedef typename matrix_type::MapType Type;
typedef typename matrix_type::ConstMapType ConstType;
};
template<class T>
struct UnalignedType {
typedef T type;
};
}
template<class scalar, int dim>
class vectview : public internal::VectviewBase<scalar, dim>::Type {
typedef internal::VectviewBase<scalar, dim> VectviewBase;
public:
//! plain matrix type
typedef typename VectviewBase::matrix_type matrix_type;
//! base type
typedef typename VectviewBase::Type base;
//! construct from pointer
explicit
vectview(scalar* data, int dim_=dim) : base(data, dim_) {}
//! construct from plain matrix
vectview(matrix_type& m) : base(m.data(), m.size()) {}
//! construct from another @c vectview
vectview(const vectview &v) : base(v) {}
//! construct from Eigen::Block:
template<class Base>
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0), block.size()) {}
template<class Base, bool PacketAccess>
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0), block.size()) {}
//! inherit assignment operator
using base::operator=;
//! data pointer
scalar* data() {return const_cast<scalar*>(base::data());}
};
/**
* @c const version of @c vectview.
* Compared to @c Eigen::Map this implementation is const correct, i.e.,
* data will not be modifiable using this view.
*
* @tparam scalar Scalar type of the vector.
* @tparam dim Dimension of the vector.
*
* @sa vectview
*/
template<class scalar, int dim>
class vectview<const scalar, dim> : public internal::VectviewBase<scalar, dim>::ConstType {
typedef internal::VectviewBase<scalar, dim> VectviewBase;
public:
//! plain matrix type
typedef typename VectviewBase::matrix_type matrix_type;
//! base type
typedef typename VectviewBase::ConstType base;
//! construct from const pointer
explicit
vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}
//! construct from column vector
template<int options>
vectview(const Eigen::Matrix<scalar, dim, 1, options>& m) : base(m.data()) {}
//! construct from row vector
template<int options, int phony>
vectview(const Eigen::Matrix<scalar, 1, dim, options, phony>& m) : base(m.data()) {}
//! construct from another @c vectview
vectview(vectview<scalar, dim> x) : base(x.data()) {}
//! construct from base
vectview(const base &x) : base(x) {}
/**
* Construct from Block
* @todo adapt this, when Block gets const-correct
*/
template<class Base>
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0)) {}
template<class Base, bool PacketAccess>
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0)) {}
};
} // namespace MTK
#endif /* VECTVIEW_HPP_ */

328
include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp Executable file
View File

@@ -0,0 +1,328 @@
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/startIdx.hpp
* @brief Tools to access sub-elements of compound manifolds.
*/
#ifndef GET_START_INDEX_H_
#define GET_START_INDEX_H_
#include <Eigen/Core>
#include "src/SubManifold.hpp"
#include "src/vectview.hpp"
namespace MTK {
/**
* \defgroup SubManifolds Accessing Submanifolds
* For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers
* can be used to get sub-vectors or matrix-blocks of a corresponding big matrix.
* E.g. for a type @a pose consisting of @a orient and @a trans the member pointers
* @c &pose::orient and @c &pose::trans give all required information and are still
* valid if the base type gets extended or the actual types of @a orient and @a trans
* change (e.g. from 2D to 3D).
*
* @todo Maybe require manifolds to typedef MatrixType and VectorType, etc.
*/
//@{
/**
* Determine the index of a sub-variable within a compound variable.
*/
template<class Base, class T, int idx, int dim>
int getStartIdx( MTK::SubManifold<T, idx, dim> Base::*)
{
return idx;
}
template<class Base, class T, int idx, int dim>
int getStartIdx_( MTK::SubManifold<T, idx, dim> Base::*)
{
return dim;
}
/**
* Determine the degrees of freedom of a sub-variable within a compound variable.
*/
template<class Base, class T, int idx, int dim>
int getDof( MTK::SubManifold<T, idx, dim> Base::*)
{
return T::DOF;
}
template<class Base, class T, int idx, int dim>
int getDim( MTK::SubManifold<T, idx, dim> Base::*)
{
return T::DIM;
}
/**
* set the diagonal elements of a covariance matrix corresponding to a sub-variable
*/
template<class Base, class T, int idx, int dim>
void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
{
cov.diagonal().template segment<T::DOF>(idx).setConstant(val);
}
template<class Base, class T, int idx, int dim>
void setDiagonal_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
{
cov.diagonal().template segment<T::DIM>(dim).setConstant(val);
}
/**
* Get the subblock of corresponding to two members, i.e.
* \code
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
* MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression;
* MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans();
* \endcode
* lets you modify mixed covariance entries in a bigger covariance matrix.
*/
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
typename MTK::internal::CovBlock<Base, T1, T2>::Type
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
{
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
}
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
typename MTK::internal::CovBlock_<Base, T1, T2>::Type
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
{
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
}
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
typename MTK::internal::CrossCovBlock<Base1, Base2, T1, T2>::Type
subblock(Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
{
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
}
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
typename MTK::internal::CrossCovBlock_<Base1, Base2, T1, T2>::Type
subblock_(Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
{
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
}
/**
* Get the subblock of corresponding to a member, i.e.
* \code
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
* MTK::subblock(m, &Pose::orient) = some_expression;
* \endcode
* lets you modify covariance entries in a bigger covariance matrix.
*/
template<class Base, class T, int idx, int dim>
typename MTK::internal::CovBlock_<Base, T, T>::Type
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
MTK::SubManifold<T, idx, dim> Base::*)
{
return cov.template block<T::DIM, T::DIM>(dim, dim);
}
template<class Base, class T, int idx, int dim>
typename MTK::internal::CovBlock<Base, T, T>::Type
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
MTK::SubManifold<T, idx, dim> Base::*)
{
return cov.template block<T::DOF, T::DOF>(idx, idx);
}
template<typename Base>
class get_cov {
public:
typedef Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> type;
typedef const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> const_type;
};
template<typename Base>
class get_cov_ {
public:
typedef Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> type;
typedef const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> const_type;
};
template<typename Base1, typename Base2>
class get_cross_cov {
public:
typedef Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> type;
typedef const type const_type;
};
template<typename Base1, typename Base2>
class get_cross_cov_ {
public:
typedef Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> type;
typedef const type const_type;
};
template<class Base, class T, int idx, int dim>
vectview<typename Base::scalar, T::DIM>
subvector_impl_(vectview<typename Base::scalar, Base::DIM> vec, SubManifold<T, idx, dim> Base::*)
{
return vec.template segment<T::DIM>(dim);
}
template<class Base, class T, int idx, int dim>
vectview<typename Base::scalar, T::DOF>
subvector_impl(vectview<typename Base::scalar, Base::DOF> vec, SubManifold<T, idx, dim> Base::*)
{
return vec.template segment<T::DOF>(idx);
}
/**
* Get the subvector corresponding to a sub-manifold from a bigger vector.
*/
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<Scalar, T::DIM>
subvector_(vectview<Scalar, BaseDIM> vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl_(vec, ptr);
}
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<Scalar, T::DOF>
subvector(vectview<Scalar, BaseDOF> vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(vec, ptr);
}
/**
* @todo This should be covered already by subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
*/
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<Scalar, T::DOF>
subvector(Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(vectview<Scalar, BaseDOF>(vec), ptr);
}
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<Scalar, T::DIM>
subvector_(Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl_(vectview<Scalar, BaseDIM>(vec), ptr);
}
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DIM>
subvector_(const Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl_(vectview<const Scalar, BaseDIM>(vec), ptr);
}
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DOF>
subvector(const Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(vectview<const Scalar, BaseDOF>(vec), ptr);
}
/**
* const version of subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
*/
template<class Base, class T, int idx, int dim>
vectview<const typename Base::scalar, T::DOF>
subvector_impl(const vectview<const typename Base::scalar, Base::DOF> cvec, SubManifold<T, idx, dim> Base::*)
{
return cvec.template segment<T::DOF>(idx);
}
template<class Base, class T, int idx, int dim>
vectview<const typename Base::scalar, T::DIM>
subvector_impl_(const vectview<const typename Base::scalar, Base::DIM> cvec, SubManifold<T, idx, dim> Base::*)
{
return cvec.template segment<T::DIM>(dim);
}
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DOF>
subvector(const vectview<const Scalar, BaseDOF> cvec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(cvec, ptr);
}
} // namespace MTK
#endif // GET_START_INDEX_H_

327
include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp Executable file
View File

@@ -0,0 +1,327 @@
// This is a NEW implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/types/S2.hpp
* @brief Unit vectors on the sphere, or directions in 3D.
*/
#ifndef S2_H_
#define S2_H_
#include "vect.hpp"
#include "SOn.hpp"
#include "../src/mtkmath.hpp"
namespace MTK {
/**
* Manifold representation of @f$ S^2 @f$.
* Used for unit vectors on the sphere or directions in 3D.
*
* @todo add conversions from/to polar angles?
*/
template<class _scalar = double, int den = 1, int num = 1, int S2_typ = 3>
struct S2 {
typedef _scalar scalar;
typedef vect<3, scalar> vect_type;
typedef SO3<scalar> SO3_type;
typedef typename vect_type::base vec3;
scalar length = scalar(den)/scalar(num);
enum {DOF=2, TYP = 1, DIM = 3};
//private:
/**
* Unit vector on the sphere, or vector pointing in a direction
*/
vect_type vec;
public:
S2() {
if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1));
if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0);
if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0);
}
S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) {
vec.normalize();
vec = vec * length;
}
S2(const vect_type &_vec) : vec(_vec) {
vec.normalize();
vec = vec * length;
}
void oplus(MTK::vectview<const scalar, 3> delta, scalar scale = 1)
{
// SO3_type res;
// res.w() = MTK::exp<scalar, 3>(res.vec(), delta, scalar(scale/2));
vec = SO3_type::exp(delta) * vec; // res.normalized().toRotationMatrix() * vec;
}
void boxplus(MTK::vectview<const scalar, 2> delta, scalar scale=1) {
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
vect_type Bu = Bx*delta;
// SO3_type res;
// res.w() = MTK::exp<scalar, 3>(res.vec(), Bu, scalar(scale/2));
vec = SO3_type::exp(delta) * vec; // res.normalized().toRotationMatrix() * vec;
}
void boxminus(MTK::vectview<scalar, 2> res, const S2<scalar, den, num, S2_typ>& other) const {
scalar v_sin = (MTK::hat(vec)*other.vec).norm();
scalar v_cos = vec.transpose() * other.vec;
scalar theta = std::atan2(v_sin, v_cos);
if(v_sin < MTK::tolerance<scalar>())
{
if(std::fabs(theta) > MTK::tolerance<scalar>() )
{
res[0] = 3.1415926;
res[1] = 0;
}
else{
res[0] = 0;
res[1] = 0;
}
}
else
{
S2<scalar, den, num, S2_typ> other_copy = other;
Eigen::Matrix<scalar, 3, 2>Bx;
other_copy.S2_Bx(Bx);
res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec;
}
}
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
Eigen::Matrix<scalar, 3, 3> skew_vec;
skew_vec << scalar(0), -vec[2], vec[1],
vec[2], scalar(0), -vec[0],
-vec[1], vec[0], scalar(0);
res = skew_vec;
}
void S2_Bx(Eigen::Matrix<scalar, 3, 2> &res)
{
if(S2_typ == 3)
{
if(vec[2] + length > tolerance<scalar>())
{
res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]),
-vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]),
-vec[0], -vec[1];
res /= length;
}
else
{
res = Eigen::Matrix<scalar, 3, 2>::Zero();
res(1, 1) = -1;
res(2, 0) = 1;
}
}
else if(S2_typ == 2)
{
if(vec[1] + length > tolerance<scalar>())
{
res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]),
-vec[0], -vec[2],
-vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]);
res /= length;
}
else
{
res = Eigen::Matrix<scalar, 3, 2>::Zero();
res(1, 1) = -1;
res(2, 0) = 1;
}
}
else
{
if(vec[0] + length > tolerance<scalar>())
{
res << -vec[1], -vec[2],
length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]),
-vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]);
res /= length;
}
else
{
res = Eigen::Matrix<scalar, 3, 2>::Zero();
res(1, 1) = -1;
res(2, 0) = 1;
}
}
}
void S2_Nx(Eigen::Matrix<scalar, 2, 3> &res, S2<scalar, den, num, S2_typ>& subtrahend)
{
if((vec+subtrahend.vec).norm() > tolerance<scalar>())
{
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
if((vec-subtrahend.vec).norm() > tolerance<scalar>())
{
scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm();
scalar v_cos = vec.transpose() * subtrahend.vec;
res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length));
}
else
{
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
}
}
else
{
std::cerr << "No N(x, y) for x=-y" << std::endl;
std::exit(100);
}
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
if(delta.norm() < tolerance<scalar>())
{
res = -MTK::hat(vec)*Bx;
}
else{
vect_type Bu = Bx*delta;
// SO3_type exp_delta;
// exp_delta.w() = MTK::exp<scalar, 3>(exp_delta.vec(), Bu, scalar(1/2));
res = -SO3_type::exp(Bu)*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; // exp_delta.normalized().toRotationMatrix()
}
}
operator const vect_type&() const{
return vec;
}
const vect_type& get_vect() const {
return vec;
}
friend S2<scalar, den, num, S2_typ> operator*(const SO3<scalar>& rot, const S2<scalar, den, num, S2_typ>& dir)
{
S2<scalar, den, num, S2_typ> ret;
ret.vec = rot * dir.vec;
return ret;
}
scalar operator[](int idx) const {return vec[idx]; }
friend std::ostream& operator<<(std::ostream &os, const S2<scalar, den, num, S2_typ>& vec){
return os << vec.vec.transpose() << " ";
}
friend std::istream& operator>>(std::istream &is, S2<scalar, den, num, S2_typ>& vec){
for(int i=0; i<3; ++i)
is >> vec.vec[i];
vec.vec.normalize();
vec.vec = vec.vec * vec.length;
return is;
}
};
} // namespace MTK
#endif /*S2_H_*/

334
include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp Executable file
View File

@@ -0,0 +1,334 @@
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/types/SEn.hpp
* @brief Standard Orthogonal Groups i.e.\ rotatation groups.
*/
#ifndef SEN_H_
#define SEN_H_
#include <Eigen/Geometry>
#include "SOn.hpp"
#include "vect.hpp"
#include "../src/mtkmath.hpp"
namespace MTK {
/**
* Three-dimensional orientations represented as Quaternion.
* It is assumed that the internal Quaternion always stays normalized,
* should this not be the case, call inherited member function @c normalize().
*/
template<class _scalar = double, int num_of_vec_plus1 = 6, int dim_of_mat = 4, int Options = Eigen::AutoAlign>
struct SEN {
enum {DOF = num_of_vec_plus1, DIM = num_of_vec_plus1, TYP = 4};
typedef _scalar scalar;
typedef Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> base;
typedef SO3<scalar> SO3_type;
// typedef Eigen::Quaternion<scalar, Options> base;
// typedef Eigen::Quaternion<scalar> Quaternion;
typedef vect<DIM, scalar, Options> vect_type;
SO3_type SO3_data;
base mat;
/**
* Construct from real part and three imaginary parts.
* Quaternion is normalized after construction.
*/
// SEN(const base& src) : mat(src) {
// // base::normalize();
// }
/**
* Construct from Eigen::Quaternion.
* @note Non-normalized input may result result in spurious behavior.
*/
SEN(const base& src = base::Identity()) : mat(src) {}
/**
* Construct from rotation matrix.
* @note Invalid rotation matrices may lead to spurious behavior.
*/
// template<class Derived>
// SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
/**
* Construct from arbitrary rotation type.
* @note Invalid rotation matrices may lead to spurious behavior.
*/
// template<class Derived>
// SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}
//! @name Manifold requirements
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
SEN delta = exp(vec, scale); // ?
mat = mat * delta.mat;
}
void boxminus(MTK::vectview<scalar, DOF> res, const SEN<scalar,num_of_vec_plus1,dim_of_mat, Options>& other) const {
base error_mat = other.mat.inverse() * mat;
res = log(error_mat);
}
//}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
SEN delta = exp(vec, scale);
mat = mat * delta.mat;
}
// void hat(MTK::vectview<const scalar, DOF>& v, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> &res) {
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
Eigen::Matrix<scalar, 3, 3> psi;
psi << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
res.block<3, 3>(0, 0) = psi;
for(int i = 3; i < v.size() / 3 + 2; i++)
{
res.block<3, 1>(0, i) = v.segment<3>(i + (i-3)*3);
}
// return res;
}
// void Jacob_right_inv(MTK::vectview<const scalar, DOF> vec, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> & res){
void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
Eigen::Matrix<scalar, 3, 3> M_v;
Eigen::VectorXd vec_psi, vec_ro;
Eigen::MatrixXd jac_v;
Eigen::MatrixXd hat_v, hat_ro;
vec_psi = vec.segment<3>(0);
// Eigen::Matrix<scalar, 3, 1> ;
SO3_data.hat(vec_psi, hat_v);
SO3_data.Jacob_right_inv(vec_psi, jac_v);
double norm = vec_psi.norm();
for(int i = 0; i < vec.size() / 3; i++)
{
res.block<3, 3>(i*3, i*3) = jac_v;
}
for(int i = 1; i < vec.size() / 3; i++)
{
vec_ro = vec.segment<3>(i * 3);
SO3_data.hat(vec_ro, hat_ro);
if(norm > MTK::tolerance<scalar>())
{
res.block<3,3>(i*3, 0) = 0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v;
}
else
{
res.block<3,3>(i*3, 0) = 0.5 * hat_ro;
}
}
// return res;
}
// void Jacob_right(MTK::vectview<const scalar, DOF> & vec, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> &res){
void Jacob_right(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
Eigen::MatrixXd hat_v, hat_ro;
Eigen::VectorXd vec_psi, vec_ro;
Eigen::MatrixXd jac_v;
vec_psi = vec.segment<3>(0);
// Eigen::Matrix<scalar, 3, 1> ;
SO3_data.hat(vec_psi, hat_v);
SO3_data.Jacob_right(vec_psi, jac_v);
// double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
// double norm = std::sqrt(squaredNorm);
double norm = vec_psi.norm();
for(int i = 0; i < vec.size() / 3; i++)
{
res.block<3, 3>(i*3, i*3) = jac_v;
}
for(int i = 1; i < vec.size() / 3; i++)
{
vec_ro = vec.segment<3>(i * 3);
SO3_data.hat(vec_ro, hat_ro);
if(norm > MTK::tolerance<scalar>())
{
res.block<3,3>(i*3, 0) = -1 * jac_v * (0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v) * jac_v;
}
else
{
res.block<3,3>(i*3, 0) = -0.5 * jac_v * hat_ro * jac_v;
}
}
// return res;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::ostream& operator<<(std::ostream &os, const SEN<scalar, DOF, dim_of_mat, Options>& q){
for(int i=0; i<dim_of_mat; i++)
{
for(int j = 0; j < dim_of_mat; j++)
{
os << q.mat(i, j) << " ";
}
}
return os;
}
friend std::istream& operator>>(std::istream &is, SEN<scalar, DOF, dim_of_mat, Options>& q){
// vect<dim_of_mat * dim_of_mat,scalar> coeffs;
for(int i=0; i<dim_of_mat; i++)
{
for(int j = 0; j < dim_of_mat; j++)
{
is >> q.mat(i, j);
}
}
// is >> q.mat;
// coeffs;
// q.coeffs() = coeffs.normalized();
return is;
}
//! @name Helper functions
//{
/**
* Calculate the exponential map. In matrix terms this would correspond
* to the Rodrigues formula.
*/
// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
// static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
static SEN exp(const Eigen::Matrix<scalar, DOF, 1>& dvec, scalar scale = 1){
SEN res;
res.mat = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Identity();
Eigen::Matrix<scalar, 3, 3> exp_; //, jac;
Eigen::MatrixXd jac;
Eigen::Matrix<scalar, 3, 1> psi;
Eigen::VectorXd minus_psi;
psi = dvec.template block<3,1>(0, 0);
minus_psi = -psi;
SO3_type SO3_temp;
exp_ = SO3_type::exp(psi);
SO3_temp.Jacob_right(minus_psi, jac);
res.mat.template block<3,3>(0, 0) = exp_;
for(int i = 3; i < DOF / 3 + 2; i++)
{
res.mat.template block<3, 1>(0, i) = jac * dvec.template block<3,1>(i + (i-3)*3,0);
}
return res;
}
/**
* Calculate the inverse of @c exp.
* Only guarantees that <code>exp(log(x)) == x </code>
*/
static Eigen::Matrix<scalar, DOF, 1> log(base &orient){
Eigen::Matrix<scalar, DOF, 1> res;
Eigen::Matrix<scalar, 3, 1> psi;
Eigen::VectorXd minus_psi;
Eigen::Matrix<scalar, 3, 3> mat_psi;
Eigen::MatrixXd jac;
mat_psi = orient.template block<3, 3>(0, 0);
SO3_type SO3_temp;
SO3_type exp_psi(mat_psi);
psi = SO3_type::log(exp_psi);
minus_psi = -psi;
SO3_temp.Jacob_right_inv(minus_psi, jac);
for(int i = 3; i < dim_of_mat; i++)
{
res.template block<3,1>(i + (i-3)*3,0) = jac * orient.template block<3, 1>(0, i);
}
return res;
}
};
} // namespace MTK
#endif /*SON_H_*/

361
include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp Executable file
View File

@@ -0,0 +1,361 @@
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/types/SOn.hpp
* @brief Standard Orthogonal Groups i.e.\ rotatation groups.
*/
#ifndef SON_H_
#define SON_H_
#include <Eigen/Geometry>
#include "vect.hpp"
#include "../src/mtkmath.hpp"
namespace MTK {
/**
* Two-dimensional orientations represented as scalar.
* There is no guarantee that the representing scalar is within any interval,
* but the result of boxminus will always have magnitude @f$\le\pi @f$.
*/
template<class _scalar = double, int Options = Eigen::AutoAlign>
struct SO2 : public Eigen::Rotation2D<_scalar> {
enum {DOF = 1, DIM = 2, TYP = 3};
typedef _scalar scalar;
typedef Eigen::Rotation2D<scalar> base;
typedef vect<DIM, scalar, Options> vect_type;
//! Construct from angle
SO2(const scalar& angle = 0) : base(angle) { }
//! Construct from Eigen::Rotation2D
SO2(const base& src) : base(src) {}
/**
* Construct from 2D vector.
* Resulting orientation will rotate the first unit vector to point to vec.
*/
SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};
//! Calculate @c this->inverse() * @c r
SO2 operator%(const base &r) const {
return base::inverse() * r;
}
//! Calculate @c this->inverse() * @c r
template<class Derived>
vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
return base::inverse() * vec;
}
//! Calculate @c *this * @c r.inverse()
SO2 operator/(const SO2 &r) const {
return *this * r.inverse();
}
//! Gets the angle as scalar.
operator scalar() const {
return base::angle();
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
//! @name Manifold requirements
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
base::angle() += scale * vec[0];
}
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
base::angle() += scale * vec[0];
}
void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& other) const {
res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));
}
friend std::istream& operator>>(std::istream &is, SO2<scalar>& ang){
return is >> ang.angle();
}
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
};
/**
* Three-dimensional orientations represented as Quaternion.
* It is assumed that the internal Quaternion always stays normalized,
* should this not be the case, call inherited member function @c normalize().
*/
template<class _scalar = double> //, int Options = Eigen::AutoAlign>
struct SO3 : public Eigen::Matrix<_scalar, 3, 3> {
enum {DOF = 3, DIM = 3, TYP = 2};
typedef _scalar scalar;
typedef Eigen::Matrix<scalar, 3, 3> base;
typedef Eigen::Matrix<scalar, 3, 3> Matrix;
typedef vect<DIM, scalar> vect_type;
/**
* Construct from Eigen::Quaternion.
* @note Non-normalized input may result result in spurious behavior.
*/
SO3(const base& src = base::Identity()) : base(src) {}
/**
* Construct from rotation matrix.
* @note Invalid rotation matrices may lead to spurious behavior.
*/
template<class Derived>
SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
/**
* Construct from arbitrary rotation type.
* @note Invalid rotation matrices may lead to spurious behavior.
*/
// template<class Derived>
// SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}
//! @name Manifold requirements
// SO3 operator=(const base &r) const {
// return r;
// }
// //! Calculate @c *this * @c r.inverse()
// SO3 operator*(const SO3 &r) const {
// return *this * r;
// }
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
SO3 delta = exp(vec, scale);
*this = *this * delta;
}
void boxminus(MTK::vectview<scalar, DOF> res, const SO3<scalar>& other) const {
res = SO3::log(other.transpose() * *this);
}
//}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
SO3 delta = exp(vec, scale);
*this = *this * delta;
}
// void hat(MTK::vectview<const scalar, DOF>& v, Eigen::Matrix<scalar, 3, 3> &res) {
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
// Eigen::Matrix<scalar, 3, 3> res;
res << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
// return res;
}
// void Jacob_right_inv(MTK::vectview<const scalar, DOF> vec, Eigen::Matrix<scalar, 3, 3> & res){
void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
Eigen::MatrixXd hat_v;
hat(vec, hat_v);
if(vec.norm() > MTK::tolerance<scalar>())
{
res = Eigen::Matrix<scalar, 3, 3>::Identity() + 0.5 * hat_v + (1 - vec.norm() * std::cos(vec.norm() / 2) / 2 / std::sin(vec.norm() / 2)) * hat_v * hat_v / vec.squaredNorm();
}
else
{
res = Eigen::Matrix<scalar, 3, 3>::Identity();
}
// return res;
}
// void Jacob_right(MTK::vectview<const scalar, DOF> & v, Eigen::Matrix<scalar, 3, 3> &res){
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res){
Eigen::MatrixXd hat_v;
hat(v, hat_v);
double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
double norm = std::sqrt(squaredNorm);
if(norm < MTK::tolerance<scalar>()){
res = Eigen::Matrix<scalar, 3, 3>::Identity();
}
else{
res = Eigen::Matrix<scalar, 3, 3>::Identity() - (1 - std::cos(norm)) / squaredNorm * hat_v + (1 - std::sin(norm) / norm) / squaredNorm * hat_v * hat_v;
}
// return res;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::ostream& operator<<(std::ostream &os, const SO3<scalar>& q){ // wrong!
return os << q.data() << " ";
}
friend std::istream& operator>>(std::istream &is, SO3<scalar>& q){ // wrong!
SO3<scalar> coeffs;
is >> coeffs;
q = coeffs;
return is;
}
//! @name Helper functions
//{
/**
* Calculate the exponential map. In matrix terms this would correspond
* to the Rodrigues formula.
*/
// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
// static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
static SO3 exp(const Eigen::Matrix<scalar, 3, 1>& dvec, scalar scale = 1){
Eigen::Matrix<scalar, 3, 1> ang = dvec * scale;
double ang_norm = ang.norm();
Eigen::Matrix<double, 3, 3> Eye3 = Eigen::Matrix<double, 3, 3>::Identity();
if (ang_norm > 0.0000001)
{
Eigen::Matrix<double, 3, 1> r_axis = ang / ang_norm;
Eigen::Matrix<double, 3, 3> K;
K << 0.0, -r_axis(2), r_axis(1),
r_axis(2), 0.0, -r_axis(0),
-r_axis(1), r_axis(0), 0.0; // SKEW_SYM_MATRX(r_axis);
/// Roderigous Tranformation
return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
}
else
{
return Eye3;
}
}
/**
* Calculate the inverse of @c exp.
* Only guarantees that <code>exp(log(x)) == x </code>
*/
static Eigen::Vector3d log(const SO3 &orient){
double theta = (orient.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (orient.trace() - 1));
Eigen::Matrix<double,3,1> K(orient(2,1) - orient(1,2), orient(0,2) - orient(2,0), orient(1,0) - orient(0,1));
return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
}
};
namespace internal {
template<class Scalar, int Options>
struct UnalignedType<SO2<Scalar, Options > >{
typedef SO2<Scalar, Options | Eigen::DontAlign> type;
};
template<class Scalar>
struct UnalignedType<SO3<Scalar> >{
typedef SO3<Scalar> type;
};
} // namespace internal
} // namespace MTK
#endif /*SON_H_*/

511
include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp Executable file
View File

@@ -0,0 +1,511 @@
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file mtk/types/vect.hpp
* @brief Basic vectors interpreted as manifolds.
*
* This file also implements a simple wrapper for matrices, for arbitrary scalars
* and for positive scalars.
*/
#ifndef VECT_H_
#define VECT_H_
#include <iosfwd>
#include <iostream>
#include <vector>
#include "../src/vectview.hpp"
namespace MTK {
static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]");
/**
* A simple vector class.
* Implementation is basically a wrapper around Eigen::Matrix with manifold
* requirements added.
*/
template<int D = 3, class _scalar = double, int _Options=Eigen::AutoAlign>
struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> {
typedef Eigen::Matrix<_scalar, D, 1, _Options> base;
enum {DOF = D, DIM = D, TYP = 0};
typedef _scalar scalar;
//using base::operator=;
/** Standard constructor. Sets all values to zero. */
vect(const base &src = base::Zero()) : base(src) {}
/** Constructor copying the value of the expression \a other */
template<typename OtherDerived>
EIGEN_STRONG_INLINE vect(const Eigen::DenseBase<OtherDerived>& other) : base(other) {}
/** Construct from memory. */
vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { }
void boxplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
*this += scale * vec;
}
void boxminus(MTK::vectview<scalar, D> res, const vect<D, scalar>& other) const {
res = *this - other;
}
void oplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
*this += scale * vec;
}
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::ostream& operator<<(std::ostream &os, const vect<D, scalar, _Options>& v){
// Eigen sometimes messes with the streams flags, so output manually:
for(int i=0; i<DOF; ++i)
os << v(i) << " ";
return os;
}
friend std::istream& operator>>(std::istream &is, vect<D, scalar, _Options>& v){
char term=0;
is >> std::ws; // skip whitespace
switch(is.peek()) {
case '(': term=')'; is.ignore(1); break;
case '[': term=']'; is.ignore(1); break;
case '{': term='}'; is.ignore(1); break;
default: break;
}
if(D==Eigen::Dynamic) {
assert(term !=0 && "Dynamic vectors must be embraced");
std::vector<scalar> temp;
while(is.good() && is.peek() != term) {
scalar x;
is >> x;
temp.push_back(x);
if(is.peek()==',') is.ignore(1);
}
v = vect::Map(temp.data(), temp.size());
} else
for(int i=0; i<v.size(); ++i){
is >> v[i];
if(is.peek()==',') { // ignore commas between values
is.ignore(1);
}
}
if(term!=0) {
char x;
is >> x;
if(x!=term) {
is.setstate(is.badbit);
// assert(x==term && "start and end bracket do not match!");
}
}
return is;
}
template<int dim>
vectview<scalar, dim> tail(){
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template tail<dim>();
}
template<int dim>
vectview<const scalar, dim> tail() const{
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template tail<dim>();
}
template<int dim>
vectview<scalar, dim> head(){
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template head<dim>();
}
template<int dim>
vectview<const scalar, dim> head() const{
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template head<dim>();
}
};
/**
* A simple matrix class.
* Implementation is basically a wrapper around Eigen::Matrix with manifold
* requirements added, i.e., matrix is viewed as a plain vector for that.
*/
template<int M, int N, class _scalar = double, int _Options = Eigen::Matrix<_scalar, M, N>::Options>
struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> {
typedef Eigen::Matrix<_scalar, M, N, _Options> base;
enum {DOF = M * N, TYP = 4, DIM=0};
typedef _scalar scalar;
using base::operator=;
/** Standard constructor. Sets all values to zero. */
matrix() {
base::setZero();
}
/** Constructor copying the value of the expression \a other */
template<typename OtherDerived>
EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase<OtherDerived>& other) : base(other) {}
/** Construct from memory. */
matrix(const scalar* src) : base(src) { }
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
*this += scale * base::Map(vec.data());
}
void boxminus(MTK::vectview<scalar, DOF> res, const matrix& other) const {
base::Map(res.data()) = *this - other;
}
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
*this += scale * base::Map(vec.data());
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::ostream& operator<<(std::ostream &os, const matrix<M, N, scalar, _Options>& mat){
for(int i=0; i<DOF; ++i){
os << mat.data()[i] << " ";
}
return os;
}
friend std::istream& operator>>(std::istream &is, matrix<M, N, scalar, _Options>& mat){
for(int i=0; i<DOF; ++i){
is >> mat.data()[i];
}
return is;
}
};// @todo What if M / N = Eigen::Dynamic?
/**
* A simple scalar type.
*/
template<class _scalar = double>
struct Scalar {
enum {DOF = 1, TYP = 5, DIM=0};
typedef _scalar scalar;
scalar value;
Scalar(const scalar& value = scalar(0)) : value(value) {}
operator const scalar&() const { return value; }
operator scalar&() { return value; }
Scalar& operator=(const scalar& val) { value = val; return *this; }
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
value += scale * vec[0];
}
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
value += scale * vec[0];
}
void boxminus(MTK::vectview<scalar, DOF> res, const Scalar& other) const {
res[0] = *this - other;
}
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
};
/**
* Positive scalars.
* Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$.
*/
template<class _scalar = double>
struct PositiveScalar {
enum {DOF = 1, TYP = 6, DIM=0};
typedef _scalar scalar;
scalar value;
PositiveScalar(const scalar& value = scalar(1)) : value(value) {
assert(value > scalar(0));
}
operator const scalar&() const { return value; }
PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; }
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
value *= std::exp(scale * vec[0]);
}
void boxminus(MTK::vectview<scalar, DOF> res, const PositiveScalar& other) const {
res[0] = std::log(*this / other);
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
value *= std::exp(scale * vec[0]);
}
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::istream& operator>>(std::istream &is, PositiveScalar<scalar>& s){
is >> s.value;
assert(s.value > 0);
return is;
}
};
template<class _scalar = double>
struct Complex : public std::complex<_scalar>{
enum {DOF = 2, TYP = 7, DIM=0};
typedef _scalar scalar;
typedef std::complex<scalar> Base;
Complex(const Base& value) : Base(value) {}
Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {}
Complex(const MTK::vectview<const scalar, 2> &in) : Base(in[0], in[1]) {}
template<class Derived>
Complex(const Eigen::DenseBase<Derived> &in) : Base(in[0], in[1]) {}
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
Base::real() += scale * vec[0];
Base::imag() += scale * vec[1];
};
void boxminus(MTK::vectview<scalar, DOF> res, const Complex& other) const {
Complex diff = *this - other;
res << diff.real(), diff.imag();
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
std::cout << "wrong idx" << std::endl;
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
Base::real() += scale * vec[0];
Base::imag() += scale * vec[1];
};
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
scalar squaredNorm() const {
return std::pow(Base::real(),2) + std::pow(Base::imag(),2);
}
const scalar& operator()(int i) const {
assert(0<=i && i<2 && "Index out of range");
return i==0 ? Base::real() : Base::imag();
}
scalar& operator()(int i){
assert(0<=i && i<2 && "Index out of range");
return i==0 ? Base::real() : Base::imag();
}
};
namespace internal {
template<int dim, class Scalar, int Options>
struct UnalignedType<vect<dim, Scalar, Options > >{
typedef vect<dim, Scalar, Options | Eigen::DontAlign> type;
};
} // namespace internal
} // namespace MTK
#endif /*VECT_H_*/

113
include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp Executable file
View File

@@ -0,0 +1,113 @@
/*
* Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH
* All rights reserved.
*
* Author: Rene Wagner <rene.wagner@dfki.de>
* Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the DFKI GmbH
* nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific
* prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef WRAPPED_CV_MAT_HPP_
#define WRAPPED_CV_MAT_HPP_
#include <Eigen/Core>
#include <opencv/cv.h>
namespace MTK {
template<class f_type>
struct cv_f_type;
template<>
struct cv_f_type<double>
{
enum {value = CV_64F};
};
template<>
struct cv_f_type<float>
{
enum {value = CV_32F};
};
/**
* cv_mat wraps a CvMat around an Eigen Matrix
*/
template<int rows, int cols, class f_type = double>
class cv_mat : public matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor>
{
typedef matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor> base_type;
enum {type_ = cv_f_type<f_type>::value};
CvMat cv_mat_;
public:
cv_mat()
{
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
}
cv_mat(const cv_mat& oth) : base_type(oth)
{
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
}
template<class Derived>
cv_mat(const Eigen::MatrixBase<Derived> &value) : base_type(value)
{
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
}
template<class Derived>
cv_mat& operator=(const Eigen::MatrixBase<Derived> &value)
{
base_type::operator=(value);
return *this;
}
cv_mat& operator=(const cv_mat& value)
{
base_type::operator=(value);
return *this;
}
// FIXME: Maybe overloading operator& is not a good idea ...
CvMat* operator&()
{
return &cv_mat_;
}
const CvMat* operator&() const
{
return &cv_mat_;
}
};
} // namespace MTK
#endif /* WRAPPED_CV_MAT_HPP_ */

339
include/IKFoM/LICENSE Normal file
View File

@@ -0,0 +1,339 @@
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.

489
include/IKFoM/README.md Normal file
View File

@@ -0,0 +1,489 @@
## IKFoM
**IKFoM** (Iterated Kalman Filters on Manifolds) is a computationally efficient and convenient toolkit for deploying iterated Kalman filters on various robotic systems, especially systems operating on high-dimension manifold. It implements a manifold-embedding Kalman filter which separates the manifold structures from system descriptions and is able to be used by only defining the system in a canonical form and calling the respective steps accordingly. The current implementation supports the full iterated Kalman filtering for systems on manifold <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbb{R}^m\times&space;SO(3)\times\cdots\times&space;SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbb{R}^m\times&space;SO(3)\times\cdots\times&space;SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" title="\mathbb{R}^m\times SO(3)\times\cdots\times SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" /></a> and any of its sub-manifolds, and it is extendable to other types of manifold when necessary.
**Developers**
[Dongjiao He](https://github.com/Joanna-HE)
**Our related video**: https://youtu.be/sz_ZlDkl6fA
## 1. Prerequisites
### 1.1. **Eigen && Boost**
Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).
Boost >= 1.65.
## 2. Usage when the measurement is of constant dimension and type.
Clone the repository:
```
git clone https://github.com/hku-mars/IKFoM.git
```
1. include the necessary head file:
```
#include<esekfom/esekfom.hpp>
```
2. Select and instantiate the primitive manifolds:
```
typedef MTK::SO3<double> SO3; // scalar type of variable: double
typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
```
3. Build system state, input and measurement as compound manifolds which are composed of the primitive manifolds:
```
MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
((vect3, pos)) // ((primitive manifold type, name of variable))
((vect3, vel))
((SO3, rot))
((vect3, bg))
((vect3, ba))
((S2, grav))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
);
```
4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k&plus;1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k&plus;1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"><see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
```
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
res(0) = s.vel[0];
res(1) = s.vel[1];
res(2) = s.vel[2];
return res;
}
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
return cov;
}
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
return cov;
}
```
Those functions would be called during the ekf state predict
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
```
measurement h(state &s, bool &valid) // the iteration stops before convergence whenever the user set valid as false
{
if (condition){ valid = false;
} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
measurement h_;
h_.position = s.pos;
return h_;
}
Eigen::Matrix<double, measurement_dof, state_dof> dh_dx(state &s) {}
Eigen::Matrix<double, measurement_dof, measurement_noise_dof> dh_dv(state &s) {}
```
Those functions would be called during the ekf state update
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
(1) initial state and covariance:
```
state init_state;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);
```
(2) default state and covariance:
```
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;
```
where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**.
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
```
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated(z, R); // measurement noise covariance: R, an Eigen matrix
```
*Remarks(1):*
- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
```
measurement h_share(state &s, esekfom::share_datastruct<state, measurement, measurement_noise_dof> &share_data)
{
if(share_data.converge) {} // this value is true means iteration is converged
if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
share_data.R = R; // R is the measurement noise covariance
share_data.z = z; // z is the obtained measurement
measurement h_;
h_.position = s.pos;
return h_;
}
```
This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
(1) initial state and covariance:
```
state init_state;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);
```
(2) default state and covariance:
```
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;
```
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
```
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_share(f, df_dx, df_dw, h_share, Maximum_iter, epsi);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated_share();
```
*Remarks(2):*
- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
```
state set_x;
kf.change_x(set_x);
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov set_P;
kf.change_P(set_P);
```
## 3. Usage when the measurement is an Eigen vector of changing dimension.
Clone the repository:
```
git clone https://github.com/hku-mars/IKFoM.git
```
1. include the necessary head file:
```
#include<esekfom/esekfom.hpp>
```
2. Select and instantiate the primitive manifolds:
```
typedef MTK::SO3<double> SO3; // scalar type of variable: double
typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
```
3. Build system state and input as compound manifolds which are composed of the primitive manifolds:
```
MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
((vect3, pos)) // ((primitive manifold type, name of variable))
((vect3, vel))
((SO3, rot))
((vect3, bg))
((vect3, ba))
((S2, grav))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
);
```
4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k&plus;1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k&plus;1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"> <see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
```
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
res(0) = s.vel[0];
res(1) = s.vel[1];
res(2) = s.vel[2];
return res;
}
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
return cov;
}
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
return cov;
}
```
Those functions would be called during ekf state predict
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
```
Eigen::Matrix<double, Eigen::Dynamic, 1> h(state &s, bool &valid) //the iteration stops before convergence when valid is false {
if (condition){ valid = false;
} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
h_(0) = s.pos[0];
return h_;
}
Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s) {}
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s) {}
```
Those functions would be called during ekf state update
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
(1) initial state and covariance:
```
state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
```
(2) default state and covariance:
```
esekfom::esekf<state, process_noise_dof, input> kf;
```
where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
```
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated_dyn(z, R); // measurement noise covariance: R, an Eigen matrix
```
*Remarks(1):*
- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
```
Eigen::Matrix<double, Eigen::Dynamic, 1> h_dyn_share(state &s, esekfom::dyn_share_datastruct<double> &dyn_share_data)
{
if(dyn_share_data.converge) {} // this value is true means iteration is converged
if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
dyn_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
dyn_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
dyn_share_data.R = R; // R is the measurement noise covariance
dyn_share_data.z = z; // z is the obtained measurement
Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
h_(0) = s.pos[0];
return h_;
}
This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
```
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
(1) initial state and covariance:
```
state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
```
(2) default state and covariance:
```
esekfom::esekf<state, process_noise_dof, input> kf;
```
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
```
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn_share(f, df_dx, df_dw, h_dyn_share, Maximum_iter, epsi);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
```
9. Once a measurement **z** is received, an iterated update is executed:
```
kf.update_iterated_dyn_share();
```
*Remarks(2):*
- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
```
state set_x;
kf.change_x(set_x);
esekfom::esekf<state, process_noise_dof, input>::cov set_P;
kf.change_P(set_P);
```
## 4. Usage when the measurement is a changing manifold during the run time.
Clone the repository:
```
git clone https://github.com/hku-mars/IKFoM.git
```
1. include the necessary head file:
```
#include<esekfom/esekfom.hpp>
```
2. Select and instantiate the primitive manifolds:
```
typedef MTK::SO3<double> SO3; // scalar type of variable: double
typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
```
3. Build system state and input as compound manifolds which are composed of the primitive manifolds:
```
MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
((vect3, pos)) // ((primitive manifold type, name of variable))
((vect3, vel))
((SO3, rot))
((vect3, bg))
((vect3, ba))
((S2, grav))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
);
```
4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k&plus;1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k&plus;1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"> <see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
```
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
res(0) = s.vel[0];
res(1) = s.vel[1];
res(2) = s.vel[2];
return res;
}
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
return cov;
}
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
return cov;
}
```
Those functions would be called during ekf state predict
5. Implement the differentiation of the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
```
Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s, bool &valid) {}
```
Those functions would be called during ekf state update
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
(1) initial state and covariance:
```
state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
```
(2)
```
esekfom::esekf<state, process_noise_dof, input> kf;
```
Where **process_noise_dof** is the dimension of process noise, of type of std int
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
```
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn_runtime(f, df_dx, df_dw, dh_dx, dh_dv, Maximum_iter, epsi);
```
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q
```
9. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> :
```
measurement h(state &s, bool &valid) //the iteration stops before convergence when valid is false
{
if (condition) valid = false; // the update iteration could be stopped when the condition other than convergence is satisfied
measurement h_;
h_.pos = s.pos;
return h_;
}
```
then an iterated update is executed:
```
kf.update_iterated_dyn_runtime(z, R, h); // measurement noise covariance: R, an Eigen matrix
```
*Remarks(1):*
- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
5. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
(1) initial state and covariance:
```
state init_state;
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
```
(2) default state and covariance:
```
esekfom::esekf<state, process_noise_dof, input> kf;
```
6. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
```
double epsi[state_dof] = {0.001};
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
kf.init_dyn_runtime_share(f, df_dx, df_dw, Maximum_iter, epsi);
```
7. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
```
kf.predict(dt, Q, in); // process noise covariance: Q. an Eigen matrix
```
8. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
```
measurement h_dyn_runtime_share(state &s, esekfom::dyn_runtime_share_datastruct<double> &dyn_runtime_share_data)
{
if(dyn_runtime_share_data.converge) {} // this value is true means iteration is converged
if(condition) dyn_runtime_share_data.valid = false; // the iteration stops before convergence when this value is false, if conditions other than convergence is satisfied
dyn_runtime_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
dyn_runtime_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
dyn_runtime_share_data.R = R; // R is the measurement noise covariance
measurement h_;
h_.pos = s.pos;
return h_;
}
```
This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
then an iterated update is executed:
```
kf.update_iterated_dyn_runtime_share(z, h_dyn_runtime_share);
```
*Remarks(2):*
- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
```
state set_x;
kf.change_x(set_x);
esekfom::esekf<state, process_noise_dof, input>::cov set_P;
kf.change_P(set_P);
```
## 5. Run the sample
Clone the repository:
```
git clone https://github.com/hku-mars/IKFoM.git
```
In the **Samples** file folder, there is the scource code that applys the **IKFoM** on the original source code from [FAST LIO](https://github.com/hku-mars/FAST_LIO). Please follow the README.md shown in that repository excepting the step **2. Build**, which is modified as:
```
cd ~/catkin_ws/src
cp -r ~/IKFoM/Samples/FAST_LIO-stable FAST_LIO-stable
cd ..
catkin_make
source devel/setup.bash
```
## 6.Acknowledgments
Thanks for C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.

239
include/common_lib.h Executable file
View File

@@ -0,0 +1,239 @@
#ifndef COMMON_LIB_H
#define COMMON_LIB_H
#include <so3_math.h>
#include <Eigen/Eigen>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp>
#include <queue>
using namespace std;
using namespace Eigen;
typedef MTK::vect<3, double> vect3;
typedef MTK::SO3<double> SO3;
typedef MTK::S2<double, 98090, 10000, 1> S2;
typedef MTK::vect<1, double> vect1;
typedef MTK::vect<2, double> vect2;
MTK_BUILD_MANIFOLD(state_input,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((vect3, gravity))
);
MTK_BUILD_MANIFOLD(state_output,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, omg))
((vect3, acc))
((vect3, gravity))
((vect3, bg))
((vect3, ba))
);
MTK_BUILD_MANIFOLD(input_ikfom,
((vect3, acc))
((vect3, gyro))
);
MTK_BUILD_MANIFOLD(process_noise_input,
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);
MTK_BUILD_MANIFOLD(process_noise_output,
((vect3, vel))
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);
extern esekfom::esekf<state_input, 24, input_ikfom> kf_input;
extern esekfom::esekf<state_output, 30, input_ikfom> kf_output;
#define PBWIDTH 30
#define PBSTR "||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||"
#define PI_M (3.14159265358)
#define G_m_s2 (9.81) // Gravaty const in GuangDong/China
#define DIM_STATE (24) // Dimension of states (Let Dim(SO(3)) = 3)
#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3)
#define CUBE_LEN (6.0)
#define LIDAR_SP_LEN (2)
#define INIT_COV (0.0001)
#define NUM_MATCH_POINTS (5)
#define MAX_MEAS_DIM (10000)
#define VEC_FROM_ARRAY(v) v[0],v[1],v[2]
#define VEC_FROM_ARRAY_SIX(v) v[0],v[1],v[2],v[3],v[4],v[5]
#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]
#define CONSTRAIN(v,min,max) ((v>min)?((v<max)?v:max):min)
#define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols()
#define STD_VEC_FROM_EIGEN(mat) vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())
#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name))
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointXYZRGB PointTypeRGB;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
typedef pcl::PointCloud<PointTypeRGB> PointCloudXYZRGB;
typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
typedef Vector3d V3D;
typedef Matrix3d M3D;
typedef Vector3f V3F;
typedef Matrix3f M3F;
#define MD(a,b) Matrix<double, (a), (b)>
#define VD(a) Matrix<double, (a), 1>
#define MF(a,b) Matrix<float, (a), (b)>
#define VF(a) Matrix<float, (a), 1>
const M3D Eye3d(M3D::Identity());
const M3F Eye3f(M3F::Identity());
const V3D Zero3d(0, 0, 0);
const V3F Zero3f(0, 0, 0);
struct MeasureGroup // Lidar data and imu dates for the curent process
{
MeasureGroup()
{
lidar_beg_time = 0.0;
lidar_last_time = 0.0;
this->lidar.reset(new PointCloudXYZI());
};
double lidar_beg_time;
double lidar_last_time;
PointCloudXYZI::Ptr lidar;
deque<sensor_msgs::Imu::ConstPtr> imu;
};
template <typename T>
T calc_dist(PointType p1, PointType p2){
T d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
return d;
}
template <typename T>
T calc_dist(Eigen::Vector3d p1, PointType p2){
T d = (p1(0) - p2.x) * (p1(0) - p2.x) + (p1(1) - p2.y) * (p1(1) - p2.y) + (p1(2) - p2.z) * (p1(2) - p2.z);
return d;
}
template<typename T>
std::vector<int> time_compressing(const PointCloudXYZI::Ptr &point_cloud)
{
int points_size = point_cloud->points.size();
int j = 0;
std::vector<int> time_seq;
// time_seq.clear();
time_seq.reserve(points_size);
for(int i = 0; i < points_size - 1; i++)
{
j++;
if (point_cloud->points[i+1].curvature > point_cloud->points[i].curvature)
{
time_seq.emplace_back(j);
j = 0;
}
}
// if (j == 0)
// {
// time_seq.emplace_back(1);
// }
// else
{
time_seq.emplace_back(j+1);
}
return time_seq;
}
/* comment
plane equation: Ax + By + Cz + D = 0
convert to: A/D*x + B/D*y + C/D*z = -1
solve: A0*x0 = b0
where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T
normvec: normalized x0
*/
template<typename T>
bool esti_normvector(Matrix<T, 3, 1> &normvec, const PointVector &point, const T &threshold, const int &point_num)
{
MatrixXf A(point_num, 3);
MatrixXf b(point_num, 1);
b.setOnes();
b *= -1.0f;
for (int j = 0; j < point_num; j++)
{
A(j,0) = point[j].x;
A(j,1) = point[j].y;
A(j,2) = point[j].z;
}
normvec = A.colPivHouseholderQr().solve(b);
for (int j = 0; j < point_num; j++)
{
if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
{
return false;
}
}
normvec.normalize();
return true;
}
template<typename T>
bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
{
Matrix<T, NUM_MATCH_POINTS, 3> A;
Matrix<T, NUM_MATCH_POINTS, 1> b;
A.setZero();
b.setOnes();
b *= -1.0f;
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
A(j,0) = point[j].x;
A(j,1) = point[j].y;
A(j,2) = point[j].z;
}
Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);
T n = normvec.norm();
pca_result(0) = normvec(0) / n;
pca_result(1) = normvec(1) / n;
pca_result(2) = normvec(2) / n;
pca_result(3) = 1.0 / n;
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
{
return false;
}
}
return true;
}
// const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);};
// template<typename T>
// const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
#endif

84
include/ivox/eigen_types.h Normal file
View File

@@ -0,0 +1,84 @@
//
// Created by xiang on 2021/7/16.
//
#ifndef FASTER_LIO_EIGEN_TYPES_H
#define FASTER_LIO_EIGEN_TYPES_H
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
/// alias for eigen
using Vec2i = Eigen::Vector2i;
using Vec3i = Eigen::Vector3i;
using Vec2d = Eigen::Vector2d;
using Vec2f = Eigen::Vector2f;
using Vec3d = Eigen::Vector3d;
using Vec3f = Eigen::Vector3f;
using Vec5d = Eigen::Matrix<double, 5, 1>;
using Vec5f = Eigen::Matrix<float, 5, 1>;
using Vec6d = Eigen::Matrix<double, 6, 1>;
using Vec6f = Eigen::Matrix<float, 6, 1>;
using Vec15d = Eigen::Matrix<double, 15, 15>;
using Mat1d = Eigen::Matrix<double, 1, 1>;
using Mat3d = Eigen::Matrix3d;
using Mat3f = Eigen::Matrix3f;
using Mat4d = Eigen::Matrix4d;
using Mat4f = Eigen::Matrix4f;
using Mat5d = Eigen::Matrix<double, 5, 5>;
using Mat5f = Eigen::Matrix<float, 5, 5>;
using Mat6d = Eigen::Matrix<double, 6, 6>;
using Mat6f = Eigen::Matrix<float, 6, 6>;
using Mat15d = Eigen::Matrix<double, 15, 15>;
using Quatd = Eigen::Quaterniond;
using Quatf = Eigen::Quaternionf;
namespace faster_lio {
/// less of vector
template <int N>
struct less_vec {
inline bool operator()(const Eigen::Matrix<int, N, 1>& v1, const Eigen::Matrix<int, N, 1>& v2) const;
};
/// hash of vector
template <int N>
struct hash_vec {
inline size_t operator()(const Eigen::Matrix<int, N, 1>& v) const;
};
/// implementation
template <>
inline bool less_vec<2>::operator()(const Eigen::Matrix<int, 2, 1>& v1, const Eigen::Matrix<int, 2, 1>& v2) const {
return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]);
}
template <>
inline bool less_vec<3>::operator()(const Eigen::Matrix<int, 3, 1>& v1, const Eigen::Matrix<int, 3, 1>& v2) const {
return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]) && (v1[0] == v2[0] && v1[1] == v2[1] && v1[2] < v2[2]);
}
/// vec 2 hash
/// @see Optimized Spatial Hashing for Collision Detection of Deformable Objects, Matthias Teschner et. al., VMV 2003
template <>
inline size_t hash_vec<2>::operator()(const Eigen::Matrix<int, 2, 1>& v) const {
return size_t(((v[0]) * 73856093) ^ ((v[1]) * 471943)) % 10000000;
}
/// vec 3 hash
template <>
inline size_t hash_vec<3>::operator()(const Eigen::Matrix<int, 3, 1>& v) const {
return size_t(((v[0]) * 73856093) ^ ((v[1]) * 471943) ^ ((v[2]) * 83492791)) % 10000000;
}
// constexpr auto less_vec2i = [](const Vec2i& v1, const Vec2i& v2) {
// return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]);
// };
} // namespace faster_lio
#endif

830
include/ivox/hilbert.hpp Normal file
View File

@@ -0,0 +1,830 @@
// 2021-09-18, https://github.com/spectral3d/hilbert_hpp is under MIT license.
//Copyright (c) 2019 David Beynon
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files (the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions:
//
//The above copyright notice and this permission notice shall be included in all
//copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.
#ifndef INCLUDED_HILBERT_HPP
#define INCLUDED_HILBERT_HPP
#pragma once
#include <algorithm>
#include <array>
#include <cstdint>
#include <limits>
#include <type_traits>
// Version 1.1 - 29 August 2019
//
// N dimensional hilbert curve encoding & decoding based on the paper
// "Programming the Hilbert Curve" by John Skilling.
//
// The interface assumes an std::array of some unsigned integer type. This
// contains an index in lexographic order, or a set of coordinates on the
// hilbert curve.
//
// Two implementations are included.
//
// hilbert::v1 contains a fairly straightforward implementation of the paper,
// with standard looping constructs.
//
// hilbert::v2 performs uses template metaprogramming to unroll loops and
// theoretically improve performance on some systems.
//
// v1 produces smaller code, which may be more performant on machines with
// small caches. v2 produces relatively large code that seems more efficient
// on modern systems for dimension up to about 100.
//
// v2 should be built with -O3 for best results. -O0 is extremely slow
// on most systems.
//
// Interface is as follows:
//
// Find the position of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
//template<typename T, size_t N>
//std::array<T, N>
//IndexToPosition(std::array<T, N> const &in);
// Find the index of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
//template<typename T, size_t N>
//std::array<T, N>
//PositionToIndex(std::array<T, N> const &in);
//
namespace hilbert
{
// Fairly straightforward implementation. Loops are loops and code mostly
// does what one would expect.
namespace v1
{
namespace internal
{
// Extract bits from transposed form.
//
// e.g.
//
// a d g j a b c d
// b e h k -> e f g h
// c f i l i j k l
//
template<typename T, size_t N>
std::array<T, N>
UntransposeBits(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
const T high_bit(T(1) << (bits - 1));
const size_t bit_count(bits * N);
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
// go through all bits in input, msb first. Shift distances are
// from msb.
for(size_t b=0;b<bit_count;b++)
{
size_t src_bit, dst_bit, src, dst;
src = b % N;
dst = b / bits;
src_bit = b / N;
dst_bit = b % bits;
out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit);
}
return out;
}
// Pack bits into transposed form.
//
// e.g.
//
// a b c d a d g j
// e f g h -> b e h k
// i j k l c f i l
//
template<typename T, size_t N>
std::array<T, N>
TransposeBits(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
const T high_bit(T(1) << (bits - 1));
const size_t bit_count(bits * N);
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
// go through all bits in input, msb first. Shift distances
// are from msb.
for(size_t b=0;b<bit_count;b++)
{
size_t src_bit, dst_bit, src, dst;
src = b / bits;
dst = b % N;
src_bit = b % bits;
dst_bit = b / N;
out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit);
}
return out;
}
}
//
// Public interfaces.
//
// Find the position of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
IndexToPosition(
std::array<T, N> const &in)
{
// First convert index to transpose.
std::array<T, N> out(internal::TransposeBits(in));
// Initial gray encoding of transposed vector.
{
T tmp = out[N-1] >> 1;
for(size_t n=N-1;n;n--)
{
out[n]^= out[n-1];
}
out[0]^= tmp;
}
// Apply transforms to gray code.
{
T cur_bit(2),
low_bits;
while(cur_bit)
{
low_bits = cur_bit - 1;
size_t n(N);
do
{
n--;
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
}
while(n);
cur_bit<<= 1;
}
}
return out;
}
// Find the index of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
PositionToIndex(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
std::array<T, N> out(in);
// reverse transforms to convert into transposed gray code.
{
T cur_bit(T(1) << (bits - 1)),
low_bits;
do
{
low_bits = cur_bit - 1;
for(size_t n=0;n<N;n++)
{
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
}
cur_bit>>= 1;
} while(low_bits > 1);
}
// Remove gray code from transposed vector.
{
T cur_bit(T(1) << (bits - 1)),
t(0);
for(size_t n=1;n<N;n++)
{
out[n]^= out[n-1];
}
do
{
if(out[N-1] & cur_bit)
{
t^= (cur_bit - 1);
}
cur_bit>>= 1;
} while(cur_bit > 1);
for(auto &v : out)
{
v^= t;
}
}
return internal::UntransposeBits(out);
}
} // namespace v1
// Implementation using metaprogramming to unroll most loops.
// Optimised performance should be superior to v1 provided all code remains
// in cache etc.
//
// At some value of N v1 should overtake v2.
namespace v2
{
namespace internal
{
// Metaprogramming guts. Unrolled loops, abandon all hope etc.
namespace tmp
{
template<typename T, size_t N, size_t D>
T
TransposeBits2(
std::array<T, N> const &,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, 0>)
{
return T(0);
}
template<typename T, size_t N, size_t D, size_t B>
T
TransposeBits2(
std::array<T, N> const &in,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, B>)
{
const size_t size = std::numeric_limits<T>::digits;
const size_t src = ((D + ((size - B) * N)) / size);
const size_t src_bit =
size - (1+((D + ((size - B) * N)) % size));
const T src_bit_val = T(1) << src_bit;
const size_t dst_bit = (B - 1);
const T dst_bit_val = T(1) << dst_bit;
// Multiply rather than shift to avoid clang implicit
// conversion warning.
T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val;
return bit + TransposeBits2(
in,
std::integral_constant<size_t, D>(),
std::integral_constant<size_t, B-1>());
}
template<typename T, size_t N>
void
TransposeBits(
std::array<T, N> const &,
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t D>
void
TransposeBits(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, D>)
{
out[D-1] = TransposeBits2(
in,
std::integral_constant<size_t, D-1>(),
std::integral_constant<
size_t,
std::numeric_limits<T>::digits>());
TransposeBits(
in,
out,
std::integral_constant<size_t, D-1>());
}
template<typename T, size_t N, size_t D>
T
UntransposeBits2(
std::array<T, N> const &,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, 0>)
{
return T(0);
}
template<typename T, size_t N, size_t D, size_t B>
T
UntransposeBits2(
std::array<T, N> const &in,
std::integral_constant<size_t, D>,
std::integral_constant<size_t, B>)
{
const size_t size = std::numeric_limits<T>::digits;
const size_t src = ((D * size) + (size - B)) % N;
const size_t src_bit =
size - (((((D * size) + (size - B))) / N) + 1);
const T src_bit_val = T(1) << src_bit;
const size_t dst_bit(B - 1);
const T dst_bit_val = T(1) << dst_bit;
// Multiply rather than shift to avoid clang implicit
// conversion warning.
T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val;
return bit + UntransposeBits2(
in,
std::integral_constant<size_t, D>(),
std::integral_constant<size_t, size_t(B-1)>());
}
template<typename T, size_t N>
void
UntransposeBits(
std::array<T, N> const &,
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t D>
void
UntransposeBits(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, D>)
{
out[D-1] = UntransposeBits2(
in,
std::integral_constant<size_t, D-1>(),
std::integral_constant<
size_t,
std::numeric_limits<T>::digits>());
UntransposeBits(
in,
out,
std::integral_constant<size_t, D-1>());
}
template<typename T, size_t N>
void
ApplyGrayCode1(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, 0>)
{
out[0]^= in[N-1] >> 1;
}
template<typename T, size_t N, size_t I>
void
ApplyGrayCode1(
std::array<T, N> const &in,
std::array<T, N> &out,
std::integral_constant<size_t, I>)
{
out[I]^= out[I-1];
ApplyGrayCode1(
in,
out,
std::integral_constant<size_t, I-1>());
}
// Remove a gray code from a transposed vector
template<typename T, size_t N>
void
RemoveGrayCode1(
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
// xor array values with previous values.
template<typename T, size_t N, size_t D>
void
RemoveGrayCode1(
std::array<T, N> &in,
std::integral_constant<size_t, D>)
{
const size_t src_idx = N - (D + 1);
const size_t dst_idx = N - D;
in[dst_idx]^= in[src_idx];
RemoveGrayCode1(in, std::integral_constant<size_t, D-1>());
}
template<typename T>
T RemoveGrayCode2(T, std::integral_constant<size_t, 1>)
{
return T(0);
}
template<typename T, size_t B>
T RemoveGrayCode2(T v, std::integral_constant<size_t, B>)
{
const T cur_bit(T(1) << (B-1));
const T low_bits(cur_bit - 1);
if(v & cur_bit)
{
return low_bits ^ RemoveGrayCode2(
v,
std::integral_constant<size_t, B-1>());
}
else
{
return RemoveGrayCode2(
v,
std::integral_constant<size_t, B-1>());
}
}
template<typename T, size_t N, size_t B>
void
GrayToHilbert2(
std::array<T, N> &,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B, size_t I>
void
GrayToHilbert2(
std::array<T, N> &out,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, I>)
{
const size_t n(I-1);
const T cur_bit(T(1) << (std::numeric_limits<T>::digits - B));
const T low_bits(cur_bit - 1);
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
GrayToHilbert2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, I-1>());
}
template<typename T, size_t N>
void
GrayToHilbert(
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B>
void
GrayToHilbert(
std::array<T, N> &out,
std::integral_constant<size_t, B>)
{
GrayToHilbert2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, N>());
GrayToHilbert(out, std::integral_constant<size_t, B-1>());
}
template<typename T, size_t N, size_t B>
void
HilbertToGray2(
std::array<T, N> &,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B, size_t I>
void
HilbertToGray2(
std::array<T, N> &out,
std::integral_constant<size_t, B>,
std::integral_constant<size_t, I>)
{
const size_t cur_bit(T(1) << B);
const size_t low_bits(cur_bit-1);
const size_t n(N-I);
if(out[n] & cur_bit)
{
// flip low bits of X
out[0]^= low_bits;
}
else
{
// swap low bits with X
T t((out[n] ^ out[0]) & low_bits);
out[n]^= t;
out[0]^= t;
}
HilbertToGray2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, I-1>());
}
template<typename T, size_t N>
void
HilbertToGray(
std::array<T, N> &,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t B>
void
HilbertToGray(
std::array<T, N> &out,
std::integral_constant<size_t, B>)
{
HilbertToGray2(
out,
std::integral_constant<size_t, B>(),
std::integral_constant<size_t, N>());
HilbertToGray(out, std::integral_constant<size_t, B-1>());
}
template<typename T, size_t N>
void
ApplyMaskToArray(
std::array<T, N> &,
T,
std::integral_constant<size_t, 0>)
{
}
template<typename T, size_t N, size_t I>
void
ApplyMaskToArray(
std::array<T, N> &a,
T mask, std::integral_constant<size_t, I>)
{
a[I-1]^= mask;
ApplyMaskToArray(
a,
mask,
std::integral_constant<size_t, I-1>());
}
} // namespace tmp
// Pack bits into transposed form.
//
// e.g.
//
// a b c d a d g j
// e f g h -> b e h k
// i j k l c f i l
//
template<typename T, size_t N>
std::array<T, N>
TransposeBits(std::array<T, N> const &in)
{
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
tmp::TransposeBits(
in,
out,
std::integral_constant<size_t, N>());
return out;
}
// Extract bits from transposed form.
// e.g.
//
// a d g j a b c d
// b e h k -> e f g h
// c f i l i j k l
//
template<typename T, size_t N>
std::array<T, N>
UntransposeBits(std::array<T, N> const &in)
{
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
tmp::UntransposeBits(
in,
out,
std::integral_constant<size_t, N>());
return out;
}
// Apply a gray code to a transformed vector.
template<typename T, size_t N>
std::array<T, N>
ApplyGrayCode(std::array<T, N> const &in)
{
std::array<T, N> out(in);
tmp::ApplyGrayCode1(
in,
out,
std::integral_constant<size_t, N-1>());
return out;
}
template<typename T, size_t N>
std::array<T, N>
RemoveGrayCode(std::array<T, N> const &in)
{
const size_t bits = std::numeric_limits<T>::digits;
std::array<T, N> out(in);
// Remove gray code from transposed vector.
{
// xor values with prev values.
tmp::RemoveGrayCode1(
out,
std::integral_constant<size_t, N-1>());
// create a mask.
T t = tmp::RemoveGrayCode2(
out[N-1],
std::integral_constant<size_t, bits>());
// Apply mask to output.
tmp::ApplyMaskToArray(
out,
t,
std::integral_constant<size_t, N>());
}
return out;
}
// Generate code to convert from a transposed gray code to a hilbert
// code.
template<typename T, size_t N>
std::array<T, N>
GrayToHilbert(std::array<T, N> const &in)
{
std::array<T, N> out(in);
tmp::GrayToHilbert(
out,
std::integral_constant<
size_t,
std::numeric_limits<T>::digits - 1>());
return out;
}
// Generate code to convert from a hilbert code to a transposed gray
// code.
template<typename T, size_t N>
std::array<T, N>
HilbertToGray(std::array<T, N> const &in)
{
std::array<T, N> out(in);
tmp::HilbertToGray(
out,
std::integral_constant<
size_t,
std::numeric_limits<T>::digits-1>());
return out;
}
}
//
// Public interfaces.
//
// Find the position of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
IndexToPosition(std::array<T, N> const &in)
{
// First convert index to transpose.
return internal::GrayToHilbert(
internal::ApplyGrayCode(
internal::TransposeBits(in)));
}
// Find the index of a point on an N dimensional Hilbert Curve.
//
// Based on the paper "Programming the Hilbert Curve" by John Skilling.
//
// Index is encoded with most significant objects first. Lexographic
// sort order.
template<typename T, size_t N>
std::array<T, N>
PositionToIndex(std::array<T, N> const &in)
{
return internal::UntransposeBits(
internal::RemoveGrayCode(
internal::HilbertToGray(in)));
}
} // namespace v2
} // namespace hilbert
#endif

315
include/ivox/ivox3d.h Normal file
View File

@@ -0,0 +1,315 @@
//
// Created by xiang on 2021/9/16.
//
#ifndef FASTER_LIO_IVOX3D_H
#define FASTER_LIO_IVOX3D_H
#include <glog/logging.h>
// #include <execution>
#include <list>
#include <thread>
#include <unordered_map>
#include "eigen_types.h"
#include "ivox3d_node.hpp"
namespace faster_lio {
enum class IVoxNodeType {
DEFAULT, // linear ivox
PHC, // phc ivox
};
/// traits for NodeType
template <IVoxNodeType node_type, typename PointT, int dim>
struct IVoxNodeTypeTraits {};
template <typename PointT, int dim>
struct IVoxNodeTypeTraits<IVoxNodeType::DEFAULT, PointT, dim> {
using NodeType = IVoxNode<PointT, dim>;
};
template <typename PointT, int dim>
struct IVoxNodeTypeTraits<IVoxNodeType::PHC, PointT, dim> {
using NodeType = IVoxNodePhc<PointT, dim>;
};
template <int dim = 3, IVoxNodeType node_type = IVoxNodeType::DEFAULT, typename PointType = pcl::PointXYZ>
class IVox {
public:
using KeyType = Eigen::Matrix<int, dim, 1>;
using PtType = Eigen::Matrix<float, dim, 1>;
using NodeType = typename IVoxNodeTypeTraits<node_type, PointType, dim>::NodeType;
using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
using DistPoint = typename NodeType::DistPoint;
enum class NearbyType {
CENTER, // center only
NEARBY6,
NEARBY18,
NEARBY26,
};
struct Options {
float resolution_ = 0.2; // ivox resolution
float inv_resolution_ = 10.0; // inverse resolution
NearbyType nearby_type_ = NearbyType::NEARBY6; // nearby range
std::size_t capacity_ = 1000000; // capacity
};
/**
* constructor
* @param options ivox options
*/
explicit IVox(Options options) : options_(options) {
options_.inv_resolution_ = 1.0 / options_.resolution_;
GenerateNearbyGrids();
}
/**
* add points
* @param points_to_add
*/
void AddPoints(const PointVector& points_to_add);
/// get nn
bool GetClosestPoint(const PointType& pt, PointType& closest_pt);
/// get nn with condition
bool GetClosestPoint(const PointType& pt, PointVector& closest_pt, int max_num = 5, double max_range = 5.0);
/// get nn in cloud
bool GetClosestPoint(const PointVector& cloud, PointVector& closest_cloud);
/// get number of points
size_t NumPoints() const;
/// get number of valid grids
size_t NumValidGrids() const;
/// get statistics of the points
std::vector<float> StatGridPoints() const;
std::unordered_map<KeyType, typename std::list<std::pair<KeyType, NodeType>>::iterator, hash_vec<dim>>
grids_map_;
KeyType Pos2Grid(const PtType& pt) const;
KeyType Pos2Grid_(const PtType& pt, const double &defined_res) const;
private:
/// generate the nearby grids according to the given options
void GenerateNearbyGrids();
/// position to grid
// KeyType Pos2Grid(const PtType& pt) const;
Options options_;
// std::unordered_map<KeyType, typename std::list<std::pair<KeyType, NodeType>>::iterator, hash_vec<dim>>
// grids_map_; // voxel hash map
std::list<std::pair<KeyType, NodeType>> grids_cache_; // voxel cache
std::vector<KeyType> nearby_grids_; // nearbys
};
template <int dim, IVoxNodeType node_type, typename PointType>
bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointType& pt, PointType& closest_pt) {
std::vector<DistPoint> candidates;
auto key = Pos2Grid(ToEigen<float, dim>(pt));
std::for_each(nearby_grids_.begin(), nearby_grids_.end(), [&key, &candidates, &pt, this](const KeyType& delta) {
auto dkey = key + delta;
auto iter = grids_map_.find(dkey);
if (iter != grids_map_.end()) {
DistPoint dist_point;
bool found = iter->second->second.NNPoint(pt, dist_point);
if (found) {
candidates.emplace_back(dist_point);
}
}
});
if (candidates.empty()) {
return false;
}
auto iter = std::min_element(candidates.begin(), candidates.end());
closest_pt = iter->Get();
return true;
}
template <int dim, IVoxNodeType node_type, typename PointType>
bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointType& pt, PointVector& closest_pt, int max_num,
double max_range) {
std::vector<DistPoint> candidates;
candidates.reserve(max_num * nearby_grids_.size());
auto key = Pos2Grid(ToEigen<float, dim>(pt));
// #define INNER_TIMER
#ifdef INNER_TIMER
static std::unordered_map<std::string, std::vector<int64_t>> stats;
if (stats.empty()) {
stats["knn"] = std::vector<int64_t>();
stats["nth"] = std::vector<int64_t>();
}
#endif
for (const KeyType& delta : nearby_grids_) {
auto dkey = key + delta;
auto iter = grids_map_.find(dkey);
if (iter != grids_map_.end()) {
#ifdef INNER_TIMER
auto t1 = std::chrono::high_resolution_clock::now();
#endif
auto tmp = iter->second->second.KNNPointByCondition(candidates, pt, max_num, max_range);
#ifdef INNER_TIMER
auto t2 = std::chrono::high_resolution_clock::now();
auto knn = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
stats["knn"].emplace_back(knn);
#endif
}
}
if (candidates.empty()) {
return false;
}
#ifdef INNER_TIMER
auto t1 = std::chrono::high_resolution_clock::now();
#endif
if (candidates.size() <= max_num) {
} else {
std::nth_element(candidates.begin(), candidates.begin() + max_num - 1, candidates.end());
candidates.resize(max_num);
}
std::nth_element(candidates.begin(), candidates.begin(), candidates.end());
#ifdef INNER_TIMER
auto t2 = std::chrono::high_resolution_clock::now();
auto nth = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
stats["nth"].emplace_back(nth);
constexpr int STAT_PERIOD = 100000;
if (!stats["nth"].empty() && stats["nth"].size() % STAT_PERIOD == 0) {
for (auto& it : stats) {
const std::string& key = it.first;
std::vector<int64_t>& stat = it.second;
int64_t sum_ = std::accumulate(stat.begin(), stat.end(), 0);
int64_t num_ = stat.size();
stat.clear();
std::cout << "inner_" << key << "(ns): sum=" << sum_ << " num=" << num_ << " ave=" << 1.0 * sum_ / num_
<< " ave*n=" << 1.0 * sum_ / STAT_PERIOD << std::endl;
}
}
#endif
closest_pt.clear();
for (auto& it : candidates) {
closest_pt.emplace_back(it.Get());
}
return closest_pt.empty() == false;
}
template <int dim, IVoxNodeType node_type, typename PointType>
size_t IVox<dim, node_type, PointType>::NumValidGrids() const {
return grids_map_.size();
}
template <int dim, IVoxNodeType node_type, typename PointType>
void IVox<dim, node_type, PointType>::GenerateNearbyGrids() {
if (options_.nearby_type_ == NearbyType::CENTER) {
nearby_grids_.emplace_back(KeyType::Zero());
} else if (options_.nearby_type_ == NearbyType::NEARBY6) {
nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
} else if (options_.nearby_type_ == NearbyType::NEARBY18) {
nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1), KeyType(1, 1, 0),
KeyType(-1, 1, 0), KeyType(1, -1, 0), KeyType(-1, -1, 0), KeyType(1, 0, 1),
KeyType(-1, 0, 1), KeyType(1, 0, -1), KeyType(-1, 0, -1), KeyType(0, 1, 1),
KeyType(0, -1, 1), KeyType(0, 1, -1), KeyType(0, -1, -1)};
} else if (options_.nearby_type_ == NearbyType::NEARBY26) {
nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1), KeyType(1, 1, 0),
KeyType(-1, 1, 0), KeyType(1, -1, 0), KeyType(-1, -1, 0), KeyType(1, 0, 1),
KeyType(-1, 0, 1), KeyType(1, 0, -1), KeyType(-1, 0, -1), KeyType(0, 1, 1),
KeyType(0, -1, 1), KeyType(0, 1, -1), KeyType(0, -1, -1), KeyType(1, 1, 1),
KeyType(-1, 1, 1), KeyType(1, -1, 1), KeyType(1, 1, -1), KeyType(-1, -1, 1),
KeyType(-1, 1, -1), KeyType(1, -1, -1), KeyType(-1, -1, -1)};
} else {
// LOG(ERROR) << "Unknown nearby_type!";
}
}
template <int dim, IVoxNodeType node_type, typename PointType>
bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointVector& cloud, PointVector& closest_cloud) {
std::vector<size_t> index(cloud.size());
closest_cloud.resize(cloud.size());
for (int i = 0; i < cloud.size(); ++i) {
PointType pt;
if (GetClosestPoint(cloud[i], pt)) {
closest_cloud[i] = pt;
} else {
closest_cloud[i] = PointType();
}
};
return true;
}
template <int dim, IVoxNodeType node_type, typename PointType>
void IVox<dim, node_type, PointType>::AddPoints(const PointVector& points_to_add) {
for(size_t i = 0; i<points_to_add.size(); i++) {
auto key = Pos2Grid(Eigen::Matrix<float, dim, 1>(points_to_add[i].x, points_to_add[i].y, points_to_add[i].z));
auto iter = grids_map_.find(key);
if (iter == grids_map_.end()) {
PointType center;
center.getVector3fMap() = key.template cast<float>() * options_.resolution_;
grids_cache_.push_front({key, NodeType(center, options_.resolution_)});
grids_map_.insert({key, grids_cache_.begin()});
grids_cache_.front().second.InsertPoint(points_to_add[i]);
if (grids_map_.size() >= options_.capacity_) {
grids_map_.erase(grids_cache_.back().first);
grids_cache_.pop_back();
}
} else {
iter->second->second.InsertPoint(points_to_add[i]);
grids_cache_.splice(grids_cache_.begin(), grids_cache_, iter->second);
grids_map_[key] = grids_cache_.begin();
}
}
}
template <int dim, IVoxNodeType node_type, typename PointType>
Eigen::Matrix<int, dim, 1> IVox<dim, node_type, PointType>::Pos2Grid(const IVox::PtType& pt) const {
return (pt * options_.inv_resolution_).array().floor().template cast<int>();
}
template <int dim, IVoxNodeType node_type, typename PointType>
Eigen::Matrix<int, dim, 1> IVox<dim, node_type, PointType>::Pos2Grid_(const IVox::PtType& pt, const double &defined_res) const {
return (pt / defined_res).array().floor().template cast<int>();
}
template <int dim, IVoxNodeType node_type, typename PointType>
std::vector<float> IVox<dim, node_type, PointType>::StatGridPoints() const {
int num = grids_cache_.size(), valid_num = 0, max = 0, min = 100000000;
int sum = 0, sum_square = 0;
for (auto& it : grids_cache_) {
int s = it.second.Size();
valid_num += s > 0;
max = s > max ? s : max;
min = s < min ? s : min;
sum += s;
sum_square += s * s;
}
float ave = float(sum) / num;
float stddev = num > 1 ? sqrt((float(sum_square) - num * ave * ave) / (num - 1)) : 0;
return std::vector<float>{valid_num, ave, max, min, stddev};
}
} // namespace faster_lio
#endif

413
include/ivox/ivox3d_node.hpp Normal file
View File

@@ -0,0 +1,413 @@
#include <pcl/common/centroid.h>
#include <algorithm>
#include <cmath>
#include <list>
#include <vector>
#include "hilbert.hpp"
namespace faster_lio {
// squared distance of two pcl points
template <typename PointT>
inline double distance2(const PointT& pt1, const PointT& pt2) {
Eigen::Vector3f d = pt1.getVector3fMap() - pt2.getVector3fMap();
return d.squaredNorm();
}
// convert from pcl point to eigen
template <typename T, int dim, typename PointType>
inline Eigen::Matrix<T, dim, 1> ToEigen(const PointType& pt) {
cout << "line 21" << endl;
return Eigen::Matrix<T, dim, 1>(pt.x, pt.y, pt.z);
cout << "line 23" << endl;
}
template <>
inline Eigen::Matrix<float, 3, 1> ToEigen<float, 3, pcl::PointXYZ>(const pcl::PointXYZ& pt) {
return pt.getVector3fMap();
}
template <>
inline Eigen::Matrix<float, 3, 1> ToEigen<float, 3, pcl::PointXYZI>(const pcl::PointXYZI& pt) {
return pt.getVector3fMap();
}
template <>
inline Eigen::Matrix<float, 3, 1> ToEigen<float, 3, pcl::PointXYZINormal>(const pcl::PointXYZINormal& pt) {
return pt.getVector3fMap();
}
template <typename PointT, int dim = 3>
class IVoxNode {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
struct DistPoint;
IVoxNode() = default;
IVoxNode(const PointT& center, const float& side_length) {} /// same with phc
void InsertPoint(const PointT& pt);
inline bool Empty() const;
inline std::size_t Size() const;
inline PointT GetPoint(const std::size_t idx) const;
int KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& point, const int& K,
const double& max_range);
private:
std::vector<PointT> points_;
};
template <typename PointT, int dim = 3>
class IVoxNodePhc {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
struct DistPoint;
struct PhcCube;
IVoxNodePhc() = default;
IVoxNodePhc(const PointT& center, const float& side_length, const int& phc_order = 6);
void InsertPoint(const PointT& pt);
void ErasePoint(const PointT& pt, const double erase_distance_th_);
inline bool Empty() const;
inline std::size_t Size() const;
PointT GetPoint(const std::size_t idx) const;
bool NNPoint(const PointT& cur_pt, DistPoint& dist_point) const;
int KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& cur_pt, const int& K = 5,
const double& max_range = 5.0);
private:
uint32_t CalculatePhcIndex(const PointT& pt) const;
private:
std::vector<PhcCube> phc_cubes_;
PointT center_;
float side_length_ = 0;
int phc_order_ = 6;
float phc_side_length_ = 0;
float phc_side_length_inv_ = 0;
Eigen::Matrix<float, dim, 1> min_cube_;
};
template <typename PointT, int dim>
struct IVoxNode<PointT, dim>::DistPoint {
double dist = 0;
IVoxNode* node = nullptr;
int idx = 0;
DistPoint() = default;
DistPoint(const double d, IVoxNode* n, const int i) : dist(d), node(n), idx(i) {}
PointT Get() { return node->GetPoint(idx); }
inline bool operator()(const DistPoint& p1, const DistPoint& p2) { return p1.dist < p2.dist; }
inline bool operator<(const DistPoint& rhs) { return dist < rhs.dist; }
};
template <typename PointT, int dim>
void IVoxNode<PointT, dim>::InsertPoint(const PointT& pt) {
points_.template emplace_back(pt);
}
template <typename PointT, int dim>
bool IVoxNode<PointT, dim>::Empty() const {
return points_.empty();
}
template <typename PointT, int dim>
std::size_t IVoxNode<PointT, dim>::Size() const {
return points_.size();
}
template <typename PointT, int dim>
PointT IVoxNode<PointT, dim>::GetPoint(const std::size_t idx) const {
return points_[idx];
}
template <typename PointT, int dim>
int IVoxNode<PointT, dim>::KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& point, const int& K,
const double& max_range) {
std::size_t old_size = dis_points.size();
// #define INNER_TIMER
#ifdef INNER_TIMER
static std::unordered_map<std::string, std::vector<int64_t>> stats;
if (stats.empty()) {
stats["dis"] = std::vector<int64_t>();
stats["put"] = std::vector<int64_t>();
stats["nth"] = std::vector<int64_t>();
}
#endif
for (const auto& pt : points_) {
#ifdef INNER_TIMER
auto t0 = std::chrono::high_resolution_clock::now();
#endif
double d = distance2(pt, point);
#ifdef INNER_TIMER
auto t1 = std::chrono::high_resolution_clock::now();
#endif
if (d < max_range * max_range) {
dis_points.template emplace_back(DistPoint(d, this, &pt - points_.data()));
}
#ifdef INNER_TIMER
auto t2 = std::chrono::high_resolution_clock::now();
auto dis = std::chrono::duration_cast<std::chrono::nanoseconds>(t1 - t0).count();
stats["dis"].emplace_back(dis);
auto put = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
stats["put"].emplace_back(put);
#endif
}
#ifdef INNER_TIMER
auto t1 = std::chrono::high_resolution_clock::now();
#endif
// sort by distance
if (old_size + K >= dis_points.size()) {
} else {
std::nth_element(dis_points.begin() + old_size, dis_points.begin() + old_size + K - 1, dis_points.end());
dis_points.resize(old_size + K);
}
#ifdef INNER_TIMER
auto t2 = std::chrono::high_resolution_clock::now();
auto nth = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
stats["nth"].emplace_back(nth);
constexpr int STAT_PERIOD = 100000;
if (!stats["nth"].empty() && stats["nth"].size() % STAT_PERIOD == 0) {
for (auto& it : stats) {
const std::string& key = it.first;
std::vector<int64_t>& stat = it.second;
int64_t sum_ = std::accumulate(stat.begin(), stat.end(), 0);
int64_t num_ = stat.size();
stat.clear();
std::cout << "inner_" << key << "(ns): sum=" << sum_ << " num=" << num_ << " ave=" << 1.0 * sum_ / num_
<< " ave*n=" << 1.0 * sum_ / STAT_PERIOD << std::endl;
}
}
#endif
return dis_points.size();
}
template <typename PointT, int dim>
struct IVoxNodePhc<PointT, dim>::DistPoint {
double dist = 0;
IVoxNodePhc* node = nullptr;
int idx = 0;
DistPoint() {}
DistPoint(const double d, IVoxNodePhc* n, const int i) : dist(d), node(n), idx(i) {}
PointT Get() { return node->GetPoint(idx); }
inline bool operator()(const DistPoint& p1, const DistPoint& p2) { return p1.dist < p2.dist; }
inline bool operator<(const DistPoint& rhs) { return dist < rhs.dist; }
};
template <typename PointT, int dim>
struct IVoxNodePhc<PointT, dim>::PhcCube {
uint32_t idx = 0;
pcl::CentroidPoint<PointT> mean;
PhcCube(uint32_t index, const PointT& pt) { mean.add(pt); }
void AddPoint(PointT& pt) { mean.add(pt); }
PointT GetPoint() const {
PointT pt;
mean.get(pt);
return std::move(pt);
}
};
template <typename PointT, int dim>
IVoxNodePhc<PointT, dim>::IVoxNodePhc(const PointT& center, const float& side_length, const int& phc_order)
: center_(center), side_length_(side_length), phc_order_(phc_order) {
assert(phc_order <= 8);
phc_side_length_ = side_length_ / (std::pow(2, phc_order_));
phc_side_length_inv_ = (std::pow(2, phc_order_)) / side_length_;
min_cube_ = center_.getArray3fMap() - side_length / 2.0;
phc_cubes_.reserve(64);
}
template <typename PointT, int dim>
void IVoxNodePhc<PointT, dim>::InsertPoint(const PointT& pt) {
uint32_t idx = CalculatePhcIndex(pt);
PhcCube cube{idx, pt};
auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
[](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
if (it == phc_cubes_.end()) {
phc_cubes_.emplace_back(cube);
} else {
if (it->idx == idx) {
it->AddPoint(pt);
} else {
phc_cubes_.insert(it, cube);
}
}
}
template <typename PointT, int dim>
void IVoxNodePhc<PointT, dim>::ErasePoint(const PointT& pt, const double erase_distance_th_) {
uint32_t idx = CalculatePhcIndex(pt);
PhcCube cube{idx, pt};
auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
[](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
if (erase_distance_th_ > 0) {
}
if (it != phc_cubes_.end() && it->idx == idx) {
phc_cubes_.erase(it);
}
}
template <typename PointT, int dim>
bool IVoxNodePhc<PointT, dim>::Empty() const {
return phc_cubes_.empty();
}
template <typename PointT, int dim>
std::size_t IVoxNodePhc<PointT, dim>::Size() const {
return phc_cubes_.size();
}
template <typename PointT, int dim>
PointT IVoxNodePhc<PointT, dim>::GetPoint(const std::size_t idx) const {
return phc_cubes_[idx].GetPoint();
}
template <typename PointT, int dim>
bool IVoxNodePhc<PointT, dim>::NNPoint(const PointT& cur_pt, DistPoint& dist_point) const {
if (phc_cubes_.empty()) {
return false;
}
uint32_t cur_idx = CalculatePhcIndex(cur_pt);
PhcCube cube{cur_idx, cur_pt};
auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
[](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
if (it == phc_cubes_.end()) {
it--;
dist_point = DistPoint(distance2(cur_pt, it->GetPoint()), this, it - phc_cubes_.begin());
} else if (it == phc_cubes_.begin()) {
dist_point = DistPoint(distance2(cur_pt, it->GetPoint()), this, it - phc_cubes_.begin());
} else {
auto last_it = it;
last_it--;
double d1 = distance2(cur_pt, it->GetPoint());
double d2 = distance2(cur_pt, last_it->GetPoint());
if (d1 > d2) {
dist_point = DistPoint(d2, this, it - phc_cubes_.begin());
} else {
dist_point = DistPoint(d1, this, it - phc_cubes_.begin());
}
}
return true;
}
template <typename PointT, int dim>
int IVoxNodePhc<PointT, dim>::KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& cur_pt,
const int& K, const double& max_range) {
uint32_t cur_idx = CalculatePhcIndex(cur_pt);
PhcCube cube{cur_idx, cur_pt};
auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
[](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
const int max_search_cube_side_length = std::pow(2, std::ceil(std::log2(max_range * phc_side_length_inv_)));
const int max_search_idx_th =
8 * max_search_cube_side_length * max_search_cube_side_length * max_search_cube_side_length;
auto create_dist_point = [&cur_pt, this](typename std::vector<PhcCube>::const_iterator forward_it) {
double d = distance2(forward_it->GetPoint(), cur_pt);
return DistPoint(d, this, forward_it - phc_cubes_.begin());
};
typename std::vector<PhcCube>::const_iterator forward_it(it);
typename std::vector<PhcCube>::const_reverse_iterator backward_it(it);
if (it != phc_cubes_.end()) {
dis_points.emplace_back(create_dist_point(it));
forward_it++;
}
if (backward_it != phc_cubes_.rend()) {
backward_it++;
}
auto forward_reach_boundary = [&]() {
return forward_it == phc_cubes_.end() || forward_it->idx - cur_idx > max_search_idx_th;
};
auto backward_reach_boundary = [&]() {
return backward_it == phc_cubes_.rend() || cur_idx - backward_it->idx > max_search_idx_th;
};
while (!forward_reach_boundary() && !backward_reach_boundary()) {
if (forward_it->idx - cur_idx > cur_idx - backward_it->idx) {
dis_points.emplace_back(create_dist_point(forward_it));
forward_it++;
} else {
dis_points.emplace_back(create_dist_point(backward_it.base()));
backward_it++;
}
if (dis_points.size() > K) {
break;
}
}
if (forward_reach_boundary()) {
while (!backward_reach_boundary() && dis_points.size() < K) {
dis_points.emplace_back(create_dist_point(backward_it.base()));
backward_it++;
}
}
if (backward_reach_boundary()) {
while (!forward_reach_boundary() && dis_points.size() < K) {
dis_points.emplace_back(create_dist_point(forward_it));
forward_it++;
}
}
return dis_points.size();
}
template <typename PointT, int dim>
uint32_t IVoxNodePhc<PointT, dim>::CalculatePhcIndex(const PointT& pt) const {
Eigen::Matrix<float, dim, 1> eposf = (pt.getVector3fMap() - min_cube_) * phc_side_length_inv_;
Eigen::Matrix<int, dim, 1> eposi = eposf.template cast<int>();
for (int i = 0; i < dim; ++i) {
if (eposi(i, 0) < 0) {
eposi(i, 0) = 0;
}
if (eposi(i, 0) > std::pow(2, phc_order_)) {
eposi(i, 0) = std::pow(2, phc_order_) - 1;
}
}
std::array<uint8_t, 3> apos{eposi(0), eposi(1), eposi(2)};
std::array<uint8_t, 3> tmp = hilbert::v2::PositionToIndex(apos);
uint32_t idx = (uint32_t(tmp[0]) << 16) + (uint32_t(tmp[1]) << 8) + (uint32_t(tmp[2]));
return idx;
}
} // namespace faster_lio

2500
include/matplotlibcpp.h Normal file
View File

File diff suppressed because it is too large Load Diff

128
include/so3_math.h Executable file
View File

@@ -0,0 +1,128 @@
#ifndef SO3_MATH_H
#define SO3_MATH_H
#include <math.h>
#include <Eigen/Core>
// #include <common_lib.h>
#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
template<typename T>
Eigen::Matrix<T, 3, 3> skew_sym_mat(const Eigen::Matrix<T, 3, 1> &v)
{
Eigen::Matrix<T, 3, 3> skew_sym_mat;
skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0;
return skew_sym_mat;
}
template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang)
{
T ang_norm = ang.norm();
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (ang_norm > 0.0000001)
{
Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
Eigen::Matrix<T, 3, 3> K;
K << SKEW_SYM_MATRX(r_axis);
/// Roderigous Tranformation
return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
}
else
{
return Eye3;
}
}
template<typename T, typename Ts>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
{
T ang_vel_norm = ang_vel.norm();
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (ang_vel_norm > 0.0000001)
{
Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
Eigen::Matrix<T, 3, 3> K;
K << SKEW_SYM_MATRX(r_axis);
T r_ang = ang_vel_norm * dt;
/// Roderigous Tranformation
return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
}
else
{
return Eye3;
}
}
template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
{
T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (norm > 0.00001)
{
T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
Eigen::Matrix<T, 3, 3> K;
K << SKEW_SYM_MATRX(r_ang);
/// Roderigous Tranformation
return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
}
else
{
return Eye3;
}
}
/* Logrithm of a Rotation Matrix */
template<typename T>
Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> R)
{
T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));
Eigen::Matrix<T,3,1> K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
}
template<typename T>
Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
{
T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
bool singular = sy < 1e-6;
T x, y, z;
if(!singular)
{
x = atan2(rot(2, 1), rot(2, 2));
y = atan2(-rot(2, 0), sy);
z = atan2(rot(1, 0), rot(0, 0));
}
else
{
x = atan2(-rot(1, 2), rot(1, 1));
y = atan2(-rot(2, 0), sy);
z = 0;
}
Eigen::Matrix<T, 3, 1> ang(x, y, z);
return ang;
}
template<typename T>
Eigen::Matrix3d Jacob_right_inv(Eigen::Vector3d &vec){
Eigen::Matrix3d hat_v, res;
hat_v << SKEW_SYM_MATRX(vec);
if(vec.norm() > 1e-6)
{
res = Eigen::Matrix<double, 3, 3>::Identity() + 0.5 * hat_v + (1 - vec.norm() * std::cos(vec.norm() / 2) / 2 / std::sin(vec.norm() / 2)) * hat_v * hat_v / vec.squaredNorm();
}
else
{
res = Eigen::Matrix<double, 3, 3>::Identity();
}
return res;
}
#endif

25
launch/gdb_debug_example.launch Executable file
View File

@@ -0,0 +1,25 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen" required="true" launch-prefix="gdb -ex run --args">
<rosparam command="load" file="$(find point_lio)/config/avia.yaml" />
<param name="use_imu_as_input" type="bool" value="0"/> <!--change to 1 to use IMU as input of Point-LIO-->
<param name="prop_at_freq_of_imu" type="bool" value="1"/>
<param name="check_satu" type="bool" value="1"/>
<param name="init_map_size" type="int" value="10"/>
<param name="point_filter_num" type="int" value="1"/> <!--4, 3-->
<param name="space_down_sample" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.3" /> <!--0.5, 0.3, 0.2, 0.15, 0.1-->
<param name="filter_size_map" type="double" value="0.2" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="ivox_nearby_type" type="int" value="6" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="runtime_pos_log_enable" type="bool" value="0" /> <!--1-->
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find point_lio)/rviz_cfg/loam_livox.rviz" />
</group>
launch-prefix="gdb -ex run --args"
</launch>

25
launch/mapping_avia.launch Executable file
View File

@@ -0,0 +1,25 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen">
<rosparam command="load" file="$(find point_lio)/config/avia.yaml" />
<param name="use_imu_as_input" type="bool" value="0"/> <!--change to 1 to use IMU as input of Point-LIO-->
<param name="prop_at_freq_of_imu" type="bool" value="1"/>
<param name="check_satu" type="bool" value="1"/>
<param name="init_map_size" type="int" value="10"/>
<param name="point_filter_num" type="int" value="1"/> <!--4, 3-->
<param name="space_down_sample" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.5" /> <!--0.5, 0.3, 0.2, 0.15, 0.1-->
<param name="filter_size_map" type="double" value="0.4" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="ivox_nearby_type" type="int" value="6" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="runtime_pos_log_enable" type="bool" value="0" /> <!--1-->
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find point_lio)/rviz_cfg/loam_livox.rviz" />
</group>
launch-prefix="valgrind"
</launch>

25
launch/mapping_horizon.launch Executable file
View File

@@ -0,0 +1,25 @@
<launch>
<!-- Launch file for Livox Horizon LiDAR -->
<arg name="rviz" default="true" />
<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen">
<rosparam command="load" file="$(find point_lio)/config/horizon.yaml" />
<param name="use_imu_as_input" type="bool" value="0"/> <!--change to 1 to use IMU as input of Point-LIO-->
<param name="prop_at_freq_of_imu" type="bool" value="1"/>
<param name="check_satu" type="bool" value="1"/>
<param name="init_map_size" type="int" value="10"/>
<param name="point_filter_num" type="int" value="3"/> <!--1, 3-->
<param name="space_down_sample" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.5" /> <!--0.5, 0.3, 0.2, 0.15, 0.1-->
<param name="filter_size_map" type="double" value="0.5" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="ivox_nearby_type" type="int" value="6" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="runtime_pos_log_enable" type="bool" value="0" /> <!--1-->
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find point_lio)/rviz_cfg/loam_livox.rviz" />
</group>
launch-prefix="gdb -ex run --args"
</launch>

25
launch/mapping_ouster64.launch Executable file
View File

@@ -0,0 +1,25 @@
<launch>
<!-- Launch file for Ouster OS2-64 LiDAR -->
<arg name="rviz" default="true" />
<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen">
<rosparam command="load" file="$(find point_lio)/config/ouster64.yaml" />
<param name="use_imu_as_input" type="bool" value="0"/> <!--change to 1 to use IMU as input of Point-LIO-->
<param name="prop_at_freq_of_imu" type="bool" value="1"/>
<param name="check_satu" type="bool" value="1"/>
<param name="init_map_size" type="int" value="10"/>
<param name="point_filter_num" type="int" value="4"/> <!--4, 3-->
<param name="space_down_sample" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.5" /> <!--0.5, 0.3, 0.2, 0.15, 0.1-->
<param name="filter_size_map" type="double" value="0.5" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="ivox_nearby_type" type="int" value="6" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="runtime_pos_log_enable" type="bool" value="0" /> <!--1-->
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find point_lio)/rviz_cfg/loam_livox.rviz" />
</group>
launch-prefix="gdb -ex run --args"
</launch>

25
launch/mapping_velody16.launch Executable file
View File

@@ -0,0 +1,25 @@
<launch>
<!-- Launch file for Velodyne16 VLP-16 LiDAR -->
<arg name="rviz" default="true" />
<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen">
<rosparam command="load" file="$(find point_lio)/config/velody16.yaml" />
<param name="use_imu_as_input" type="bool" value="0"/> <!--change to 1 to use IMU as input of Point-LIO-->
<param name="prop_at_freq_of_imu" type="bool" value="1"/>
<param name="check_satu" type="bool" value="1"/>
<param name="init_map_size" type="int" value="10"/>
<param name="point_filter_num" type="int" value="4"/> <!--4, 3-->
<param name="space_down_sample" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.5" /> <!--0.5, 0.3, 0.2, 0.15, 0.1-->
<param name="filter_size_map" type="double" value="0.5" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="ivox_nearby_type" type="int" value="6" /> <!--0.5, 0.3, 0.15, 0.1-->
<param name="runtime_pos_log_enable" type="bool" value="0" /> <!--1-->
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find point_lio)/rviz_cfg/loam_livox.rviz" />
</group>
launch-prefix="gdb -ex run --args"
</launch>

6
msg/LocalSensorExternalTrigger.msg Normal file
View File

@@ -0,0 +1,6 @@
# This message contains essential time information of the
# local sensor (lidar/inertial sensor) when get triggered
Header header # local timestamp of the trigger
uint32 trigger_id
uint32 event_id
time timestamp_host # corresponding timestamp of the Host PC, not used

47
package.xml Executable file
View File

@@ -0,0 +1,47 @@
<?xml version="1.0"?>
<package>
<name>point_lio</name>
<version>0.0.0</version>
<description>
This is a modified version of LOAM which is original algorithm
is described in the following paper:
J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
</description>
<maintainer email="dev@livoxtech.com">claydergc</maintainer>
<license>BSD</license>
<author email="hdj65822@connect.hku.hk">Dongjiao He</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>livox_ros_driver</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>livox_ros_driver</run_depend>
<run_depend>message_runtime</run_depend>
<test_depend>rostest</test_depend>
<test_depend>rosbag</test_depend>
<export>
</export>
</package>

0
rviz_cfg/.gitignore vendored Executable file
View File

365
rviz_cfg/loam_livox.rviz Executable file
View File

@@ -0,0 +1,365 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Axes1
- /mapping1
- /mapping1/surround1
- /mapping1/currPoints1
- /mapping1/currPoints1/Autocompute Value Bounds1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Odometry1/Odometry1/Covariance1
- /Odometry1/Odometry1/Covariance1/Position1
- /Odometry1/Odometry1/Covariance1/Orientation1
- /MarkerArray1/Namespaces1
Splitter Ratio: 0.6432291865348816
Tree Height: 777
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 1000
Class: rviz/Grid
Color: 160; 160; 164
Enabled: false
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 40
Reference Frame: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.05999999865889549
Reference Frame: aft_mapped
Show Trail: false
Value: true
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 238; 238; 236
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 3
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1000
Enabled: true
Invert Rainbow: true
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: mapping
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.009999999776482582
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.20000000298023224
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /Odometry
Unreliable: false
Value: true
Enabled: true
Name: Odometry
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: true
- Alpha: 0
Buffer Length: 2
Class: rviz/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.20000000298023224
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 239; 41; 41
Max Intensity: 0
Min Color: 239; 41; 41
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 4
Size (m): 0.30000001192092896
Style: Spheres
Topic: /cloud_effected
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 13.139549255371094
Min Value: -32.08251953125
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 138; 226; 52
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 138; 226; 52
Min Color: 138; 226; 52
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /MarkerArray
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: camera_init
Frame Rate: 10
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 100.72154235839844
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 34.07209014892578
Y: -2.53304386138916
Z: -8.43538761138916
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.06979652494192123
Target Frame: global
Yaw: 4.245370388031006
Saved: ~
Window Geometry:
Displays:
collapsed: true
Height: 1016
Hide Left Dock: true
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001c800000346fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000346000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000052fc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007380000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1848
X: 72
Y: 27

401
src/Estimator.cpp Executable file
View File

@@ -0,0 +1,401 @@
// #include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp>
#include "Estimator.h"
PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));
std::vector<int> time_seq;
PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI(10000, 1));
PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI(10000, 1));
std::vector<V3D> pbody_list;
std::vector<PointVector> Nearest_Points;
std::shared_ptr<IVoxType> ivox_ = nullptr; // localmap in ivox
std::vector<float> pointSearchSqDis(NUM_MATCH_POINTS);
bool point_selected_surf[100000] = {0};
std::vector<M3D> crossmat_list;
int effct_feat_num = 0;
int k = 0;
int idx = -1;
esekfom::esekf<state_input, 24, input_ikfom> kf_input;
esekfom::esekf<state_output, 30, input_ikfom> kf_output;
input_ikfom input_in;
V3D angvel_avr, acc_avr, acc_avr_norm;
int feats_down_size = 0;
V3D Lidar_T_wrt_IMU(Zero3d);
M3D Lidar_R_wrt_IMU(Eye3d);
Eigen::Matrix<double, 24, 24> process_noise_cov_input()
{
Eigen::Matrix<double, 24, 24> cov;
cov.setZero();
cov.block<3, 3>(3, 3).diagonal() << gyr_cov_input, gyr_cov_input, gyr_cov_input;
cov.block<3, 3>(12, 12).diagonal() << acc_cov_input, acc_cov_input, acc_cov_input;
cov.block<3, 3>(15, 15).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov;
cov.block<3, 3>(18, 18).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov;
// MTK::get_cov<process_noise_input>::type cov = MTK::get_cov<process_noise_input>::type::Zero();
// MTK::setDiagonal<process_noise_input, vect3, 0>(cov, &process_noise_input::ng, gyr_cov_input);// 0.03
// MTK::setDiagonal<process_noise_input, vect3, 3>(cov, &process_noise_input::na, acc_cov_input); // *dt 0.01 0.01 * dt * dt 0.05
// MTK::setDiagonal<process_noise_input, vect3, 6>(cov, &process_noise_input::nbg, b_gyr_cov); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
// MTK::setDiagonal<process_noise_input, vect3, 9>(cov, &process_noise_input::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01
return cov;
}
Eigen::Matrix<double, 30, 30> process_noise_cov_output()
{
Eigen::Matrix<double, 30, 30> cov;
cov.setZero();
cov.block<3, 3>(12, 12).diagonal() << vel_cov, vel_cov, vel_cov;
cov.block<3, 3>(15, 15).diagonal() << gyr_cov_output, gyr_cov_output, gyr_cov_output;
cov.block<3, 3>(18, 18).diagonal() << acc_cov_output, acc_cov_output, acc_cov_output;
cov.block<3, 3>(24, 24).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov;
cov.block<3, 3>(27, 27).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov;
return cov;
}
Eigen::Matrix<double, 24, 1> get_f_input(state_input &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
vect3 omega;
in.gyro.boxminus(omega, s.bg);
vect3 a_inertial = s.rot * (in.acc-s.ba); // .normalized()
for(int i = 0; i < 3; i++ ){
res(i) = s.vel[i];
res(i + 3) = omega[i];
res(i + 12) = a_inertial[i] + s.gravity[i];
}
return res;
}
Eigen::Matrix<double, 30, 1> get_f_output(state_output &s, const input_ikfom &in)
{
Eigen::Matrix<double, 30, 1> res = Eigen::Matrix<double, 30, 1>::Zero();
vect3 a_inertial = s.rot * s.acc; // .normalized()
for(int i = 0; i < 3; i++ ){
res(i) = s.vel[i];
res(i + 3) = s.omg[i];
res(i + 12) = a_inertial[i] + s.gravity[i];
}
return res;
}
Eigen::Matrix<double, 24, 24> df_dx_input(state_input &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 24> cov = Eigen::Matrix<double, 24, 24>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
vect3 acc_;
in.acc.boxminus(acc_, s.ba);
vect3 omega;
in.gyro.boxminus(omega, s.bg);
cov.template block<3, 3>(12, 3) = -s.rot*MTK::hat(acc_); // .normalized().toRotationMatrix()
cov.template block<3, 3>(12, 18) = -s.rot; //.normalized().toRotationMatrix();
// Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
// Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
// s.S2_Mx(grav_matrix, vec, 21);
cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix;
cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity();
return cov;
}
Eigen::Matrix<double, 30, 30> df_dx_output(state_output &s, const input_ikfom &in)
{
Eigen::Matrix<double, 30, 30> cov = Eigen::Matrix<double, 30, 30>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
cov.template block<3, 3>(12, 3) = -s.rot*MTK::hat(s.acc); // .normalized().toRotationMatrix()
cov.template block<3, 3>(12, 18) = s.rot; //.normalized().toRotationMatrix();
// Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
// Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
// s.S2_Mx(grav_matrix, vec, 21);
cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix;
cov.template block<3, 3>(3, 15) = Eigen::Matrix3d::Identity();
return cov;
}
void h_model_input(state_input &s, Eigen::Matrix3d cov_p, Eigen::Matrix3d cov_R, esekfom::dyn_share_modified<double> &ekfom_data)
{
bool match_in_map = false;
VF(4) pabcd;
pabcd.setZero();
normvec->resize(time_seq[k]);
int effect_num_k = 0;
for (int j = 0; j < time_seq[k]; j++)
{
PointType &point_body_j = feats_down_body->points[idx+j+1];
PointType &point_world_j = feats_down_world->points[idx+j+1];
pointBodyToWorld(&point_body_j, &point_world_j);
V3D p_body = pbody_list[idx+j+1];
double p_norm = p_body.norm();
V3D p_world;
p_world << point_world_j.x, point_world_j.y, point_world_j.z;
{
auto &points_near = Nearest_Points[idx+j+1];
ivox_->GetClosestPoint(point_world_j, points_near, NUM_MATCH_POINTS); //
if ((points_near.size() < NUM_MATCH_POINTS)) // || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5) // 5)
{
point_selected_surf[idx+j+1] = false;
}
else
{
point_selected_surf[idx+j+1] = false;
if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid)
{
float pd2 = fabs(pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3));
// V3D norm_vec;
// M3D Rpf, pf;
// pf = crossmat_list[idx+j+1];
// // pf << SKEW_SYM_MATRX(p_body);
// Rpf = s.rot * pf;
// norm_vec << pabcd(0), pabcd(1), pabcd(2);
// double noise_state = norm_vec.transpose() * (cov_p+Rpf*cov_R*Rpf.transpose()) * norm_vec + sqrt(p_norm) * 0.001;
// // if (p_norm > match_s * pd2 * pd2)
// double epsilon = pd2 / sqrt(noise_state);
// // cout << "check epsilon:" << epsilon << endl;
// double weight = 1.0; // epsilon / sqrt(epsilon * epsilon+1);
// if (epsilon > 1.0)
// {
// weight = sqrt(2 * epsilon - 1) / epsilon;
// pabcd(0) = weight * pabcd(0);
// pabcd(1) = weight * pabcd(1);
// pabcd(2) = weight * pabcd(2);
// pabcd(3) = weight * pabcd(3);
// }
if (p_norm > match_s * pd2 * pd2)
{
point_selected_surf[idx+j+1] = true;
normvec->points[j].x = pabcd(0);
normvec->points[j].y = pabcd(1);
normvec->points[j].z = pabcd(2);
normvec->points[j].intensity = pabcd(3);
effect_num_k ++;
}
}
}
}
}
if (effect_num_k == 0)
{
ekfom_data.valid = false;
return;
}
ekfom_data.M_Noise = laser_point_cov;
ekfom_data.h_x.resize(effect_num_k, 12);
ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12);
ekfom_data.z.resize(effect_num_k);
int m = 0;
for (int j = 0; j < time_seq[k]; j++)
{
// ekfom_data.converge = false;
if(point_selected_surf[idx+j+1])
{
V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z);
if (extrinsic_est_en)
{
V3D p_body = pbody_list[idx+j+1];
M3D p_crossmat, p_imu_crossmat;
p_crossmat << SKEW_SYM_MATRX(p_body);
V3D point_imu = s.offset_R_L_I * p_body + s.offset_T_L_I;
p_imu_crossmat << SKEW_SYM_MATRX(point_imu);
V3D C(s.rot.transpose() * norm_vec);
V3D A(p_imu_crossmat * C);
V3D B(p_crossmat * s.offset_R_L_I.transpose() * C);
ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
}
else
{
M3D point_crossmat = crossmat_list[idx+j+1];
V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized()
V3D A(point_crossmat * C);
ekfom_data.h_x.block<1, 6>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
}
ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;
m++;
}
}
effct_feat_num += effect_num_k;
}
void h_model_output(state_output &s, Eigen::Matrix3d cov_p, Eigen::Matrix3d cov_R, esekfom::dyn_share_modified<double> &ekfom_data)
{
bool match_in_map = false;
VF(4) pabcd;
pabcd.setZero();
normvec->resize(time_seq[k]);
int effect_num_k = 0;
for (int j = 0; j < time_seq[k]; j++)
{
PointType &point_body_j = feats_down_body->points[idx+j+1];
PointType &point_world_j = feats_down_world->points[idx+j+1];
pointBodyToWorld(&point_body_j, &point_world_j);
V3D p_body = pbody_list[idx+j+1];
double p_norm = p_body.norm();
V3D p_world;
p_world << point_world_j.x, point_world_j.y, point_world_j.z;
{
auto &points_near = Nearest_Points[idx+j+1];
ivox_->GetClosestPoint(point_world_j, points_near, NUM_MATCH_POINTS); //
if ((points_near.size() < NUM_MATCH_POINTS)) // || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5)
{
point_selected_surf[idx+j+1] = false;
}
else
{
point_selected_surf[idx+j+1] = false;
if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid)
{
float pd2 = fabs(pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3));
// V3D norm_vec;
// M3D Rpf, pf;
// pf = crossmat_list[idx+j+1];
// // pf << SKEW_SYM_MATRX(p_body);
// Rpf = s.rot * pf;
// norm_vec << pabcd(0), pabcd(1), pabcd(2);
// double noise_state = norm_vec.transpose() * (cov_p+Rpf*cov_R*Rpf.transpose()) * norm_vec + sqrt(p_norm) * 0.001;
// // if (p_norm > match_s * pd2 * pd2)
// double epsilon = pd2 / sqrt(noise_state);
// double weight = 1.0; // epsilon / sqrt(epsilon * epsilon+1);
// if (epsilon > 1.0)
// {
// weight = sqrt(2 * epsilon - 1) / epsilon;
// pabcd(0) = weight * pabcd(0);
// pabcd(1) = weight * pabcd(1);
// pabcd(2) = weight * pabcd(2);
// pabcd(3) = weight * pabcd(3);
// }
if (p_norm > match_s * pd2 * pd2)
{
// point_selected_surf[i] = true;
point_selected_surf[idx+j+1] = true;
normvec->points[j].x = pabcd(0);
normvec->points[j].y = pabcd(1);
normvec->points[j].z = pabcd(2);
normvec->points[j].intensity = pabcd(3);
effect_num_k ++;
}
}
}
}
}
if (effect_num_k == 0)
{
ekfom_data.valid = false;
return;
}
ekfom_data.M_Noise = laser_point_cov;
ekfom_data.h_x.resize(effect_num_k, 12);
ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12);
ekfom_data.z.resize(effect_num_k);
int m = 0;
for (int j = 0; j < time_seq[k]; j++)
{
// ekfom_data.converge = false;
if(point_selected_surf[idx+j+1])
{
V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z);
if (extrinsic_est_en)
{
V3D p_body = pbody_list[idx+j+1];
M3D p_crossmat, p_imu_crossmat;
p_crossmat << SKEW_SYM_MATRX(p_body);
V3D point_imu = s.offset_R_L_I * p_body + s.offset_T_L_I;
p_imu_crossmat << SKEW_SYM_MATRX(point_imu);
V3D C(s.rot.transpose() * norm_vec);
V3D A(p_imu_crossmat * C);
V3D B(p_crossmat * s.offset_R_L_I.transpose() * C);
ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
}
else
{
M3D point_crossmat = crossmat_list[idx+j+1];
V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized()
V3D A(point_crossmat * C);
ekfom_data.h_x.block<1, 6>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
}
ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;
m++;
}
}
effct_feat_num += effect_num_k;
}
void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data)
{
std::memset(ekfom_data.satu_check, false, 6);
ekfom_data.z_IMU.block<3,1>(0, 0) = angvel_avr - s.omg - s.bg;
ekfom_data.z_IMU.block<3,1>(3, 0) = acc_avr * G_m_s2 / acc_norm - s.acc - s.ba;
ekfom_data.R_IMU << imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_acc_cov, imu_meas_acc_cov, imu_meas_acc_cov;
if(check_satu)
{
if(fabs(angvel_avr(0)) >= 0.99 * satu_gyro)
{
ekfom_data.satu_check[0] = true;
ekfom_data.z_IMU(0) = 0.0;
}
if(fabs(angvel_avr(1)) >= 0.99 * satu_gyro)
{
ekfom_data.satu_check[1] = true;
ekfom_data.z_IMU(1) = 0.0;
}
if(fabs(angvel_avr(2)) >= 0.99 * satu_gyro)
{
ekfom_data.satu_check[2] = true;
ekfom_data.z_IMU(2) = 0.0;
}
if(fabs(acc_avr(0)) >= 0.99 * satu_acc)
{
ekfom_data.satu_check[3] = true;
ekfom_data.z_IMU(3) = 0.0;
}
if(fabs(acc_avr(1)) >= 0.99 * satu_acc)
{
ekfom_data.satu_check[4] = true;
ekfom_data.z_IMU(4) = 0.0;
}
if(fabs(acc_avr(2)) >= 0.99 * satu_acc)
{
ekfom_data.satu_check[5] = true;
ekfom_data.z_IMU(5) = 0.0;
}
}
}
void pointBodyToWorld(PointType const * const pi, PointType * const po)
{
V3D p_body(pi->x, pi->y, pi->z);
V3D p_global;
if (extrinsic_est_en)
{
if (!use_imu_as_input)
{
p_global = kf_output.x_.rot * (kf_output.x_.offset_R_L_I * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos;
}
else
{
p_global = kf_input.x_.rot * (kf_input.x_.offset_R_L_I * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos;
}
}
else
{
if (!use_imu_as_input)
{
p_global = kf_output.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; // .normalized()
}
else
{
p_global = kf_input.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; // .normalized()
}
}
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}

61
src/Estimator.h Executable file
View File

@@ -0,0 +1,61 @@
#ifndef Estimator_H
#define Estimator_H
#include "common_lib.h"
#include "parameters.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
// #include <ikd-Tree/ikd_Tree.h>
#include <pcl/io/pcd_io.h>
#include <unordered_set>
extern PointCloudXYZI::Ptr normvec; //(new PointCloudXYZI(100000, 1));
extern std::vector<int> time_seq;
extern PointCloudXYZI::Ptr feats_down_body; //(new PointCloudXYZI());
extern PointCloudXYZI::Ptr feats_down_world; //(new PointCloudXYZI());
extern std::vector<V3D> pbody_list;
extern std::vector<PointVector> Nearest_Points;
extern std::shared_ptr<IVoxType> ivox_; // localmap in ivox
extern std::vector<float> pointSearchSqDis;
extern bool point_selected_surf[100000]; // = {0};
extern std::vector<M3D> crossmat_list;
extern int effct_feat_num;
extern int k;
extern int idx;
extern V3D angvel_avr, acc_avr, acc_avr_norm;
extern int feats_down_size;
// extern std::vector<Eigen::Vector3d> normvec_holder;
extern V3D Lidar_T_wrt_IMU; //(Zero3d);
extern M3D Lidar_R_wrt_IMU; //(Eye3d);
extern input_ikfom input_in;
Eigen::Matrix<double, 24, 24> process_noise_cov_input();
Eigen::Matrix<double, 30, 30> process_noise_cov_output();
//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia
//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
Eigen::Matrix<double, 24, 1> get_f_input(state_input &s, const input_ikfom &in);
Eigen::Matrix<double, 30, 1> get_f_output(state_output &s, const input_ikfom &in);
Eigen::Matrix<double, 24, 24> df_dx_input(state_input &s, const input_ikfom &in);
// Eigen::Matrix<double, 24, 12> df_dw_input(state_input &s, const input_ikfom &in);
Eigen::Matrix<double, 30, 30> df_dx_output(state_output &s, const input_ikfom &in);
// Eigen::Matrix<double, 30, 15> df_dw_output(state_output &s);
void h_model_input(state_input &s, Eigen::Matrix3d cov_p, Eigen::Matrix3d cov_R, esekfom::dyn_share_modified<double> &ekfom_data);
void h_model_output(state_output &s, Eigen::Matrix3d cov_p, Eigen::Matrix3d cov_R, esekfom::dyn_share_modified<double> &ekfom_data);
void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data);
void pointBodyToWorld(PointType const * const pi, PointType * const po);
#endif

136
src/IMU_Processing.cpp Executable file
View File

@@ -0,0 +1,136 @@
#include "IMU_Processing.h"
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
void ImuProcess::set_gyr_cov(const V3D &scaler)
{
cov_gyr_scale = scaler;
}
void ImuProcess::set_acc_cov(const V3D &scaler)
{
cov_vel_scale = scaler;
}
ImuProcess::ImuProcess()
: b_first_frame_(true), imu_need_init_(true)
{
imu_en = true;
init_iter_num = 1;
mean_acc = V3D(0, 0, 0.0);
mean_gyr = V3D(0, 0, 0);
after_imu_init_ = false;
state_cov.setIdentity();
}
ImuProcess::~ImuProcess() {}
void ImuProcess::Reset()
{
ROS_WARN("Reset ImuProcess");
mean_acc = V3D(0, 0, 0.0);
mean_gyr = V3D(0, 0, 0);
imu_need_init_ = true;
init_iter_num = 1;
after_imu_init_ = false;
time_last_scan = 0.0;
}
void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
// V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity;
M3D hat_grav;
hat_grav << 0.0, gravity_(2), -gravity_(1),
-gravity_(2), 0.0, gravity_(0),
gravity_(1), -gravity_(0), 0.0;
double align_norm = (hat_grav * tmp_gravity).norm() / gravity_.norm() / tmp_gravity.norm();
double align_cos = gravity_.transpose() * tmp_gravity;
align_cos = align_cos / gravity_.norm() / tmp_gravity.norm();
if (align_norm < 1e-6)
{
if (align_cos > 1e-6)
{
rot = Eye3d;
}
else
{
rot = -Eye3d;
}
}
else
{
V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos);
rot = Exp(align_angle(0), align_angle(1), align_angle(2));
}
}
void ImuProcess::IMU_init(const MeasureGroup &meas, int &N)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
V3D cur_acc, cur_gyr;
if (b_first_frame_)
{
Reset();
N = 1;
b_first_frame_ = false;
const auto &imu_acc = meas.imu.front()->linear_acceleration;
const auto &gyr_acc = meas.imu.front()->angular_velocity;
mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
}
for (const auto &imu : meas.imu)
{
const auto &imu_acc = imu->linear_acceleration;
const auto &gyr_acc = imu->angular_velocity;
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
mean_acc += (cur_acc - mean_acc) / N;
mean_gyr += (cur_gyr - mean_gyr) / N;
N ++;
}
}
void ImuProcess::Process(const MeasureGroup &meas, PointCloudXYZI::Ptr cur_pcl_un_)
{
if (imu_en)
{
if(meas.imu.empty()) return;
if (imu_need_init_)
{
{
/// The very first lidar frame
IMU_init(meas, init_iter_num);
imu_need_init_ = true;
if (init_iter_num > MAX_INI_COUNT)
{
ROS_INFO("IMU Initializing: %.1f %%", 100.0);
imu_need_init_ = false;
*cur_pcl_un_ = *(meas.lidar);
}
// *cur_pcl_un_ = *(meas.lidar);
}
return;
}
if (!after_imu_init_) after_imu_init_ = true;
*cur_pcl_un_ = *(meas.lidar);
return;
}
else
{
*cur_pcl_un_ = *(meas.lidar);
return;
}
}

59
src/IMU_Processing.h Executable file
View File

@@ -0,0 +1,59 @@
#pragma once
#include <cmath>
#include <math.h>
// #include <deque>
// #include <mutex>
// #include <thread>
#include <csignal>
#include <ros/ros.h>
// #include <so3_math.h>
#include <Eigen/Eigen>
// #include "Estimator.h"
#include <common_lib.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <nav_msgs/Odometry.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
/// *************Preconfiguration
#define MAX_INI_COUNT (100)
const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);};
/// *************IMU Process and undistortion
class ImuProcess
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ImuProcess();
~ImuProcess();
void Reset();
void Process(const MeasureGroup &meas, PointCloudXYZI::Ptr pcl_un_);
void set_gyr_cov(const V3D &scaler);
void set_acc_cov(const V3D &scaler);
void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot);
MD(12, 12) state_cov = MD(12, 12)::Identity();
int lidar_type;
V3D gravity_;
bool imu_en;
V3D mean_acc;
bool imu_need_init_ = true;
bool after_imu_init_ = false;
bool b_first_frame_ = true;
double time_last_scan = 0.0;
V3D cov_gyr_scale = V3D(0.0001, 0.0001, 0.0001);
V3D cov_vel_scale = V3D(0.0001, 0.0001, 0.0001);
private:
void IMU_init(const MeasureGroup &meas, int &N);
V3D mean_gyr;
int init_iter_num = 1;
};

1070
src/laserMapping.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

348
src/li_initialization.cpp Executable file
View File

@@ -0,0 +1,348 @@
#include "li_initialization.h"
bool data_accum_finished = false, data_accum_start = false, online_calib_finish = false, refine_print = false;
int frame_num_init = 0;
double time_lag_IMU_wtr_lidar = 0.0, move_start_time = 0.0, online_calib_starts_time = 0.0; //, mean_acc_norm = 9.81;
double imu_first_time = 0.0;
bool lose_lid = false;
double timediff_imu_wrt_lidar = 0.0;
bool timediff_set_flg = false;
V3D gravity_lio = V3D::Zero();
mutex mtx_buffer;
sensor_msgs::Imu imu_last, imu_next;
// sensor_msgs::Imu::ConstPtr imu_last_ptr;
PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI());
double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN];
condition_variable sig_buffer;
int scan_count = 0;
int frame_ct = 0, wait_num = 0;
std::mutex m_time;
bool lidar_pushed = false, imu_pushed = false;
std::deque<PointCloudXYZI::Ptr> lidar_buffer;
std::deque<double> time_buffer;
std::deque<sensor_msgs::Imu::Ptr> imu_deque;
void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
// mtx_buffer.lock();
scan_count ++;
double preprocess_start_time = omp_get_wtime();
if (msg->header.stamp.toSec() < last_timestamp_lidar)
{
ROS_ERROR("lidar loop back, clear buffer");
// lidar_buffer.shrink_to_fit();
// mtx_buffer.unlock();
// sig_buffer.notify_all();
return;
}
last_timestamp_lidar = msg->header.stamp.toSec();
// printf("check lidar time %f\n", last_timestamp_lidar);
// if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_deque.empty()) {
// timediff_set_flg = true;
// timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar;
// printf("Self sync IMU and LiDAR, HARD time lag is %.10lf \n \n", timediff_imu_wrt_lidar);
// }
if ((lidar_type == VELO16 || lidar_type == OUST64 || lidar_type == HESAIxt32) && cut_frame_init) {
deque<PointCloudXYZI::Ptr> ptr;
deque<double> timestamp_lidar;
p_pre->process_cut_frame_pcl2(msg, ptr, timestamp_lidar, cut_frame_num, scan_count);
while (!ptr.empty() && !timestamp_lidar.empty()) {
lidar_buffer.push_back(ptr.front());
ptr.pop_front();
time_buffer.push_back(timestamp_lidar.front() / double(1000));//unit:s
timestamp_lidar.pop_front();
}
}
else
{
PointCloudXYZI::Ptr ptr(new PointCloudXYZI(20000,1));
p_pre->process(msg, ptr);
if (con_frame)
{
if (frame_ct == 0)
{
time_con = last_timestamp_lidar; //msg->header.stamp.toSec();
}
if (frame_ct < 10)
{
for (int i = 0; i < ptr->size(); i++)
{
ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000;
ptr_con->push_back(ptr->points[i]);
}
frame_ct ++;
}
else
{
PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI(10000,1));
// cout << "ptr div num:" << ptr_div->size() << endl;
*ptr_con_i = *ptr_con;
lidar_buffer.push_back(ptr_con_i);
double time_con_i = time_con;
time_buffer.push_back(time_con_i);
ptr_con->clear();
frame_ct = 0;
}
}
else
{
lidar_buffer.emplace_back(ptr);
time_buffer.emplace_back(msg->header.stamp.toSec());
}
}
s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
// mtx_buffer.unlock();
// sig_buffer.notify_all();
}
void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
// mtx_buffer.lock();
double preprocess_start_time = omp_get_wtime();
scan_count ++;
if (msg->header.stamp.toSec() < last_timestamp_lidar)
{
ROS_ERROR("lidar loop back, clear buffer");
// mtx_buffer.unlock();
// sig_buffer.notify_all();
return;
// lidar_buffer.shrink_to_fit();
}
last_timestamp_lidar = msg->header.stamp.toSec();
// if (abs(last_timestamp_imu - last_timestamp_lidar) > 1.0 && !timediff_set_flg && !imu_deque.empty()) {
// timediff_set_flg = true;
// timediff_imu_wrt_lidar = last_timestamp_imu - last_timestamp_lidar;
// printf("Self sync IMU and LiDAR, HARD time lag is %.10lf \n \n", timediff_imu_wrt_lidar);
// }
if (cut_frame_init) {
deque<PointCloudXYZI::Ptr> ptr;
deque<double> timestamp_lidar;
p_pre->process_cut_frame_livox(msg, ptr, timestamp_lidar, cut_frame_num, scan_count);
while (!ptr.empty() && !timestamp_lidar.empty()) {
lidar_buffer.push_back(ptr.front());
ptr.pop_front();
time_buffer.push_back(timestamp_lidar.front() / double(1000));//unit:s
timestamp_lidar.pop_front();
}
}
else
{
PointCloudXYZI::Ptr ptr(new PointCloudXYZI(10000,1));
p_pre->process(msg, ptr);
if (con_frame)
{
if (frame_ct == 0)
{
time_con = last_timestamp_lidar; //msg->header.stamp.toSec();
}
if (frame_ct < 10)
{
for (int i = 0; i < ptr->size(); i++)
{
ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000;
ptr_con->push_back(ptr->points[i]);
}
frame_ct ++;
}
else
{
PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI(10000,1));
// cout << "ptr div num:" << ptr_div->size() << endl;
*ptr_con_i = *ptr_con;
double time_con_i = time_con;
lidar_buffer.push_back(ptr_con_i);
time_buffer.push_back(time_con_i);
ptr_con->clear();
frame_ct = 0;
}
}
else
{
lidar_buffer.emplace_back(ptr);
time_buffer.emplace_back(msg->header.stamp.toSec());
}
}
s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
// mtx_buffer.unlock();
// sig_buffer.notify_all();
}
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
{
// mtx_buffer.lock();
// publish_count ++;
sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
msg->header.stamp = ros::Time().fromSec(msg->header.stamp.toSec() - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar);
double timestamp = msg->header.stamp.toSec();
// printf("time_diff%f, %f, %f\n", last_timestamp_imu - timestamp, last_timestamp_imu, timestamp);
if (timestamp < last_timestamp_imu)
{
ROS_ERROR("imu loop back, clear deque");
// imu_deque.shrink_to_fit();
// cout << "check time:" << timestamp << ";" << last_timestamp_imu << endl;
// printf("time_diff%f, %f, %f\n", last_timestamp_imu - timestamp, last_timestamp_imu, timestamp);
// mtx_buffer.unlock();
// sig_buffer.notify_all();
return;
}
imu_deque.emplace_back(msg);
last_timestamp_imu = timestamp;
// mtx_buffer.unlock();
// sig_buffer.notify_all();
}
bool sync_packages(MeasureGroup &meas)
{
{
if (!imu_en)
{
if (!lidar_buffer.empty())
{
if (!lidar_pushed)
{
meas.lidar = lidar_buffer.front();
meas.lidar_beg_time = time_buffer.front();
lose_lid = false;
if(meas.lidar->points.size() < 1)
{
cout << "lose lidar" << std::endl;
// return false;
lose_lid = true;
}
else
{
double end_time = meas.lidar->points.back().curvature;
for (auto pt: meas.lidar->points)
{
if (pt.curvature > end_time)
{
end_time = pt.curvature;
}
}
lidar_end_time = meas.lidar_beg_time + end_time / double(1000);
meas.lidar_last_time = lidar_end_time;
}
lidar_pushed = true;
}
time_buffer.pop_front();
lidar_buffer.pop_front();
lidar_pushed = false;
if (!lose_lid)
{
return true;
}
else
{
return false;
}
}
return false;
}
if (lidar_buffer.empty() || imu_deque.empty())
{
return false;
}
/*** push a lidar scan ***/
if(!lidar_pushed)
{
lose_lid = false;
meas.lidar = lidar_buffer.front();
meas.lidar_beg_time = time_buffer.front();
if(meas.lidar->points.size() < 1)
{
cout << "lose lidar" << endl;
lose_lid = true;
// lidar_buffer.pop_front();
// time_buffer.pop_front();
// return false;
}
else
{
double end_time = meas.lidar->points.back().curvature;
for (auto pt: meas.lidar->points)
{
if (pt.curvature > end_time)
{
end_time = pt.curvature;
}
}
lidar_end_time = meas.lidar_beg_time + end_time / double(1000);
// cout << "check time lidar:" << end_time << endl;
meas.lidar_last_time = lidar_end_time;
}
lidar_pushed = true;
}
if (!lose_lid && (last_timestamp_imu < lidar_end_time))
{
return false;
}
if (lose_lid && last_timestamp_imu < meas.lidar_beg_time + lidar_time_inte)
{
return false;
}
if (!lose_lid && !imu_pushed)
{
/*** push imu data, and pop from imu buffer ***/
if (p_imu->imu_need_init_)
{
double imu_time = imu_deque.front()->header.stamp.toSec();
imu_next = *(imu_deque.front());
meas.imu.shrink_to_fit();
while (imu_time < lidar_end_time)
{
meas.imu.emplace_back(imu_deque.front());
imu_last = imu_next;
imu_deque.pop_front();
if(imu_deque.empty()) break;
imu_time = imu_deque.front()->header.stamp.toSec(); // can be changed
imu_next = *(imu_deque.front());
}
}
imu_pushed = true;
}
if (lose_lid && !imu_pushed)
{
/*** push imu data, and pop from imu buffer ***/
if (p_imu->imu_need_init_)
{
double imu_time = imu_deque.front()->header.stamp.toSec();
meas.imu.shrink_to_fit();
imu_next = *(imu_deque.front());
while (imu_time < meas.lidar_beg_time + lidar_time_inte)
{
meas.imu.emplace_back(imu_deque.front());
imu_last = imu_next;
imu_deque.pop_front();
if(imu_deque.empty()) break;
imu_time = imu_deque.front()->header.stamp.toSec(); // can be changed
imu_next = *(imu_deque.front());
}
}
imu_pushed = true;
}
lidar_buffer.pop_front();
time_buffer.pop_front();
lidar_pushed = false;
imu_pushed = false;
return true;
}
}

36
src/li_initialization.h Executable file
View File

@@ -0,0 +1,36 @@
#pragma once
#include <common_lib.h>
#include "Estimator.h"
#define MAXN (720000)
extern bool data_accum_finished, data_accum_start, online_calib_finish, refine_print;
extern int frame_num_init;
extern double time_lag_IMU_wtr_lidar, move_start_time, online_calib_starts_time; //, mean_acc_norm = 9.81;
extern double timediff_imu_wrt_lidar;
extern bool timediff_set_flg;
extern V3D gravity_lio;
extern mutex mtx_buffer;
extern condition_variable sig_buffer;
extern int scan_count;
extern int frame_ct, wait_num;
extern std::deque<PointCloudXYZI::Ptr> lidar_buffer;
extern std::deque<double> time_buffer;
extern std::deque<sensor_msgs::Imu::Ptr> imu_deque;
extern std::mutex m_time;
extern bool lidar_pushed, imu_pushed;
extern double imu_first_time;
extern bool lose_lid;
extern sensor_msgs::Imu imu_last, imu_next;
extern PointCloudXYZI::Ptr ptr_con;
extern double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN];
// extern sensor_msgs::Imu::ConstPtr imu_last_ptr;
void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg);
void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in);
bool sync_packages(MeasureGroup &meas);
// #endif

170
src/parameters.cpp Executable file
View File

@@ -0,0 +1,170 @@
#include "parameters.h"
bool is_first_frame = true;
double lidar_end_time = 0.0, first_lidar_time = 0.0, time_con = 0.0;
double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0;
int pcd_index = 0;
IVoxType::Options ivox_options_;
int ivox_nearby_type = 6;
std::vector<double> extrinT(3, 0.0);
std::vector<double> extrinR(9, 0.0);
state_input state_in;
state_output state_out;
std::string lid_topic, imu_topic;
bool prop_at_freq_of_imu = true, check_satu = true, con_frame = false, cut_frame = false;
bool use_imu_as_input = false, space_down_sample = true, publish_odometry_without_downsample = false;
int init_map_size = 10, con_frame_num = 1;
double match_s = 81, satu_acc, satu_gyro, cut_frame_time_interval = 0.1;
float plane_thr = 0.1f;
double filter_size_surf_min = 0.5, filter_size_map_min = 0.5, fov_deg = 180;
// double cube_len = 2000;
float DET_RANGE = 450;
bool imu_en = true;
double imu_time_inte = 0.005;
double laser_point_cov = 0.01, acc_norm;
double vel_cov, acc_cov_input, gyr_cov_input;
double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov;
double imu_meas_acc_cov, imu_meas_omg_cov;
int lidar_type, pcd_save_interval;
std::vector<double> gravity_init, gravity;
bool runtime_pos_log, pcd_save_en, path_en, extrinsic_est_en = true;
bool scan_pub_en, scan_body_pub_en;
shared_ptr<Preprocess> p_pre;
shared_ptr<ImuProcess> p_imu;
double time_update_last = 0.0, time_current = 0.0, time_predict_last_const = 0.0, t_last = 0.0;
double time_diff_lidar_to_imu = 0.0;
double lidar_time_inte = 0.1, first_imu_time = 0.0;
int cut_frame_num = 1, orig_odom_freq = 10;
double online_refine_time = 20.0; //unit: s
bool cut_frame_init = true;
MeasureGroup Measures;
ofstream fout_out, fout_imu_pbp, fout_rtk;
void readParameters(ros::NodeHandle &nh)
{
p_pre.reset(new Preprocess());
p_imu.reset(new ImuProcess());
nh.param<bool>("prop_at_freq_of_imu", prop_at_freq_of_imu, 1);
nh.param<bool>("use_imu_as_input", use_imu_as_input, 0);
nh.param<bool>("check_satu", check_satu, 1);
nh.param<int>("init_map_size", init_map_size, 100);
nh.param<bool>("space_down_sample", space_down_sample, 1);
nh.param<double>("mapping/satu_acc",satu_acc,3.0);
nh.param<double>("mapping/satu_gyro",satu_gyro,35.0);
nh.param<double>("mapping/acc_norm",acc_norm,1.0);
nh.param<float>("mapping/plane_thr", plane_thr, 0.05f);
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
nh.param<std::string>("common/lid_topic",lid_topic,"/livox/lidar");
nh.param<std::string>("common/imu_topic", imu_topic,"/livox/imu");
nh.param<bool>("common/con_frame",con_frame,false);
nh.param<int>("common/con_frame_num",con_frame_num,1);
nh.param<bool>("common/cut_frame",cut_frame,false);
nh.param<double>("common/cut_frame_time_interval",cut_frame_time_interval,0.1);
nh.param<double>("common/time_diff_lidar_to_imu",time_diff_lidar_to_imu,0.0);
nh.param<double>("filter_size_surf",filter_size_surf_min,0.5);
nh.param<double>("filter_size_map",filter_size_map_min,0.5);
// nh.param<double>("cube_side_length",cube_len,2000);
nh.param<float>("mapping/det_range",DET_RANGE,300.f);
nh.param<double>("mapping/fov_degree",fov_deg,180);
nh.param<bool>("mapping/imu_en",imu_en,true);
nh.param<bool>("mapping/extrinsic_est_en",extrinsic_est_en,true);
nh.param<double>("mapping/imu_time_inte",imu_time_inte,0.005);
nh.param<double>("mapping/lidar_meas_cov",laser_point_cov,0.1);
nh.param<double>("mapping/acc_cov_input",acc_cov_input,0.1);
nh.param<double>("mapping/vel_cov",vel_cov,20);
nh.param<double>("mapping/gyr_cov_input",gyr_cov_input,0.1);
nh.param<double>("mapping/gyr_cov_output",gyr_cov_output,0.1);
nh.param<double>("mapping/acc_cov_output",acc_cov_output,0.1);
nh.param<double>("mapping/b_gyr_cov",b_gyr_cov,0.0001);
nh.param<double>("mapping/b_acc_cov",b_acc_cov,0.0001);
nh.param<double>("mapping/imu_meas_acc_cov",imu_meas_acc_cov,0.1);
nh.param<double>("mapping/imu_meas_omg_cov",imu_meas_omg_cov,0.1);
nh.param<double>("preprocess/blind", p_pre->blind, 1.0);
nh.param<int>("preprocess/lidar_type", lidar_type, 1);
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);
nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, 1);
nh.param<double>("mapping/match_s", match_s, 81);
nh.param<std::vector<double>>("mapping/gravity", gravity, std::vector<double>());
nh.param<std::vector<double>>("mapping/gravity_init", gravity_init, std::vector<double>());
nh.param<std::vector<double>>("mapping/extrinsic_T", extrinT, std::vector<double>());
nh.param<std::vector<double>>("mapping/extrinsic_R", extrinR, std::vector<double>());
nh.param<bool>("odometry/publish_odometry_without_downsample", publish_odometry_without_downsample, false);
nh.param<bool>("publish/path_en",path_en, true);
nh.param<bool>("publish/scan_publish_en",scan_pub_en,1);
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en,1);
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
nh.param<double>("mapping/lidar_time_inte",lidar_time_inte,0.1);
nh.param<double>("mapping/lidar_meas_cov",laser_point_cov,0.1);
nh.param<float>("mapping/ivox_grid_resolution", ivox_options_.resolution_, 0.2);
nh.param<int>("ivox_nearby_type", ivox_nearby_type, 18);
if (ivox_nearby_type == 0) {
ivox_options_.nearby_type_ = IVoxType::NearbyType::CENTER;
} else if (ivox_nearby_type == 6) {
ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY6;
} else if (ivox_nearby_type == 18) {
ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
} else if (ivox_nearby_type == 26) {
ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY26;
} else {
// LOG(WARNING) << "unknown ivox_nearby_type, use NEARBY18";
ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
}
p_imu->gravity_ << VEC_FROM_ARRAY(gravity);
}
Eigen::Matrix<double, 3, 1> SO3ToEuler(const SO3 &rot)
{
double sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
bool singular = sy < 1e-6;
double x, y, z;
if(!singular)
{
x = atan2(rot(2, 1), rot(2, 2));
y = atan2(-rot(2, 0), sy);
z = atan2(rot(1, 0), rot(0, 0));
}
else
{
x = atan2(-rot(1, 2), rot(1, 1));
y = atan2(-rot(2, 0), sy);
z = 0;
}
Eigen::Matrix<double, 3, 1> ang(x, y, z);
return ang;
}
void open_file()
{
fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out);
fout_imu_pbp.open(DEBUG_FILE_DIR("imu_pbp.txt"),ios::out);
if (fout_out && fout_imu_pbp)
cout << "~~~~"<<ROOT_DIR<<" file opened" << endl;
else
cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl;
}
void reset_cov(Eigen::Matrix<double, 24, 24> & P_init)
{
P_init = MD(24, 24)::Identity() * 0.1;
P_init.block<3, 3>(21, 21) = MD(3,3)::Identity() * 0.0001;
P_init.block<6, 6>(15, 15) = MD(6,6)::Identity() * 0.001;
}
void reset_cov_output(Eigen::Matrix<double, 30, 30> & P_init_output)
{
P_init_output = MD(30, 30)::Identity() * 0.01;
P_init_output.block<3, 3>(21, 21) = MD(3,3)::Identity() * 0.0001;
// P_init_output.block<6, 6>(6, 6) = MD(6,6)::Identity() * 0.0001;
P_init_output.block<6, 6>(24, 24) = MD(6,6)::Identity() * 0.001;
}

83
src/parameters.h Executable file
View File

@@ -0,0 +1,83 @@
// #ifndef PARAM_H
// #define PARAM_H
#pragma once
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <cstring>
#include "preprocess.h"
#include "IMU_Processing.h"
#include <sensor_msgs/NavSatFix.h>
#include <livox_ros_driver/CustomMsg.h>
#include <sensor_msgs/PointCloud2.h>
#include <mutex>
#include <omp.h>
#include <math.h>
#include <thread>
#include <fstream>
#include <csignal>
#include <unistd.h>
#include <ivox/ivox3d.h>
#include <Python.h>
#include <condition_variable>
#include <sensor_msgs/Imu.h>
#include <pcl/common/transforms.h>
#include <geometry_msgs/Vector3.h>
// #define IVOX_NODE_TYPE_PHC
#ifdef IVOX_NODE_TYPE_PHC
using IVoxType = faster_lio::IVox<3, faster_lio::IVoxNodeType::PHC, PointType>;
#else
using IVoxType = faster_lio::IVox<3, faster_lio::IVoxNodeType::DEFAULT, PointType>;
#endif
extern bool is_first_frame;
extern double lidar_end_time, first_lidar_time, time_con;
extern double last_timestamp_lidar, last_timestamp_imu;
extern int pcd_index;
extern IVoxType::Options ivox_options_;
extern int ivox_nearby_type;
extern state_input state_in;
extern state_output state_out;
extern std::string lid_topic, imu_topic;
extern bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame;
extern bool use_imu_as_input, space_down_sample;
extern bool extrinsic_est_en, publish_odometry_without_downsample;
extern int init_map_size, con_frame_num;
extern double match_s, satu_acc, satu_gyro, cut_frame_time_interval;
extern float plane_thr;
extern double filter_size_surf_min, filter_size_map_min, fov_deg;
// extern double cube_len;
extern float DET_RANGE;
extern bool imu_en;
extern double imu_time_inte;
extern double laser_point_cov, acc_norm;
extern double acc_cov_input, gyr_cov_input, vel_cov;
extern double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov;
extern double imu_meas_acc_cov, imu_meas_omg_cov;
extern int lidar_type, pcd_save_interval;
extern std::vector<double> gravity_init, gravity;
extern bool runtime_pos_log, pcd_save_en, path_en;
extern bool scan_pub_en, scan_body_pub_en;
extern shared_ptr<Preprocess> p_pre;
extern shared_ptr<ImuProcess> p_imu;
extern bool is_first_frame;
extern std::vector<double> extrinT;
extern std::vector<double> extrinR;
extern double time_diff_lidar_to_imu;
extern double lidar_time_inte, first_imu_time;
extern int cut_frame_num, orig_odom_freq;
extern double online_refine_time; //unit: s
extern bool cut_frame_init;
extern double time_update_last, time_current, time_predict_last_const, t_last;
extern MeasureGroup Measures;
extern ofstream fout_out, fout_imu_pbp, fout_rtk;
void readParameters(ros::NodeHandle &n);
void open_file();
Eigen::Matrix<double, 3, 1> SO3ToEuler(const SO3 &orient);
void reset_cov(Eigen::Matrix<double, 24, 24> & P_init);
void reset_cov_output(Eigen::Matrix<double, 30, 30> & P_init_output);

1026
src/preprocess.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

149
src/preprocess.h Executable file
View File

@@ -0,0 +1,149 @@
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomMsg.h>
using namespace std;
#define IS_VALID(a) ((abs(a)>1e8) ? true : false)
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
enum LID_TYPE{AVIA = 1, VELO16, OUST64, HESAIxt32}; //{1, 2, 3, 4}
enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3};
enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
enum Surround{Prev, Next};
enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
const bool time_list_cut_frame(PointType &x, PointType &y);
struct orgtype
{
double range;
double dista;
double angle[2];
double intersect;
E_jump edj[2];
Feature ftype;
orgtype()
{
range = 0;
edj[Prev] = Nr_nor;
edj[Next] = Nr_nor;
ftype = Nor;
intersect = 2;
}
};
namespace velodyne_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
float time;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(float, time, time)
(std::uint16_t, ring, ring)
)
namespace hesai_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
double timestamp;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(double, timestamp, timestamp)
(std::uint16_t, ring, ring)
)
namespace ouster_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t ambient;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ouster_ros
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
// use std::uint32_t to avoid conflicting with pcl::uint32_t
(std::uint32_t, t, t)
(std::uint16_t, reflectivity, reflectivity)
(std::uint8_t, ring, ring)
(std::uint16_t, ambient, ambient)
(std::uint32_t, range, range)
)
class Preprocess
{
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Preprocess();
~Preprocess();
void process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::PointCloud2::ConstPtr pointcloud;
PointCloudXYZI pl_full, pl_corn, pl_surf;
PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
vector<orgtype> typess[128]; //maximum 128 line lidar
float time_unit_scale;
int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
double blind;
bool given_offset_time;
ros::Publisher pub_full, pub_surf, pub_corn;
private:
void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
int group_size;
double disA, disB, inf_bound;
double limit_maxmid, limit_midmin, limit_maxmin;
double p2l_ratio;
double jump_up_limit, jump_down_limit;
double cos160;
double edgea, edgeb;
double smallp_intersect, smallp_ratio;
double vx, vy, vz;
};