diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..979512c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 2.8.3) +project(point_lio_unilidar) + +SET(CMAKE_BUILD_TYPE "Debug") + +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS "-std=c++14 -O3" ) +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") + +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") +message("DROOT_DIR = ${DROOT_DIR}") + +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 +) + +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime + DEPENDS EIGEN3 PCL + INCLUDE_DIRS +) + +add_executable(pointlio_mapping + src/laserMapping.cpp + include/ikd-Tree/ikd_Tree.cpp + src/parameters.cpp + src/preprocess.cpp + src/Estimator.cpp +) +target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) +target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) \ No newline at end of file diff --git a/Log/guide.md b/Log/guide.md new file mode 100755 index 0000000..883b3d4 --- /dev/null +++ b/Log/guide.md @@ -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. diff --git a/Log/imu.txt b/Log/imu.txt new file mode 100755 index 0000000..e69de29 diff --git a/Log/imu_pbp.txt b/Log/imu_pbp.txt new file mode 100644 index 0000000..e69de29 diff --git a/Log/mat_out.txt b/Log/mat_out.txt new file mode 100644 index 0000000..e69de29 diff --git a/Log/plot.py b/Log/plot.py new file mode 100755 index 0000000..b5d1c1a --- /dev/null +++ b/Log/plot.py @@ -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() diff --git a/Log/plot_imu.py b/Log/plot_imu.py new file mode 100755 index 0000000..0177273 --- /dev/null +++ b/Log/plot_imu.py @@ -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() diff --git a/Log/plot_out.py b/Log/plot_out.py new file mode 100755 index 0000000..8dd80cb --- /dev/null +++ b/Log/plot_out.py @@ -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() diff --git a/Log/pos_log.txt b/Log/pos_log.txt new file mode 100644 index 0000000..e69de29 diff --git a/README.md b/README.md index a9271b8..cbe1e09 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,145 @@ # point_lio_unilidar -An open-source robust and high-bandwidth lidar inertial odometry for Unitree L1 LiDAR. + +## Introduction + +This repository adapts the state-of-the-art lidar inertial odometry algorithm, `Point-LIO`, for use with our lidar, `Unitree Lidar L1`. + +`Unitree Lidar L1` is a low-cost large-FOV 3D lidar, which possesses these features: +- large field of view (360° × 90°) +- non-repetitive scanning +- low cost (only $349) +- suitable for applications in low-speed mobile robots + +If you want to learn more about our lidar, you can refer to the official website for details. +- + + +`Point-LIO` is a robust and high-bandwidth lidar inertial odometry (LIO) with the capability to provide accurate, high-frequency odometry and reliable mapping under severe vibrations and aggressive motions. If you need further information about the `Point-LIO` algorithm, you can refer to their official website and paper: +- +- [Point‐LIO: Robust High‐Bandwidth Light Detection and Ranging Inertial Odometry](https://onlinelibrary.wiley.com/doi/epdf/10.1002/aisy.202200459) + +![demo](./doc/demo.png) + +## Prerequisites + +### Ubuntu and ROS +We tested our code on Ubuntu20.04 with [ROS noetic](http://wiki.ros.org/noetic/Installation/Ubuntu). Ubuntu18.04 and lower versions have problems of environments to support the Point-LIO, try to avoid using Point-LIO in those systems. + +You can refer to the official website to install ROS noetic: +- + +Additional ROS package is required: +``` +sudo apt-get install ros-xxx-pcl-conversions +``` + +### 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 +``` + +### unitree_lidar_ros + +You should download and build [unitree_lidar_ros](https://github.com/unitreerobotics/unilidar_sdk/tree/main/unitree_lidar_ros/src/unitree_lidar_ros) follwing these steps: + +``` +git clone https://github.com/unitreerobotics/unilidar_sdk.git + +cd unilidar_sdk/unitree_lidar_ros + +catkin_make +``` + + +## Build + +Clone this repository and run `catkin_make`: + +``` +mkdir -p catkin_point_lio_unilidar/src + +cd catkin_point_lio_unilidar/src + +git clone https://github.com/unitreerobotics/point_lio_unilidar.git + +cd ../.. + +catkin_make +``` + + +## Run + +### Run with Unilidar + +Firstly, you should connect our lidar to your PC serial port, and supply power for the lidar with a 12V charger. + +Besides, to ensure proper initialization of the IMU, it is advisable to keep the lidar in a stationary state during the initial few seconds of algorithm execution. + +Run `unilidar`: +``` +cd unilidar_sdk/unitree_lidar_ros + +source devel/setup.bash + +roslaunch unitree_lidar_ros run.launch +``` + +Run `Point-LIO`: +``` +cd catkin_unilidar_point_lio + +source devel/setup.bash + +roslaunch point_lio_unilidar mapping_unilidar.launch +``` + + +After completion of the run, all cached pointcloud map will be saved to the following path: +``` +catkin_point_lio_unilidar/src/point_lio_unilidar/PCD/scans.pcd +``` + +You can use the `pcl_viewer` tool to view this pcd file: +``` +pcl_viewer scans.pcd +``` + +### Run with dataset + +If you don't have our lidar for now, you can download our dataset recorded with our lidar and run testify this algorithm with it. +The download address is here: +- [unilidar-2023-09-22-12-42-04.bag - 百度云](https://pan.baidu.com/s/1PD0e9R5Q9gxHM966nurkDQ) --> 提取码: ge3c + + +Run `Point-LIO`: +``` +cd catkin_point_lio_unilidar + +source devel/setup.bash + +roslaunch point_lio_unilidar mapping_unilidar.launch +``` + +Play the dataset you downloaded: +``` +rosbag play unilidar-2023-09-22-12-42-04.bag +``` + + +After completion of the run, all cached pointcloud map will be saved to the following path: +``` +catkin_point_lio_unilidar/src/point_lio_unilidarPCD/scans.pcd +``` + +You can use the `pcl_viewer` tool to view this pcd file: +``` +pcl_viewer scans.pcd +``` + +## Version History + +### v1.0.0 (2023.09.22) +- Adapt `Point-LIO` for `Unitree Lidar L1` +- Upload codes and dataset \ No newline at end of file diff --git a/config/avia.yaml b/config/avia.yaml new file mode 100755 index 0000000..68d125e --- /dev/null +++ b/config/avia.yaml @@ -0,0 +1,59 @@ +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_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +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: 1.0 # 剔除半径位于此范围内的点 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.005 # = 1 / frequency of IMU + satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units + satu_gyro: 17.5 # the saturation value of IMU's angular velocity. not related to the units + # default: 35 rad/s + # for running `PULSAR.bag`: set it to 17.5 rad/s + 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.001 # 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 + fov_degree: 90 + det_range: 450.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state + 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. \ No newline at end of file diff --git a/config/horizon.yaml b/config/horizon.yaml new file mode 100755 index 0000000..070bf90 --- /dev/null +++ b/config/horizon.yaml @@ -0,0 +1,57 @@ +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_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme), + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +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 + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.005 # = 1 / frequency of IMU + 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 + fov_degree: 100 + det_range: 260.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, 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. \ No newline at end of file diff --git a/config/ouster64.yaml b/config/ouster64.yaml new file mode 100755 index 0000000..2ba04b4 --- /dev/null +++ b/config/ouster64.yaml @@ -0,0 +1,57 @@ +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_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +preprocess: + lidar_type: 3 # 2 #velodyne # 1 Livox Avia LiDAR + scan_line: 32 # 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: 0.20 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.01 # = 1 / frequency of IMU + 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.1 # 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 # 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 + fov_degree: 180 + det_range: 150.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, 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. \ No newline at end of file diff --git a/config/unilidar.yaml b/config/unilidar.yaml new file mode 100755 index 0000000..f3ff469 --- /dev/null +++ b/config/unilidar.yaml @@ -0,0 +1,59 @@ +common: + lid_topic: "/unilidar/cloud" + imu_topic: "/unilidar/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_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +preprocess: + lidar_type: 5 + scan_line: 18 + timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 0.5 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.004 # = 1 / frequency of IMU + 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 + fov_degree: 180 + det_range: 100.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state + + # transform from imu to lidar + extrinsic_T: [ 0.007698, 0.014655, -0.00667] # ulhk # [-0.5, 1.4, 1.5] # utbm + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] # ulhk 4 utbm 3 + +odometry: + publish_odometry_without_downsample: enable + +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: true # save map to pcd file + 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. \ No newline at end of file diff --git a/config/velody16.yaml b/config/velody16.yaml new file mode 100755 index 0000000..ed59f00 --- /dev/null +++ b/config/velody16.yaml @@ -0,0 +1,63 @@ +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_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +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 + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.01 # = 1 / frequency of IMU + 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 + fov_degree: 180 + det_range: 100.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, 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. \ No newline at end of file diff --git a/doc/demo.png b/doc/demo.png new file mode 100644 index 0000000..4b65177 Binary files /dev/null and b/doc/demo.png differ diff --git a/include/FOV_Checker/FOV_Checker.cpp b/include/FOV_Checker/FOV_Checker.cpp new file mode 100755 index 0000000..69032bf --- /dev/null +++ b/include/FOV_Checker/FOV_Checker.cpp @@ -0,0 +1,472 @@ +#include "FOV_Checker.h" + +FOV_Checker::FOV_Checker(){ + // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","w"); + // fprintf(fp,"cur_pose_x,cur_pose_y,cur_pose_z,axis_x,axis_y,axis_z,theta,depth\n"); + // fclose(fp); +} + +FOV_Checker::~FOV_Checker(){ + +} + +void FOV_Checker::Set_Env(BoxPointType env_param){ + env = env_param; +} + +void FOV_Checker::Set_BoxLength(double box_len_param){ + box_length = box_len_param; +} + +void round_v3d(Eigen::Vector3d &vec, int decimal){ + double tmp; + int t; + for (int i = 0; i < 3; i++){ + t = pow(10,decimal); + tmp = round(vec(i)*t); + vec(i) = tmp/t; + } + return; +} + +void FOV_Checker::check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector &boxes){ + round_v3d(cur_pose,4); + round_v3d(axis,3); + axis = axis/axis.norm(); + // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","a"); + // fprintf(fp,"%f,%f,%f,%f,%f,%f,%0.4f,%0.1f,",cur_pose(0),cur_pose(1),cur_pose(2),axis(0),axis(1),axis(2),theta,depth); + // fclose(fp); + // cout << "cur_pose: " << cur_pose.transpose() << endl; + // cout<< "axis: " << axis.transpose() << endl; + // cout<< "theta: " << theta << " depth: " << depth << endl; + // cout<< "env: " << env.vertex_min[0] << " " << env.vertex_max[0] << endl; + double axis_angle[6], min_angle, gap, plane_u_min, plane_u_max; + Eigen::Vector3d plane_w, plane_u, plane_v, center_point, start_point, box_p; + Eigen::Vector3d box_p_min, box_p_max; + int i, j, k, index, maxn, start_i, max_uN, max_vN, max_ulogN, u_min, u_max; + bool flag = false, box_found = false; + boxes.clear(); + BoxPointType box; + axis_angle[0] = acos(axis(0)); + axis_angle[1] = acos(axis(1)); + axis_angle[2] = acos(axis(2)); + axis_angle[3] = acos(-axis(0)); + axis_angle[4] = acos(-axis(1)); + axis_angle[5] = acos(-axis(2)); + index = 1; + min_angle = axis_angle[0]; + for (i=1;i<6;i++){ + if (axis_angle[i]=0){ + box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length; + box.vertex_min[0] = box_p_min(0); + box.vertex_min[1] = box_p_min(1); + box.vertex_min[2] = box_p_min(2); + box.vertex_max[0] = box_p(0); + box.vertex_max[1] = box_p(1); + box.vertex_max[2] = box_p(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k); + k = k-1; + } + k = max_ulogN; + u_max = 0; + while (k>=0){ + box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p_max(0); + box.vertex_max[1] = box_p_max(1); + box.vertex_max[2] = box_p_max(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_max = u_max + pow(2,k); + + k = k-1; + } + u_max = max(0, max_uN - u_max - 1); + box_found = false; + //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max); + for (k = u_min; k <= u_max; k++){ + box_p = box_p_min + plane_u * box_length * k; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p(0) + box_length; + box.vertex_max[1] = box_p(1) + box_length; + box.vertex_max[2] = box_p(2) + box_length; + if (check_box_in_env(box)){ + //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + box_found = true; + boxes.push_back(box); + } + } + if (box_found) { + flag = true; + } else { + if (j>1) break; + } + } + for (j = 1; j <= max_vN; j++){ + k = max_ulogN; + u_min = 0; + box_p_min = start_point.cwiseProduct(plane_w + plane_v) + plane_u * plane_u_min - plane_v * box_length * j; + box_p_max = plane_u * plane_u_max + start_point.cwiseProduct(plane_w + plane_v) - plane_v * box_length * (j-1) + plane_w * box_length; + //printf("---- DOWNSIDE (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box_p_min[0],box_p_min[1],box_p_min[2],box_p_max[0],box_p_max[1],box_p_max[2]); + + while (k>=0){ + box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length; + box.vertex_min[0] = box_p_min(0); + box.vertex_min[1] = box_p_min(1); + box.vertex_min[2] = box_p_min(2); + box.vertex_max[0] = box_p(0); + box.vertex_max[1] = box_p(1); + box.vertex_max[2] = box_p(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k); + k = k-1; + } + k = max_ulogN; + u_max = 0; + while (k>=0){ + box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p_max(0); + box.vertex_max[1] = box_p_max(1); + box.vertex_max[2] = box_p_max(2); + if (!check_box(cur_pose, axis, theta, depth, box)) { + u_max = u_max + pow(2,k); + // printf("-------- Not Included: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + } + + k = k-1; + } + u_max = max(0, max_uN - u_max - 1); + //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max); + box_found = 0; + for (k = u_min; k <= u_max; k++){ + box_p = box_p_min + plane_u * box_length * k; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p(0) + box_length; + box.vertex_max[1] = box_p(1) + box_length; + box.vertex_max[2] = box_p(2) + box_length; + if (check_box_in_env(box)){ + //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + box_found = 1; + boxes.push_back(box); + } + } + if (box_found) { + flag = true; + } else { + if (j>1) break; + } + } + if (!flag && i>0) break; + } +} + +bool FOV_Checker::check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box){ + Eigen::Vector3d vertex[8]; + bool s; + vertex[0] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_min[2]); + vertex[1] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_max[2]); + vertex[2] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_min[2]); + vertex[3] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_max[2]); + vertex[4] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_min[2]); + vertex[5] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_max[2]); + vertex[6] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_min[2]); + vertex[7] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_max[2]); + for (int i = 0; i < 8; i++){ + if (check_point(cur_pose, axis, theta, depth, vertex[i])){ + return true; + } + } + Eigen::Vector3d center_point = (vertex[7]+vertex[0])/2.0; + if (check_point(cur_pose, axis, theta, depth, center_point)){ + return true; + } + PlaneType plane[6]; + plane[0].p[0] = vertex[0]; + plane[0].p[1] = vertex[2]; + plane[0].p[2] = vertex[1]; + plane[0].p[3] = vertex[3]; + + plane[1].p[0] = vertex[0]; + plane[1].p[1] = vertex[4]; + plane[1].p[2] = vertex[2]; + plane[1].p[3] = vertex[6]; + + plane[2].p[0] = vertex[0]; + plane[2].p[1] = vertex[4]; + plane[2].p[2] = vertex[1]; + plane[2].p[3] = vertex[5]; + + plane[3].p[0] = vertex[4]; + plane[3].p[1] = vertex[6]; + plane[3].p[2] = vertex[5]; + plane[3].p[3] = vertex[7]; + + plane[4].p[0] = vertex[2]; + plane[4].p[1] = vertex[6]; + plane[4].p[2] = vertex[3]; + plane[4].p[3] = vertex[7]; + + plane[5].p[0] = vertex[1]; + plane[5].p[1] = vertex[5]; + plane[5].p[2] = vertex[3]; + plane[5].p[3] = vertex[7]; + if (check_surface(cur_pose, axis, theta, depth, plane[0]) || check_surface(cur_pose, axis, theta, depth, plane[1]) || check_surface(cur_pose, axis, theta, depth, plane[2]) || check_surface(cur_pose, axis, theta, depth, plane[3]) || check_surface(cur_pose, axis, theta, depth, plane[4]) || check_surface(cur_pose, axis, theta, depth, plane[5])) + s = 1; + else + s = 0; + return s; +} + +bool FOV_Checker::check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane){ + Eigen::Vector3d plane_p, plane_u, plane_v, plane_w, pc, p, vec; + bool s; + double t, vec_dot_u, vec_dot_v; + plane_p = plane.p[0]; + plane_u = plane.p[1] - plane_p; + plane_v = plane.p[2] - plane_p; + if (check_line(cur_pose, axis, theta, depth, plane_p, plane_u) || check_line(cur_pose, axis, theta, depth, plane_p, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_u, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_v, plane_u)){ + s = 1; + return s; + } + pc = plane_p + (plane.p[3]-plane.p[0])/2; + if (check_point(cur_pose, axis, theta, depth, pc)){ + s = 1; + return s; + } + plane_w = plane_u.cross(plane_v); + p = plane_p - cur_pose; + t = (p.dot(plane_w))/(axis.dot(plane_w)); + vec = cur_pose + t * axis - plane_p; + vec_dot_u = vec.dot(plane_u)/plane_u.norm(); + vec_dot_v = vec.dot(plane_v)/plane_v.norm(); + if (t>=-eps_value && t<=depth && vec_dot_u>=-eps_value && vec_dot_u<=plane_u.norm() && vec_dot_v>=-eps_value && vec_dot_v <= plane_v.norm()) + s = 1; + else + s = 0; + return s; +} + +bool FOV_Checker::check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec){ + Eigen::Vector3d p, vec_1, vec_2; + double xl, yl, zl, xn, yn, zn, dot_1, dot_2, ln, pn, pl, l2, p2; + double A, B, C, delta, t1, t2; + bool s; + p = line_p - cur_pose; + xl = line_vec(0); yl = line_vec(1); zl = line_vec(2); + xn = axis(0); yn = axis(1); zn = axis(2); + vec_1 = line_p - cur_pose; + vec_2 = line_p + line_vec - cur_pose; + dot_1 = vec_1.dot(axis); + dot_2 = vec_2.dot(axis); + //printf("xl yl zl: %0.4f, %0.4f, %0.4f\n", xl, yl, zl); + //printf("xn yn zn: %0.4f, %0.4f, %0.4f\n", xn, yn, zn); + //printf("dot_1, dot_2, %0.4f, %0.4f\n",dot_1, dot_2); + if ((dot_1<0 && dot_2<0) || (dot_1>depth && dot_2>depth)){ + s = false; + return s; + } + ln = xl*xn+yl*yn+zl*zn; + pn = p(0)*xn+p(1)*yn+p(2)*zn; + pl = p(0)*xl+p(1)*yl+p(2)*zl; + l2 = xl*xl+yl*yl+zl*zl; + p2 = p.norm()*p.norm(); + //printf("ln: %0.4f\n",ln); + //printf("pn:%0.4f\n",pn); + //printf("pl:%0.4f\n",pl); + //printf("l2:%0.4f\n",l2); + //printf("p2:%0.4f\n",p2); + //printf("theta, cos(theta):%0.4f %0.4f\n",theta,cos(theta)); + A = ln * ln - l2 * cos(theta) * cos(theta); + B = 2 * pn * ln - 2 * cos(theta) * cos(theta)*pl; + C = pn * pn - p2 * cos(theta) * cos(theta); + //printf("A:%0.4f, B:%0.4f, C:%0.4f\n", A,B,C); + if (!(fabs(A)<=eps_value)){ + delta = B*B - 4*A*C; + //printf("delta: %0.4f\n",delta); + if (delta <= eps_value){ + if (A < -eps_value){ + s = false; + return s; + } + else{ + s = true; + return s; + } + } else { + double sqrt_delta = sqrt(delta); + t1 = (-B - sqrt_delta)/(2*A); + t2 = (-B + sqrt_delta)/(2*A); + if (t1>t2) swap(t1,t2); + //printf("t1,t2: %0.4f,%0.4f\n",t1,t2); + // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)); + if ((t1>=-eps_value && t1<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)){ + s = true; + return s; + } + // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)); + if ((t2>=-eps_value && t2<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)){ + s = true; + return s; + } + if (A>-eps_value && (t21-eps_value)){ + s = true; + return s; + } + if (A1-eps_value){ + s = true; + return s; + } + s = false; + } + } else{ + if (!(fabs(B)<=eps_value)){ + s = (B>-eps_value && -C/B<=1+eps_value) || (B=-eps_value); + return s; + } + else { + s = C>=-eps_value; + return s; + } + } + return false; +} + +bool FOV_Checker::check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point){ + Eigen::Vector3d vec; + double proj_len; + bool s; + vec = point-cur_pose; + if (vec.transpose()*vec < 0.4 * box_length * box_length){ + return true; + } + proj_len = vec.dot(axis); + if (proj_len > depth){ + s = false; + return s; + } + //printf("acos: %0.4f\n",acos(proj_len/vec.norm())); + if (fabs(vec.norm()) <= 1e-4 || acos(proj_len/vec.norm()) <= theta + 0.0175) + s = true; + else + s = false; + return s; +} + +bool FOV_Checker::check_box_in_env(BoxPointType box){ + if (box.vertex_min[0] >= env.vertex_min[0]-eps_value && box.vertex_min[1] >= env.vertex_min[1]-eps_value && box.vertex_min[2] >= env.vertex_min[2]-eps_value && box.vertex_max[0]<= env.vertex_max[0]+eps_value && box.vertex_max[1]<= env.vertex_max[1]+eps_value && box.vertex_max[2]<= env.vertex_max[2]+eps_value){ + return true; + } else { + return false; + } +} + diff --git a/include/FOV_Checker/FOV_Checker.h b/include/FOV_Checker/FOV_Checker.h new file mode 100755 index 0000000..e7db390 --- /dev/null +++ b/include/FOV_Checker/FOV_Checker.h @@ -0,0 +1,33 @@ +// Include Files +#pragma once +#include +#include +#include "ikd-Tree/ikd_Tree.h" +#include +#include + +#define eps_value 1e-6 + +struct PlaneType{ + Eigen::Vector3d p[4]; +}; + +class FOV_Checker{ +public: + FOV_Checker(); + ~FOV_Checker(); + void Set_Env(BoxPointType env_param); + void Set_BoxLength(double box_len_param); + void check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector &boxes); + bool check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box); + bool check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec); + bool check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane); + bool check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point); + bool check_box_in_env(BoxPointType box); +private: + BoxPointType env; + double box_length; + FILE *fp; + +}; + diff --git a/include/IKFoM/.gitignore b/include/IKFoM/.gitignore new file mode 100644 index 0000000..259148f --- /dev/null +++ b/include/IKFoM/.gitignore @@ -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 diff --git a/include/IKFoM/.vscode/settings.json b/include/IKFoM/.vscode/settings.json new file mode 100644 index 0000000..d756850 --- /dev/null +++ b/include/IKFoM/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "complex": "cpp" + } +} \ No newline at end of file diff --git a/include/IKFoM/IKFoM_toolkit/.vscode/settings.json b/include/IKFoM/IKFoM_toolkit/.vscode/settings.json new file mode 100755 index 0000000..b619d8a --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/.vscode/settings.json @@ -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" + } +} \ No newline at end of file diff --git a/include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json b/include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json new file mode 100755 index 0000000..c807bd5 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "core": "cpp" + } +} \ No newline at end of file diff --git a/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp b/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp new file mode 100755 index 0000000..d4524e1 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp @@ -0,0 +1,390 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * 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 +#include + +#include +#include +#include +#include +#include +#include + +#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 +struct dyn_share_modified +{ + bool valid; + bool converge; + T M_Noise; + Eigen::Matrix z; + Eigen::Matrix h_x; + Eigen::Matrix z_IMU; + Eigen::Matrix R_IMU; + bool satu_check[6]; +}; + +template +class esekf{ + + typedef esekf self; + enum{ + n = state::DOF, m = state::DIM, l = measurement::DOF + }; + +public: + + typedef typename state::scalar scalar_type; + typedef Matrix cov; + typedef Matrix cov_; + typedef SparseMatrix spMt; + typedef Matrix vectorized_state; + typedef Matrix flatted_state; + typedef flatted_state processModel(state &, const input &); + typedef Eigen::Matrix processMatrix1(state &, const input &); + typedef Eigen::Matrix processMatrix2(state &, const input &); + typedef Eigen::Matrix processnoisecovariance; + + typedef void measurementModel_dyn_share_modified(state &, dyn_share_modified &); + typedef Eigen::Matrix measurementMatrix1(state &); + typedef Eigen::Matrix measurementMatrix1_dyn(state &); + typedef Eigen::Matrix measurementMatrix2(state &); + typedef Eigen::Matrix measurementMatrix2_dyn(state &); + typedef Eigen::Matrix measurementnoisecovariance; + typedef Eigen::Matrix measurementnoisecovariance_dyn; + + esekf(const state &x = state(), + const cov &P = cov::Identity()): x_(x), P_(P){}; + + void init_dyn_share_modified(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in) + { + f = f_in; + f_x = f_x_in; + // f_w = f_w_in; + h_dyn_share_modified_1 = h_dyn_share_in; + maximum_iter = 1; + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + x_.build_SEN_state(); + } + + void init_dyn_share_modified_2h(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified 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; + 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, 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 res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::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 res; + res.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(1/2)); + F_x1.template block<3, 3>(idx, idx) = 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 dyn_share; + state x_propagated = x_; + int dof_Measurement; + double m_noise; + for(int i=0; i z = dyn_share.z; + // Matrix R = dyn_share.R; + Matrix h_x = dyn_share.h_x; + // Matrix 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 PHT; + Matrix HPHT; + Matrix K_; + // if(n > dof_Measurement) + { + PHT = P_. template block(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(); + } + Matrix dx_ = K_ * z; // - h) + (K_x - Matrix::Identity()) * dx_new; + // state x_before = x_; + + x_.boxplus(dx_); + dyn_share.converge = true; + + // L_ = P_; + // Matrix res_temp_SO3; + // MTK::vect<3, scalar_type> seg_SO3; + // for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + // int idx = (*it).first; + // for(int i = 0; i < 3; i++){ + // seg_SO3(i) = dx_(i + idx); + // } + // res_temp_SO3 = A_matrix(seg_SO3).transpose(); + // for(int i = 0; i < n; i++){ + // L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + // } + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + // } + // } + // for(int i = 0; i < n; i++){ + // L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // // P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // } + // for(int i = 0; i < n; i++){ + // P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // } + // } + // Matrix res_temp_S2; + // MTK::vect<2, scalar_type> seg_S2; + // for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + // int idx = (*it).first; + + // for(int i = 0; i < 2; i++){ + // seg_S2(i) = dx_(i + idx); + // } + + // Eigen::Matrix Nx; + // Eigen::Matrix Mx; + // x_.S2_Nx_yy(Nx, idx); + // x_propagated.S2_Mx(Mx, seg_S2, idx); + // res_temp_S2 = Nx * Mx; + + // for(int i = 0; i < n; i++){ + // L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + // } + + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + // } + // } + // for(int i = 0; i < n; i++){ + // L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + // } + // for(int i = 0; i < n; i++){ + // P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + // } + // } + // if(n > dof_Measurement) + { + P_ = P_ - K_*h_x*P_. template block<12, n>(0, 0); + } + } + return true; + } + + void update_iterated_dyn_share_IMU() { + + dyn_share_modified dyn_share; + for(int i=0; i z = dyn_share.z_IMU; + + Matrix PHT; + Matrix HP; + Matrix 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 K = PHT * HPHT.inverse(); + + Matrix 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_; + } + state x_; +private: + measurement m_; + cov P_; + 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 *h_dyn_share_modified_1; + + measurementModel_dyn_share_modified *h_dyn_share_modified_2; + + int maximum_iter = 0; + scalar_type limit[n]; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace esekfom + +#endif // ESEKFOM_EKF_HPP diff --git a/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp b/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp new file mode 100755 index 0000000..ab39fc4 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * 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 +#include "../mtk/src/mtkmath.hpp" +namespace esekfom { + +template +class is_same { +public: + operator bool() { + return false; + } +}; +template +class is_same { +public: + operator bool() { + return true; + } +}; + +template +class is_double { +public: + operator bool() { + return false; + } +}; + +template<> +class is_double { +public: + operator bool() { + return true; + } +}; + +template +static T +id(const T &x) +{ + return x; +} + +} // namespace esekfom + +#endif // __MEKFOM_UTIL_HPP__ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp b/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp new file mode 100755 index 0000000..115ec7e --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp @@ -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 + * + * 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 + * + * 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 + +#include +#include +#include + +#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 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 > Pose; + * typedef MTK::vect 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 need to be typedef'ed ahead. + */ +#define MTK_BUILD_MANIFOLD(name, entries) \ +struct name { \ + typedef name self; \ + std::vector > S2_state;\ + std::vector > SO3_state;\ + std::vector, int> > vect_state;\ + std::vector, 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 & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_BOXPLUS, entries)\ + } \ + void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_OPLUS, entries)\ + } \ + void boxminus(MTK::vectview __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 &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_hat, entries)\ + }\ + void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ + }\ + void S2_Mx(Eigen::Matrix &res, Eigen::Matrix 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_*/ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp b/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp new file mode 100755 index 0000000..a1b13de --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp @@ -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 + * + * 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 + * + * 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 +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 + 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_ */ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp b/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp new file mode 100755 index 0000000..a8770e4 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp @@ -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 + * + * 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 + * + * 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 + +#include + +#include "../types/vect.hpp" + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + + +namespace MTK { + +namespace internal { + +template +struct traits { + typedef typename Manifold::scalar scalar; + enum {DOF = Manifold::DOF}; + typedef vect vectorized_type; + typedef Eigen::Matrix matrix_type; +}; + +template<> +struct traits : traits > {}; +template<> +struct traits : traits > {}; + +} // namespace internal + +/** + * \defgroup MTKMath Mathematical helper functions + */ +//@{ + +//! constant @f$ \pi @f$ +const double pi = M_PI; + +template inline scalar tolerance(); + +template<> inline float tolerance() { return 1e-5f; } +template<> inline double tolerance() { 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 +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 +std::pair 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(); + 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 +Eigen::Matrix hat(const Base& v) { + Eigen::Matrix res; + res << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return res; +} + +template +Eigen::Matrix A_inv_trans(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix A_inv(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) + { + Eigen::Matrix res; + scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::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 +Eigen::Matrix A_matrix(const Base & v){ + Eigen::Matrix res; + double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + double norm = std::sqrt(squaredNorm); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Identity(); + } + else{ + res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); + } + return res; +} + +template +scalar exp(vectview result, vectview vec, const scalar& scale = 1) { + scalar norm2 = vec.squaredNorm(); + std::pair 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 +void log(vectview result, + const scalar &w, const vectview 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()) { + if(!plus_minus_periodicity && w < 0) { + // find the maximal entry: + int i; + nv = vec.cwiseAbs().maxCoeff(&i); + result = scale * std::atan2(nv, w) * vect::Unit(i); + return; + } + nv = tolerance(); + } + scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); + + result = s * vec; +} + + +} // namespace MTK + + +#endif /* MTKMATH_H_ */ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp b/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp new file mode 100755 index 0000000..5025071 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp @@ -0,0 +1,168 @@ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * 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 + +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 + struct CovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct CrossCovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CrossCovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct VectviewBase { + typedef Eigen::Matrix matrix_type; + typedef typename matrix_type::MapType Type; + typedef typename matrix_type::ConstMapType ConstType; + }; + + template + struct UnalignedType { + typedef T type; + }; +} + +template +class vectview : public internal::VectviewBase::Type { + typedef internal::VectviewBase 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 + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} + + //! inherit assignment operator + using base::operator=; + //! data pointer + scalar* data() {return const_cast(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 vectview : public internal::VectviewBase::ConstType { + typedef internal::VectviewBase 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 + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from row vector + template + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from another @c vectview + vectview(vectview 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 + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} + +}; + + +} // namespace MTK + +#endif /* VECTVIEW_HPP_ */ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp b/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp new file mode 100755 index 0000000..4dc2958 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp @@ -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 + * + * 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 + * + * 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 + +#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 +int getStartIdx( MTK::SubManifold Base::*) +{ + return idx; +} + +template +int getStartIdx_( MTK::SubManifold Base::*) +{ + return dim; +} + +/** + * Determine the degrees of freedom of a sub-variable within a compound variable. + */ +template +int getDof( MTK::SubManifold Base::*) +{ + return T::DOF; +} +template +int getDim( MTK::SubManifold Base::*) +{ + return T::DIM; +} + +/** + * set the diagonal elements of a covariance matrix corresponding to a sub-variable + */ +template +void setDiagonal(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(idx).setConstant(val); +} + +template +void setDiagonal_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(dim).setConstant(val); +} + +/** + * Get the subblock of corresponding to two members, i.e. + * \code + * Eigen::Matrix 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 +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(dim1, dim2); +} + +template +typename MTK::internal::CrossCovBlock::Type +subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CrossCovBlock_::Type +subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(dim1, dim2); +} +/** + * Get the subblock of corresponding to a member, i.e. + * \code + * Eigen::Matrix m; + * MTK::subblock(m, &Pose::orient) = some_expression; + * \endcode + * lets you modify covariance entries in a bigger covariance matrix. + */ +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(dim, dim); +} + +template +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(idx, idx); +} + +template +class get_cov { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cov_ { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cross_cov { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + +template +class get_cross_cov_ { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + + +template +vectview +subvector_impl_(vectview vec, SubManifold Base::*) +{ + return vec.template segment(dim); +} + +template +vectview +subvector_impl(vectview vec, SubManifold Base::*) +{ + return vec.template segment(idx); +} + +/** + * Get the subvector corresponding to a sub-manifold from a bigger vector. + */ + template +vectview +subvector_(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vec, ptr); +} + +template +vectview +subvector(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl(vec, ptr); +} + +/** + * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + +template +vectview +subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + + +/** + * const version of subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector_impl(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(idx); +} + +template +vectview +subvector_impl_(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(dim); +} + +template +vectview +subvector(const vectview cvec, SubManifold Base::* ptr) +{ + return subvector_impl(cvec, ptr); +} + + +} // namespace MTK + +#endif // GET_START_INDEX_H_ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp b/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp new file mode 100755 index 0000000..1ff0250 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp @@ -0,0 +1,326 @@ +// 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 + * + * 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 + * + * 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 +struct S2 { + + typedef _scalar scalar; + typedef vect<3, scalar> vect_type; + typedef SO3 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 delta, scalar scale = 1) + { + SO3_type res; + res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); + vec = res.normalized().toRotationMatrix() * vec; + } + + void boxplus(MTK::vectview delta, scalar scale=1) { + Eigen::Matrix Bx; + S2_Bx(Bx); + vect_type Bu = Bx*delta;SO3_type res; + res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); + vec = res.normalized().toRotationMatrix() * vec; + } + + void boxminus(MTK::vectview res, const S2& 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()) + { + if(std::fabs(theta) > MTK::tolerance() ) + { + res[0] = 3.1415926; + res[1] = 0; + } + else{ + res[0] = 0; + res[1] = 0; + } + } + else + { + S2 other_copy = other; + Eigen::MatrixBx; + 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 &res) + { + Eigen::Matrix 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 &res) + { + if(S2_typ == 3) + { + if(vec[2] + length > tolerance()) + { + + 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::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else if(S2_typ == 2) + { + if(vec[1] + length > tolerance()) + { + + 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::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else + { + if(vec[0] + length > tolerance()) + { + + 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::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + } + + void S2_Nx(Eigen::Matrix &res, S2& subtrahend) + { + if((vec+subtrahend.vec).norm() > tolerance()) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if((vec-subtrahend.vec).norm() > tolerance()) + { + 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 &res) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + res = 1/length/length*Bx.transpose()*MTK::hat(vec); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if(delta.norm() < tolerance()) + { + res = -MTK::hat(vec)*Bx; + } + else{ + vect_type Bu = Bx*delta; + SO3_type exp_delta; + exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); + res = -exp_delta.normalized().toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; + } + } + + operator const vect_type&() const{ + return vec; + } + + const vect_type& get_vect() const { + return vec; + } + + friend S2 operator*(const SO3& rot, const S2& dir) + { + S2 ret; + ret.vec = rot.normalized() * dir.vec; + return ret; + } + + scalar operator[](int idx) const {return vec[idx]; } + + friend std::ostream& operator<<(std::ostream &os, const S2& vec){ + return os << vec.vec.transpose() << " "; + } + friend std::istream& operator>>(std::istream &is, S2& 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_*/ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp b/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp new file mode 100755 index 0000000..2270136 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp @@ -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 + * + * 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 + * + * 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 + +#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 +struct SEN { + enum {DOF = num_of_vec_plus1, DIM = num_of_vec_plus1, TYP = 4}; + typedef _scalar scalar; + typedef Eigen::Matrix base; + typedef SO3 SO3_type; + // typedef Eigen::Quaternion base; + // typedef Eigen::Quaternion Quaternion; + typedef vect 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 + // SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} + + /** + * Construct from arbitrary rotation type. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + // template + // SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} + + //! @name Manifold requirements + + void boxplus(MTK::vectview vec, scalar scale=1) { + SEN delta = exp(vec, scale); // ? + mat = mat * delta.mat; + } + void boxminus(MTK::vectview res, const SEN& other) const { + base error_mat = other.mat.inverse() * mat; + res = log(error_mat); + } + //} + + void oplus(MTK::vectview vec, scalar scale=1) { + SEN delta = exp(vec, scale); + mat = mat * delta.mat; + } + + // void hat(MTK::vectview& v, Eigen::Matrix &res) { + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + res = Eigen::Matrix::Zero(); + Eigen::Matrix 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 vec, Eigen::Matrix & res){ + void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + res = Eigen::Matrix::Zero(); + Eigen::Matrix 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 ; + 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()) + { + 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 & vec, Eigen::Matrix &res){ + void Jacob_right(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + res = Eigen::Matrix::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 ; + 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()) + { + 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 &res) + { + std::cerr << "wrong idx for S2" << std::endl; + res = Eigen::Matrix::Zero(); + } + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const SEN& q){ + for(int i=0; i>(std::istream &is, SEN& q){ + // vect coeffs; + for(int i=0; i> 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 dvec, scalar scale = 1){ + static SEN exp(const Eigen::Matrix& dvec, scalar scale = 1){ + SEN res; + res.mat = Eigen::Matrix::Identity(); + Eigen::Matrix exp_; //, jac; + Eigen::MatrixXd jac; + Eigen::Matrix 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 exp(log(x)) == x + */ + static Eigen::Matrix log(base &orient){ + Eigen::Matrix res; + Eigen::Matrix psi; + Eigen::VectorXd minus_psi; + Eigen::Matrix 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_*/ + diff --git a/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp b/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp new file mode 100755 index 0000000..78f9eec --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp @@ -0,0 +1,365 @@ +// 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 + * + * 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 + * + * 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 + +#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 +struct SO2 : public Eigen::Rotation2D<_scalar> { + enum {DOF = 1, DIM = 2, TYP = 3}; + + typedef _scalar scalar; + typedef Eigen::Rotation2D base; + typedef vect 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 + vect_type operator%(const Eigen::MatrixBase &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 &res) + { + res = Eigen::Matrix::Zero(); + } + //! @name Manifold requirements + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + void boxminus(MTK::vectview res, const SO2& other) const { + res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); + } + + friend std::istream& operator>>(std::istream &is, SO2& 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 +struct SO3 : public Eigen::Quaternion<_scalar, Options> { + enum {DOF = 3, DIM = 3, TYP = 2}; + typedef _scalar scalar; + typedef Eigen::Quaternion base; + typedef Eigen::Quaternion Quaternion; + typedef vect vect_type; + + //! Calculate @c this->inverse() * @c r + template EIGEN_STRONG_INLINE + Quaternion operator%(const Eigen::QuaternionBase &r) const { + return base::conjugate() * r; + } + + //! Calculate @c this->inverse() * @c r + template + vect_type operator%(const Eigen::MatrixBase &vec) const { + return base::conjugate() * vec; + } + + //! Calculate @c this * @c r.conjugate() + template EIGEN_STRONG_INLINE + Quaternion operator/(const Eigen::QuaternionBase &r) const { + return *this * r.conjugate(); + } + + /** + * Construct from real part and three imaginary parts. + * Quaternion is normalized after construction. + */ + SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) { + base::normalize(); + } + + /** + * 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 + SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} + + /** + * Construct from arbitrary rotation type. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + template + SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} + + //! @name Manifold requirements + + void boxplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + void boxminus(MTK::vectview res, const SO3& other) const { + res = SO3::log(other.conjugate() * *this); + } + //} + + void oplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + + // void hat(MTK::vectview& v, Eigen::Matrix &res) { + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + // Eigen::Matrix 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 vec, Eigen::Matrix & res){ + void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + Eigen::MatrixXd hat_v; + hat(vec, hat_v); + if(vec.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::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::Identity(); + } + // return res; + } + + // void Jacob_right(MTK::vectview & v, Eigen::Matrix &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()){ + res = Eigen::Matrix::Identity(); + } + else{ + res = Eigen::Matrix::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 &res) + { + res = Eigen::Matrix::Zero(); + } + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const SO3& q){ + return os << q.coeffs().transpose() << " "; + } + + friend std::istream& operator>>(std::istream &is, SO3& q){ + vect<4,scalar> coeffs; + is >> 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 dvec, scalar scale = 1){ + static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ + SO3 res; + res.w() = MTK::exp(res.vec(), dvec, scalar(scale/2)); + return res; + } + /** + * Calculate the inverse of @c exp. + * Only guarantees that exp(log(x)) == x + */ + static typename base::Vector3 log(const SO3 &orient){ + typename base::Vector3 res; + MTK::log(res, orient.w(), orient.vec(), scalar(2), true); + return res; + } +}; + +namespace internal { +template +struct UnalignedType >{ + typedef SO2 type; +}; + +template +struct UnalignedType >{ + typedef SO3 type; +}; + +} // namespace internal + + +} // namespace MTK + +#endif /*SON_H_*/ + diff --git a/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp b/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp new file mode 100755 index 0000000..d75e6d7 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp @@ -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 + * + * 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 + * + * 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 +#include +#include + +#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 +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 + EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} + + /** Construct from memory. */ + vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } + + void boxplus(MTK::vectview vec, scalar scale=1) { + *this += scale * vec; + } + void boxminus(MTK::vectview res, const vect& other) const { + res = *this - other; + } + + void oplus(MTK::vectview 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 &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const vect& v){ + // Eigen sometimes messes with the streams flags, so output manually: + for(int i=0; i>(std::istream &is, vect& 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 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[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 + vectview tail(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview tail() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview head(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } + template + vectview head() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } +}; + + +/** + * 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::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 + EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} + + /** Construct from memory. */ + matrix(const scalar* src) : base(src) { } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + void boxminus(MTK::vectview 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 &res) + { + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ + for(int i=0; i>(std::istream &is, matrix& mat){ + for(int i=0; i> mat.data()[i]; + } + return is; + } +};// @todo What if M / N = Eigen::Dynamic? + + + +/** + * A simple scalar type. + */ +template +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 &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + void boxminus(MTK::vectview 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 +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 vec, scalar scale = 1) { + value *= std::exp(scale * vec[0]); + } + void boxminus(MTK::vectview res, const PositiveScalar& other) const { + res[0] = std::log(*this / other); + } + + void oplus(MTK::vectview 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 &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + + friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ + is >> s.value; + assert(s.value > 0); + return is; + } +}; + +template +struct Complex : public std::complex<_scalar>{ + enum {DOF = 2, TYP = 7, DIM=0}; + typedef _scalar scalar; + + typedef std::complex 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 &in) : Base(in[0], in[1]) {} + template + Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} + + void boxplus(MTK::vectview vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + void boxminus(MTK::vectview res, const Complex& other) const { + Complex diff = *this - other; + res << diff.real(), diff.imag(); + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::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 vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::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 +struct UnalignedType >{ + typedef vect type; +}; + +} // namespace internal + + +} // namespace MTK + + + + +#endif /*VECT_H_*/ diff --git a/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp b/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp new file mode 100755 index 0000000..b6643f1 --- /dev/null +++ b/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH + * All rights reserved. + * + * Author: Rene Wagner + * Christoph Hertzberg + * + * 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 +#include + +namespace MTK { + +template +struct cv_f_type; + +template<> +struct cv_f_type +{ + enum {value = CV_64F}; +}; + +template<> +struct cv_f_type +{ + enum {value = CV_32F}; +}; + +/** + * cv_mat wraps a CvMat around an Eigen Matrix + */ +template +class cv_mat : public matrix +{ + typedef matrix base_type; + enum {type_ = cv_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 + cv_mat(const Eigen::MatrixBase &value) : base_type(value) + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + template + cv_mat& operator=(const Eigen::MatrixBase &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_ */ diff --git a/include/IKFoM/LICENSE b/include/IKFoM/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/include/IKFoM/LICENSE @@ -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. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/include/IKFoM/README.md b/include/IKFoM/README.md new file mode 100644 index 0000000..53a7dc5 --- /dev/null +++ b/include/IKFoM/README.md @@ -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 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 +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 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 that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::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 and its differentiation , : +``` +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 dh_dx(state &s) {} +Eigen::Matrix 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::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf 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 and its differentiation , : +``` +measurement h_share(state &s, esekfom::share_datastruct &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::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf 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::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 +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 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 that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::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 and its differentiation , : +``` +Eigen::Matrix 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 h_; + h_(0) = s.pos[0]; + return h_; +} +Eigen::Matrix dh_dx(state &s) {} +Eigen::Matrix 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::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf 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 and its differentiation , : +``` +Eigen::Matrix h_dyn_share(state &s, esekfom::dyn_share_datastruct &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 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::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf 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::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 +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 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 that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::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 , : +``` +Eigen::Matrix dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false +Eigen::Matrix 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::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) +``` +esekfom::esekf 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 : +``` +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::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf 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 and its differentiation , : +``` +measurement h_dyn_runtime_share(state &s, esekfom::dyn_runtime_share_datastruct &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::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. + diff --git a/include/common_lib.h b/include/common_lib.h new file mode 100755 index 0000000..7321303 --- /dev/null +++ b/include/common_lib.h @@ -0,0 +1,180 @@ +#ifndef COMMON_LIB_H +#define COMMON_LIB_H + +#include +#include +#include +#include +#include +#include +#include +#include +using namespace std; +using namespace Eigen; + +#define PI_M (3.14159265358) +#define G_m_s2 (9.81) // Gravaty const in GuangDong/China +#define DIM_STATE (18) // 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 (mat.data(), mat.data() + mat.rows() * mat.cols()) +#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) + +// 常用的PCL点类型、点云类型、点向量 +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointXYZRGB PointTypeRGB; +typedef pcl::PointCloud PointCloudXYZI; +typedef pcl::PointCloud PointCloudXYZRGB; +typedef vector> PointVector; + +// 常用的Eigen变量类型 +typedef Vector3d V3D; +typedef Matrix3d M3D; +typedef Vector3f V3F; +typedef Matrix3f M3F; + +#define MD(a,b) Matrix +#define VD(a) Matrix +#define MF(a,b) Matrix +#define VF(a) Matrix + +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 imu; // IMU队列 +}; + +template +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 +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 +std::vector time_compressing(const PointCloudXYZI::Ptr &point_cloud) +{ + int points_size = point_cloud->points.size(); + int j = 0; + std::vector 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 +bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) +{ + MatrixXf A(point_num, 3); + MatrixXf b(point_num, 1); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < point_num; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + normvec = A.colPivHouseholderQr().solve(b); + + for (int j = 0; j < point_num; j++) + { + if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) + { + return false; + } + } + + normvec.normalize(); + return true; +} + +template +bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) +{ + Matrix A; + Matrix b; + A.setZero(); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + + Matrix normvec = A.colPivHouseholderQr().solve(b); + + T n = normvec.norm(); + pca_result(0) = normvec(0) / n; + pca_result(1) = normvec(1) / n; + pca_result(2) = normvec(2) / n; + pca_result(3) = 1.0 / n; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) + { + return false; + } + } + return true; +} + +#endif \ No newline at end of file diff --git a/include/ikd-Tree/README.md b/include/ikd-Tree/README.md new file mode 100644 index 0000000..e113a91 --- /dev/null +++ b/include/ikd-Tree/README.md @@ -0,0 +1,2 @@ +# ikd-Tree +ikd-Tree is an incremental k-d tree for robotic applications. diff --git a/include/ikd-Tree/ikd_Tree.cpp b/include/ikd-Tree/ikd_Tree.cpp new file mode 100644 index 0000000..e8c4e86 --- /dev/null +++ b/include/ikd-Tree/ikd_Tree.cpp @@ -0,0 +1,1728 @@ +#include "ikd_Tree.h" + +/* +Description: ikd-Tree: an incremental k-d tree for robotic applications +Author: Yixi Cai +email: yixicai@connect.hku.hk +*/ + +template +KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) +{ + delete_criterion_param = delete_param; + balance_criterion_param = balance_param; + downsample_size = box_length; + Rebuild_Logger.clear(); + termination_flag = false; + start_thread(); +} + +template +KD_TREE::~KD_TREE() +{ + stop_thread(); + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} + + + +template +void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) +{ + Set_delete_criterion_param(delete_param); + Set_balance_criterion_param(balance_param); + set_downsample_param(box_length); +} + +template +void KD_TREE::InitTreeNode(KD_TREE_NODE *root) +{ + root->point.x = 0.0f; + root->point.y = 0.0f; + root->point.z = 0.0f; + root->node_range_x[0] = 0.0f; + root->node_range_x[1] = 0.0f; + root->node_range_y[0] = 0.0f; + root->node_range_y[1] = 0.0f; + root->node_range_z[0] = 0.0f; + root->node_range_z[1] = 0.0f; + root->radius_sq = 0.0f; + root->division_axis = 0; + root->father_ptr = nullptr; + root->left_son_ptr = nullptr; + root->right_son_ptr = nullptr; + root->TreeSize = 0; + root->invalid_point_num = 0; + root->down_del_num = 0; + root->point_deleted = false; + root->tree_deleted = false; + root->need_push_down_to_left = false; + root->need_push_down_to_right = false; + root->point_downsample_deleted = false; + root->working_flag = false; + pthread_mutex_init(&(root->push_down_mutex_lock), NULL); +} + +template +int KD_TREE::size() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + return Root_Node->TreeSize; + } + else + { + return 0; + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return Treesize_tmp; + } + } +} + +template +BoxPointType KD_TREE::tree_range() +{ + BoxPointType range; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + } + else + { + memset(&range, 0, sizeof(range)); + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + pthread_mutex_unlock(&working_flag_mutex); + } + else + { + memset(&range, 0, sizeof(range)); + } + } + return range; +} + +template +int KD_TREE::validnum() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + return (Root_Node->TreeSize - Root_Node->invalid_point_num); + else + return 0; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize - Root_Node->invalid_point_num; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return -1; + } + } +} + +template +void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del) +{ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + return; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + pthread_mutex_unlock(&working_flag_mutex); + return; + } + else + { + alpha_bal = alpha_bal_tmp; + alpha_del = alpha_del_tmp; + return; + } + } +} + +template +void KD_TREE::start_thread() +{ + pthread_mutex_init(&termination_flag_mutex_lock, NULL); + pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); + pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); + pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); + pthread_mutex_init(&working_flag_mutex, NULL); + pthread_mutex_init(&search_flag_mutex, NULL); + pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this); + printf("Multi thread started \n"); +} + +template +void KD_TREE::stop_thread() +{ + pthread_mutex_lock(&termination_flag_mutex_lock); + termination_flag = true; + pthread_mutex_unlock(&termination_flag_mutex_lock); + if (rebuild_thread) + pthread_join(rebuild_thread, NULL); + pthread_mutex_destroy(&termination_flag_mutex_lock); + pthread_mutex_destroy(&rebuild_logger_mutex_lock); + pthread_mutex_destroy(&rebuild_ptr_mutex_lock); + pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); + pthread_mutex_destroy(&working_flag_mutex); + pthread_mutex_destroy(&search_flag_mutex); +} + +template +void *KD_TREE::multi_thread_ptr(void *arg) +{ + KD_TREE *handle = (KD_TREE *)arg; + handle->multi_thread_rebuild(); + return nullptr; +} + +template +void KD_TREE::multi_thread_rebuild() +{ + bool terminated = false; + KD_TREE_NODE *father_ptr, **new_node_ptr; + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + while (!terminated) + { + pthread_mutex_lock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&working_flag_mutex); + if (Rebuild_Ptr != nullptr) + { + /* Traverse and copy */ + if (!Rebuild_Logger.empty()) + { + printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); + } + rebuild_flag = true; + if (*Rebuild_Ptr == Root_Node) + { + Treesize_tmp = Root_Node->TreeSize; + Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; + alpha_bal_tmp = Root_Node->alpha_bal; + alpha_del_tmp = Root_Node->alpha_del; + } + KD_TREE_NODE *old_root_node = (*Rebuild_Ptr); + father_ptr = (*Rebuild_Ptr)->father_ptr; + PointVector().swap(Rebuild_PCL_Storage); + // Lock Search + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + // Lock deleted points cache + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); + // Unlock deleted points cache + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + // Unlock Search + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + pthread_mutex_unlock(&working_flag_mutex); + /* Rebuild and update missed operations*/ + Operation_Logger_Type Operation; + KD_TREE_NODE *new_root_node = nullptr; + if (int(Rebuild_PCL_Storage.size()) > 0) + { + BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); + // Rebuild has been done. Updates the blocked operations into the new tree + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + int tmp_counter = 0; + while (!Rebuild_Logger.empty()) + { + Operation = Rebuild_Logger.front(); + max_queue_size = max(max_queue_size, Rebuild_Logger.size()); + Rebuild_Logger.pop(); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + pthread_mutex_unlock(&working_flag_mutex); + run_operation(&new_root_node, Operation); + tmp_counter++; + if (tmp_counter % 10 == 0) + usleep(1); + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + /* Replace to original tree*/ + // pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + if (father_ptr->left_son_ptr == *Rebuild_Ptr) + { + father_ptr->left_son_ptr = new_root_node; + } + else if (father_ptr->right_son_ptr == *Rebuild_Ptr) + { + father_ptr->right_son_ptr = new_root_node; + } + else + { + throw "Error: Father ptr incompatible with current node\n"; + } + if (new_root_node != nullptr) + new_root_node->father_ptr = father_ptr; + (*Rebuild_Ptr) = new_root_node; + int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num; + int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num; + if (father_ptr == STATIC_ROOT_NODE) + Root_Node = STATIC_ROOT_NODE->left_son_ptr; + KD_TREE_NODE *update_root = *Rebuild_Ptr; + while (update_root != nullptr && update_root != Root_Node) + { + update_root = update_root->father_ptr; + if (update_root->working_flag) + break; + if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) + break; + if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) + break; + Update(update_root); + } + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + Rebuild_Ptr = nullptr; + pthread_mutex_unlock(&working_flag_mutex); + rebuild_flag = false; + /* Delete discarded tree nodes */ + delete_tree_nodes(&old_root_node); + } + else + { + pthread_mutex_unlock(&working_flag_mutex); + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + usleep(100); + } + printf("Rebuild thread terminated normally\n"); +} + +template +void KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation) +{ + switch (operation.op) + { + case ADD_POINT: + Add_by_point(root, operation.point, false, (*root)->division_axis); + break; + case ADD_BOX: + Add_by_range(root, operation.boxpoint, false); + break; + case DELETE_POINT: + Delete_by_point(root, operation.point, false); + break; + case DELETE_BOX: + Delete_by_range(root, operation.boxpoint, false, false); + break; + case DOWNSAMPLE_DELETE: + Delete_by_range(root, operation.boxpoint, false, true); + break; + case PUSH_DOWN: + (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; + (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; + if (operation.tree_downsample_deleted) + (*root)->down_del_num = (*root)->TreeSize; + if (operation.tree_deleted) + (*root)->invalid_point_num = (*root)->TreeSize; + else + (*root)->invalid_point_num = (*root)->down_del_num; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + break; + default: + break; + } +} + +template +void KD_TREE::Build(PointVector point_cloud) +{ + if (Root_Node != nullptr) + { + delete_tree_nodes(&Root_Node); + } + if (point_cloud.size() == 0) + return; + STATIC_ROOT_NODE = new KD_TREE_NODE; + InitTreeNode(STATIC_ROOT_NODE); + BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud); + Update(STATIC_ROOT_NODE); + STATIC_ROOT_NODE->TreeSize = 0; + Root_Node = STATIC_ROOT_NODE->left_son_ptr; +} + +template +void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist) +{ + MANUAL_HEAP q(2 * k_nearest); + q.clear(); + vector().swap(Point_Distance); + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Search(Root_Node, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(Root_Node, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + int k_found = min(k_nearest, int(q.size())); + PointVector().swap(Nearest_Points); + vector().swap(Point_Distance); + for (int i = 0; i < k_found; i++) + { + Nearest_Points.insert(Nearest_Points.begin(), q.top().point); + Point_Distance.insert(Point_Distance.begin(), q.top().dist); + q.pop(); + } + return; +} + +template +void KD_TREE::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage) +{ + Storage.clear(); + Search_by_range(Root_Node, Box_of_Point, Storage); +} + +template +void KD_TREE::Radius_Search(PointType point, const float radius, PointVector &Storage) +{ + Storage.clear(); + Search_by_radius(Root_Node, point, radius, Storage); +} + +template +int KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on) +{ + int NewPointSize = PointToAdd.size(); + int tree_size = size(); + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; + float min_dist, tmp_dist; + int tmp_counter = 0; + for (int i = 0; i < PointToAdd.size(); i++) + { + if (downsample_switch) + { + Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size; + Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size; + Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size; + Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size; + Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size; + Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size; + mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0; + mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0; + mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0; + PointVector().swap(Downsample_Storage); + Search_by_range(Root_Node, Box_of_Point, Downsample_Storage); + min_dist = calc_dist(PointToAdd[i], mid_point); + downsample_result = PointToAdd[i]; + for (int index = 0; index < Downsample_Storage.size(); index++) + { + tmp_dist = calc_dist(Downsample_Storage[index], mid_point); + if (tmp_dist < min_dist) + { + min_dist = tmp_dist; + downsample_result = Downsample_Storage[index]; + } + } + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, true, true); + Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); + tmp_counter++; + } + } + else + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + Operation_Logger_Type operation_delete, operation; + operation_delete.boxpoint = Box_of_Point; + operation_delete.op = DOWNSAMPLE_DELETE; + operation.point = downsample_result; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, false, true); + Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); + tmp_counter++; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + if (Downsample_Storage.size() > 0) + Rebuild_Logger.push(operation_delete); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + }; + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToAdd[i]; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + } + return tmp_counter; +} + +template +void KD_TREE::Add_Point_Boxes(vector &BoxPoints) +{ + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_range(&Root_Node, BoxPoints[i], true); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = ADD_BOX; + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&Root_Node, BoxPoints[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Delete_Points(PointVector &PointToDel) +{ + for (int i = 0; i < PointToDel.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Delete_by_point(&Root_Node, PointToDel[i], true); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToDel[i]; + operation.op = DELETE_POINT; + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&Root_Node, PointToDel[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +int KD_TREE::Delete_Point_Boxes(vector &BoxPoints) +{ + int tmp_counter = 0; + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = DELETE_BOX; + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return tmp_counter; +} + +template +void KD_TREE::acquire_removed_points(PointVector &removed_points) +{ + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + for (int i = 0; i < Points_deleted.size(); i++) + { + removed_points.push_back(Points_deleted[i]); + } + for (int i = 0; i < Multithread_Points_deleted.size(); i++) + { + removed_points.push_back(Multithread_Points_deleted[i]); + } + Points_deleted.clear(); + Multithread_Points_deleted.clear(); + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + return; +} + +template +void KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage) +{ + if (l > r) + return; + *root = new KD_TREE_NODE; + InitTreeNode(*root); + int mid = (l + r) >> 1; + int div_axis = 0; + int i; + // Find the best division Axis + float min_value[3] = {INFINITY, INFINITY, INFINITY}; + float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; + float dim_range[3] = {0, 0, 0}; + for (i = l; i <= r; i++) + { + min_value[0] = min(min_value[0], Storage[i].x); + min_value[1] = min(min_value[1], Storage[i].y); + min_value[2] = min(min_value[2], Storage[i].z); + max_value[0] = max(max_value[0], Storage[i].x); + max_value[1] = max(max_value[1], Storage[i].y); + max_value[2] = max(max_value[2], Storage[i].z); + } + // Select the longest dimension as division axis + for (i = 0; i < 3; i++) + dim_range[i] = max_value[i] - min_value[i]; + for (i = 1; i < 3; i++) + if (dim_range[i] > dim_range[div_axis]) + div_axis = i; + // Divide by the division axis and recursively build. + + (*root)->division_axis = div_axis; + switch (div_axis) + { + case 0: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + case 1: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y); + break; + case 2: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z); + break; + default: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + } + (*root)->point = Storage[mid]; + KD_TREE_NODE *left_son = nullptr, *right_son = nullptr; + BuildTree(&left_son, l, mid - 1, Storage); + BuildTree(&right_son, mid + 1, r, Storage); + (*root)->left_son_ptr = left_son; + (*root)->right_son_ptr = right_son; + Update((*root)); + return; +} + +template +void KD_TREE::Rebuild(KD_TREE_NODE **root) +{ + KD_TREE_NODE *father_ptr; + if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) + { + if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)) + { + if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) + { + Rebuild_Ptr = root; + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + } + } + else + { + father_ptr = (*root)->father_ptr; + int size_rec = (*root)->TreeSize; + PCL_Storage.clear(); + flatten(*root, PCL_Storage, DELETE_POINTS_REC); + delete_tree_nodes(root); + BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage); + if (*root != nullptr) + (*root)->father_ptr = father_ptr; + if (*root == Root_Node) + STATIC_ROOT_NODE->left_son_ptr = *root; + } + return; +} + +template +int KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return 0; + (*root)->working_flag = true; + Push_Down(*root); + int tmp_counter = 0; + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return 0; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return 0; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return 0; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = true; + (*root)->point_deleted = true; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; + (*root)->invalid_point_num = (*root)->TreeSize; + if (is_downsample) + { + (*root)->tree_downsample_deleted = true; + (*root)->point_downsample_deleted = true; + (*root)->down_del_num = (*root)->TreeSize; + } + return tmp_counter; + } + if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = true; + tmp_counter += 1; + if (is_downsample) + (*root)->point_downsample_deleted = true; + } + Operation_Logger_Type delete_box_log; + struct timespec Timeout; + if (is_downsample) + delete_box_log.op = DOWNSAMPLE_DELETE; + else + delete_box_log.op = DELETE_BOX; + delete_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return tmp_counter; +} + +template +void KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (same_point((*root)->point, point) && !(*root)->point_deleted) + { + (*root)->point_deleted = true; + (*root)->invalid_point_num += 1; + if ((*root)->invalid_point_num == (*root)->TreeSize) + (*root)->tree_deleted = true; + return; + } + Operation_Logger_Type delete_log; + struct timespec Timeout; + delete_log.op = DELETE_POINT; + delete_log.point = point; + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->left_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->right_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild) +{ + if ((*root) == nullptr) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; + (*root)->point_deleted = false || (*root)->point_downsample_deleted; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + (*root)->invalid_point_num = (*root)->down_del_num; + return; + } + if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = (*root)->point_downsample_deleted; + } + Operation_Logger_Type add_box_log; + struct timespec Timeout; + add_box_log.op = ADD_BOX; + add_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->left_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->right_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis) +{ + if (*root == nullptr) + { + *root = new KD_TREE_NODE; + InitTreeNode(*root); + (*root)->point = point; + (*root)->division_axis = (father_axis + 1) % 3; + Update(*root); + return; + } + (*root)->working_flag = true; + Operation_Logger_Type add_log; + struct timespec Timeout; + add_log.op = ADD_POINT; + add_log.point = point; + Push_Down(*root); + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist) +{ + if (root == nullptr || root->tree_deleted) + return; + float cur_dist = calc_box_dist(root, point); + float max_dist_sqr = max_dist * max_dist; + if (cur_dist > max_dist_sqr) + return; + int retval; + if (root->need_push_down_to_left || root->need_push_down_to_right) + { + retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); + if (retval == 0) + { + Push_Down(root); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + else + { + pthread_mutex_lock(&(root->push_down_mutex_lock)); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + } + if (!root->point_deleted) + { + float dist = calc_dist(point, root->point); + if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)) + { + if (q.size() >= k_nearest) + q.pop(); + PointType_CMP current_point{root->point, dist}; + q.push(current_point); + } + } + int cur_search_counter; + float dist_left_node = calc_box_dist(root->left_son_ptr, point); + float dist_right_node = calc_box_dist(root->right_son_ptr, point); + if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) + { + if (dist_left_node <= dist_right_node) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + } + else + { + if (dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + if (dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + return; +} + +template +void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z) + { + if (!root->point_deleted) + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->left_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->left_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->right_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->right_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +void KD_TREE::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + PointType range_center; + range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5; + range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5; + range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5; + float dist = sqrt(calc_dist(range_center, point)); + if (dist > radius + sqrt(root->radius_sq)) return; + if (dist <= radius - sqrt(root->radius_sq)) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){ + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->left_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->left_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->right_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->right_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +bool KD_TREE::Criterion_Check(KD_TREE_NODE *root) +{ + if (root->TreeSize <= Minimal_Unbalanced_Tree_Size) + { + return false; + } + float balance_evaluation = 0.0f; + float delete_evaluation = 0.0f; + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + delete_evaluation = float(root->invalid_point_num) / root->TreeSize; + balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1); + if (delete_evaluation > delete_criterion_param) + { + return true; + } + if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param) + { + return true; + } + return false; +} + +template +void KD_TREE::Push_Down(KD_TREE_NODE *root) +{ + if (root == nullptr) + return; + Operation_Logger_Type operation; + operation.op = PUSH_DOWN; + operation.tree_deleted = root->tree_deleted; + operation.tree_downsample_deleted = root->tree_downsample_deleted; + if (root->need_push_down_to_left && root->left_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_left = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_left = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + if (root->need_push_down_to_right && root->right_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_right = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_right = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Update(KD_TREE_NODE *root) +{ + KD_TREE_NODE *left_son_ptr = root->left_son_ptr; + KD_TREE_NODE *right_son_ptr = root->right_son_ptr; + float tmp_range_x[2] = {INFINITY, -INFINITY}; + float tmp_range_y[2] = {INFINITY, -INFINITY}; + float tmp_range_z[2] = {INFINITY, -INFINITY}; + // Update Tree Size + if (left_son_ptr != nullptr && right_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x); + tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x); + tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y); + tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y); + tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z); + tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (left_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (right_son_ptr != nullptr) + { + root->TreeSize = right_son_ptr->TreeSize + 1; + root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else + { + root->TreeSize = 1; + root->invalid_point_num = (root->point_deleted ? 1 : 0); + root->down_del_num = (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = root->point_downsample_deleted; + root->tree_deleted = root->point_deleted; + tmp_range_x[0] = root->point.x; + tmp_range_x[1] = root->point.x; + tmp_range_y[0] = root->point.y; + tmp_range_y[1] = root->point.y; + tmp_range_z[0] = root->point.z; + tmp_range_z[1] = root->point.z; + } + memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x)); + memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y)); + memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z)); + float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5; + float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5; + float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5; + root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L; + if (left_son_ptr != nullptr) + left_son_ptr->father_ptr = root; + if (right_son_ptr != nullptr) + right_son_ptr->father_ptr = root; + if (root == Root_Node && root->TreeSize > 3) + { + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1); + root->alpha_del = float(root->invalid_point_num) / root->TreeSize; + root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal; + } + return; +} + +template +void KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type) +{ + if (root == nullptr) + return; + Push_Down(root); + if (!root->point_deleted) + { + Storage.push_back(root->point); + } + flatten(root->left_son_ptr, Storage, storage_type); + flatten(root->right_son_ptr, Storage, storage_type); + switch (storage_type) + { + case NOT_RECORD: + break; + case DELETE_POINTS_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Points_deleted.push_back(root->point); + } + break; + case MULTI_THREAD_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Multithread_Points_deleted.push_back(root->point); + } + break; + default: + break; + } + return; +} + +template +void KD_TREE::delete_tree_nodes(KD_TREE_NODE **root) +{ + if (*root == nullptr) + return; + Push_Down(*root); + delete_tree_nodes(&(*root)->left_son_ptr); + delete_tree_nodes(&(*root)->right_son_ptr); + + pthread_mutex_destroy(&(*root)->push_down_mutex_lock); + delete *root; + *root = nullptr; + + return; +} + +template +bool KD_TREE::same_point(PointType a, PointType b) +{ + return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS); +} + +template +float KD_TREE::calc_dist(PointType a, PointType b) +{ + float dist = 0.0f; + dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); + return dist; +} + +template +float KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point) +{ + if (node == nullptr) + return INFINITY; + float min_dist = 0.0; + if (point.x < node->node_range_x[0]) + min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]); + if (point.x > node->node_range_x[1]) + min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]); + if (point.y < node->node_range_y[0]) + min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]); + if (point.y > node->node_range_y[1]) + min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]); + if (point.z < node->node_range_z[0]) + min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]); + if (point.z > node->node_range_z[1]) + min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]); + return min_dist; +} +template +bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; } +template +bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; } +template +bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; } + +// Manual heap + + + +// manual queue + + +// Manual Instatiations +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; + diff --git a/include/ikd-Tree/ikd_Tree.h b/include/ikd-Tree/ikd_Tree.h new file mode 100644 index 0000000..d4b302e --- /dev/null +++ b/include/ikd-Tree/ikd_Tree.h @@ -0,0 +1,344 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define EPSS 1e-6 +#define Minimal_Unbalanced_Tree_Size 10 +#define Multi_Thread_Rebuild_Point_Num 1500 +#define DOWNSAMPLE_SWITCH true +#define ForceRebuildPercentage 0.2 +#define Q_LEN 1000000 + +using namespace std; + +// typedef pcl::PointXYZINormal PointType; +// typedef vector> PointVector; + +struct BoxPointType +{ + float vertex_min[3]; + float vertex_max[3]; +}; + +enum operation_set +{ + ADD_POINT, + DELETE_POINT, + DELETE_BOX, + ADD_BOX, + DOWNSAMPLE_DELETE, + PUSH_DOWN +}; + +enum delete_point_storage_set +{ + NOT_RECORD, + DELETE_POINTS_REC, + MULTI_THREAD_REC +}; + +template +class KD_TREE +{ + // using MANUAL_Q_ = MANUAL_Q; + // using PointVector = std::vector; + + // using MANUAL_Q_ = MANUAL_Q; +public: + using PointVector = std::vector>; + using Ptr = std::shared_ptr>; + + struct KD_TREE_NODE + { + PointType point; + int division_axis; + int TreeSize = 1; + int invalid_point_num = 0; + int down_del_num = 0; + bool point_deleted = false; + bool tree_deleted = false; + bool point_downsample_deleted = false; + bool tree_downsample_deleted = false; + bool need_push_down_to_left = false; + bool need_push_down_to_right = false; + bool working_flag = false; + pthread_mutex_t push_down_mutex_lock; + float node_range_x[2], node_range_y[2], node_range_z[2]; + float radius_sq; + KD_TREE_NODE *left_son_ptr = nullptr; + KD_TREE_NODE *right_son_ptr = nullptr; + KD_TREE_NODE *father_ptr = nullptr; + // For paper data record + float alpha_del; + float alpha_bal; + }; + + struct Operation_Logger_Type + { + PointType point; + BoxPointType boxpoint; + bool tree_deleted, tree_downsample_deleted; + operation_set op; + }; + // static const PointType zeroP; + + struct PointType_CMP + { + PointType point; + float dist = 0.0; + PointType_CMP(PointType p = PointType(), float d = INFINITY) + { + this->point = p; + this->dist = d; + }; + bool operator<(const PointType_CMP &a) const + { + if (fabs(dist - a.dist) < 1e-10) + return point.x < a.point.x; + else + return dist < a.dist; + } + }; + + class MANUAL_HEAP + { + + public: + MANUAL_HEAP(int max_capacity = 100) + + { + cap = max_capacity; + heap = new PointType_CMP[max_capacity]; + heap_size = 0; + } + + ~MANUAL_HEAP() + { + delete[] heap; + } + void pop() + { + if (heap_size == 0) + return; + heap[0] = heap[heap_size - 1]; + heap_size--; + MoveDown(0); + return; + } + PointType_CMP top() + { + return heap[0]; + } + void push(PointType_CMP point) + { + if (heap_size >= cap) + return; + heap[heap_size] = point; + FloatUp(heap_size); + heap_size++; + return; + } + int size() + { + return heap_size; + } + void clear() + { + heap_size = 0; + return; + } + + private: + PointType_CMP *heap; + void MoveDown(int heap_index) + { + int l = heap_index * 2 + 1; + PointType_CMP tmp = heap[heap_index]; + while (l < heap_size) + { + if (l + 1 < heap_size && heap[l] < heap[l + 1]) + l++; + if (tmp < heap[l]) + { + heap[heap_index] = heap[l]; + heap_index = l; + l = heap_index * 2 + 1; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + void FloatUp(int heap_index) + { + int ancestor = (heap_index - 1) / 2; + PointType_CMP tmp = heap[heap_index]; + while (heap_index > 0) + { + if (heap[ancestor] < tmp) + { + heap[heap_index] = heap[ancestor]; + heap_index = ancestor; + ancestor = (heap_index - 1) / 2; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + int heap_size = 0; + int cap = 0; + }; + + class MANUAL_Q + { + private: + int head = 0, tail = 0, counter = 0; + Operation_Logger_Type q[Q_LEN]; + bool is_empty; + + public: + void pop() + { + if (counter == 0) + return; + head++; + head %= Q_LEN; + counter--; + if (counter == 0) + is_empty = true; + return; + } + Operation_Logger_Type front() + { + return q[head]; + } + Operation_Logger_Type back() + { + return q[tail]; + } + void clear() + { + head = 0; + tail = 0; + counter = 0; + is_empty = true; + return; + } + void push(Operation_Logger_Type op) + { + q[tail] = op; + counter++; + if (is_empty) + is_empty = false; + tail++; + tail %= Q_LEN; + } + bool empty() + { + return is_empty; + } + int size() + { + return counter; + } + }; + +private: + // Multi-thread Tree Rebuild + bool termination_flag = false; + bool rebuild_flag = false; + pthread_t rebuild_thread; + pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; + pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; + // queue Rebuild_Logger; + MANUAL_Q Rebuild_Logger; + PointVector Rebuild_PCL_Storage; + KD_TREE_NODE **Rebuild_Ptr = nullptr; + int search_mutex_counter = 0; + static void *multi_thread_ptr(void *arg); + void multi_thread_rebuild(); + void start_thread(); + void stop_thread(); + void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); + // KD Tree Functions and augmented variables + int Treesize_tmp = 0, Validnum_tmp = 0; + float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; + float delete_criterion_param = 0.5f; + float balance_criterion_param = 0.7f; + float downsample_size = 0.2f; + bool Delete_Storage_Disabled = false; + KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; + PointVector Points_deleted; + PointVector Downsample_Storage; + PointVector Multithread_Points_deleted; + void InitTreeNode(KD_TREE_NODE *root); + void Test_Lock_States(KD_TREE_NODE *root); + void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); + void Rebuild(KD_TREE_NODE **root); + int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); + void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); + void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); + void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); + void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue + void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); + void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); + bool Criterion_Check(KD_TREE_NODE *root); + void Push_Down(KD_TREE_NODE *root); + void Update(KD_TREE_NODE *root); + void delete_tree_nodes(KD_TREE_NODE **root); + void downsample(KD_TREE_NODE **root); + bool same_point(PointType a, PointType b); + float calc_dist(PointType a, PointType b); + float calc_box_dist(KD_TREE_NODE *node, PointType point); + static bool point_cmp_x(PointType a, PointType b); + static bool point_cmp_y(PointType a, PointType b); + static bool point_cmp_z(PointType a, PointType b); + +public: + KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); + ~KD_TREE(); + void Set_delete_criterion_param(float delete_param) + { + delete_criterion_param = delete_param; + } + void Set_balance_criterion_param(float balance_param) + { + balance_criterion_param = balance_param; + } + void set_downsample_param(float downsample_param) + { + downsample_size = downsample_param; + } + void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); + int size(); + int validnum(); + void root_alpha(float &alpha_bal, float &alpha_del); + void Build(PointVector point_cloud); + void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist = INFINITY); + void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); + void Radius_Search(PointType point, const float radius, PointVector &Storage); + int Add_Points(PointVector &PointToAdd, bool downsample_on); + void Add_Point_Boxes(vector &BoxPoints); + void Delete_Points(PointVector &PointToDel); + int Delete_Point_Boxes(vector &BoxPoints); + void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); + void acquire_removed_points(PointVector &removed_points); + BoxPointType tree_range(); + PointVector PCL_Storage; + KD_TREE_NODE *Root_Node = nullptr; + int max_queue_size = 0; +}; + +// template +// PointType KD_TREE::zeroP = PointType(0,0,0); diff --git a/include/so3_math.h b/include/so3_math.h new file mode 100755 index 0000000..d289aed --- /dev/null +++ b/include/so3_math.h @@ -0,0 +1,113 @@ +#ifndef SO3_MATH_H +#define SO3_MATH_H + +#include +#include + +// #include + +#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 + +template +Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) +{ + Eigen::Matrix skew_sym_mat; + skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; + return skew_sym_mat; +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix K; + + K << SKEW_SYM_MATRX(r_axis); + + T r_ang = ang_vel_norm * dt; + + /// Roderigous Tranformation + return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_ang); + + /// Roderigous Tranformation + return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; + } + else + { + return Eye3; + } +} + +/* Logrithm of a Rotation Matrix */ +template +Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); +} + +template +Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) +{ + T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); + bool singular = sy < 1e-6; + T x, y, z; + if(!singular) + { + x = atan2(rot(2, 1), rot(2, 2)); + y = atan2(-rot(2, 0), sy); + z = atan2(rot(1, 0), rot(0, 0)); + } + else + { + x = atan2(-rot(1, 2), rot(1, 1)); + y = atan2(-rot(2, 0), sy); + z = 0; + } + Eigen::Matrix ang(x, y, z); + return ang; +} + +#endif diff --git a/launch/gdb_debug_example.launch b/launch/gdb_debug_example.launch new file mode 100755 index 0000000..9e3789f --- /dev/null +++ b/launch/gdb_debug_example.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch new file mode 100755 index 0000000..40c5910 --- /dev/null +++ b/launch/mapping_avia.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/launch/mapping_horizon.launch b/launch/mapping_horizon.launch new file mode 100755 index 0000000..f63558e --- /dev/null +++ b/launch/mapping_horizon.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/launch/mapping_ouster64.launch b/launch/mapping_ouster64.launch new file mode 100755 index 0000000..3e21a05 --- /dev/null +++ b/launch/mapping_ouster64.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + diff --git a/launch/mapping_unilidar.launch b/launch/mapping_unilidar.launch new file mode 100755 index 0000000..5a5c43b --- /dev/null +++ b/launch/mapping_unilidar.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/launch/mapping_velody16.launch b/launch/mapping_velody16.launch new file mode 100755 index 0000000..8625770 --- /dev/null +++ b/launch/mapping_velody16.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100755 index 0000000..ac63bd6 --- /dev/null +++ b/package.xml @@ -0,0 +1,47 @@ + + + point_lio_unilidar + 0.0.0 + + + This is a modified version of LOAM which is original algorithm + is described in the following paper: + J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. + Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + + + Lingbo Meng + + BSD + + Lingbo Meng + + catkin + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + sensor_msgs + tf + pcl_ros + + message_generation + + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + tf + pcl_ros + + message_runtime + + rostest + rosbag + + + + diff --git a/rviz_cfg/.gitignore b/rviz_cfg/.gitignore new file mode 100755 index 0000000..e69de29 diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz new file mode 100755 index 0000000..9777907 --- /dev/null +++ b/rviz_cfg/loam_livox.rviz @@ -0,0 +1,367 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Grid1 + - /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 + - /Axes2 + - /Path1 + - /MarkerArray1/Namespaces1 + Splitter Ratio: 0.5094339847564697 + Tree Height: 865 + - 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 + Name: Time + SyncMode: 0 + SyncSource: currPoints +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz/Grid + Color: 85; 87; 83 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.5 + 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: FlatColor + 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.5 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.63232421875 + Min Value: -1.107768177986145 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 1000 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 1.5 + 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: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference 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.07999999821186066 + 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/XYOrbit + Distance: 10.007177352905273 + 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: -1.3620343208312988 + Y: -2.0393662452697754 + Z: -2.1677402401110157e-05 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7297971248626709 + Target Frame: aft_mapped + Yaw: 3.9784696102142334 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1016 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001750000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c7000000000000000000000001000001520000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e00000052fc0100000002fb0000000800540069006d006500000000000000073e0000041800fffffffb0000000800540069006d006501000000000000045000000000000000000000073e0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1854 + X: 1986 + Y: 27 diff --git a/src/Estimator.cpp b/src/Estimator.cpp new file mode 100755 index 0000000..5ece1f0 --- /dev/null +++ b/src/Estimator.cpp @@ -0,0 +1,436 @@ +// #include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp> +#include "Estimator.h" + +PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); +std::vector time_seq; +PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); +std::vector pbody_list; +std::vector Nearest_Points; +KD_TREE ikdtree; +std::vector pointSearchSqDis(NUM_MATCH_POINTS); +bool point_selected_surf[100000] = {0}; +std::vector crossmat_list; +int effct_feat_num = 0; +int k; +int idx; +esekfom::esekf kf_input; +esekfom::esekf kf_output; +state_input state_in; +state_output state_out; +input_ikfom input_in; +V3D angvel_avr, acc_avr; + +V3D Lidar_T_wrt_IMU(Zero3d); +M3D Lidar_R_wrt_IMU(Eye3d); + +typedef MTK::vect<3, double> vect3; +typedef MTK::SO3 SO3; +typedef MTK::S2 S2; +typedef MTK::vect<1, double> vect1; +typedef MTK::vect<2, double> vect2; + +Eigen::Matrix process_noise_cov_input() +{ + Eigen::Matrix 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::type cov = MTK::get_cov::type::Zero(); + // MTK::setDiagonal(cov, &process_noise_input::ng, gyr_cov_input);// 0.03 + // MTK::setDiagonal(cov, &process_noise_input::na, acc_cov_input); // *dt 0.01 0.01 * dt * dt 0.05 + // MTK::setDiagonal(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(cov, &process_noise_input::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +Eigen::Matrix process_noise_cov_output() +{ + Eigen::Matrix 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; + // MTK::get_cov::type cov = MTK::get_cov::type::Zero(); + // MTK::setDiagonal(cov, &process_noise_output::vel, vel_cov);// 0.03 + // MTK::setDiagonal(cov, &process_noise_output::ng, gyr_cov_output); // *dt 0.01 0.01 * dt * dt 0.05 + // MTK::setDiagonal(cov, &process_noise_output::na, acc_cov_output); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 + // MTK::setDiagonal(cov, &process_noise_output::nbg, b_gyr_cov); //0.001 0.05 0.0001/out 0.01 + // MTK::setDiagonal(cov, &process_noise_output::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +Eigen::Matrix get_f_input(state_input &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + vect3 a_inertial = s.rot.normalized() * (in.acc-s.ba); + 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 get_f_output(state_output &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 a_inertial = s.rot.normalized() * s.acc; + 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 df_dx_input(state_input &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::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.normalized().toRotationMatrix()*MTK::hat(acc_); + cov.template block<3, 3>(12, 18) = -s.rot.normalized().toRotationMatrix(); + // Eigen::Matrix vec = Eigen::Matrix::Zero(); + // Eigen::Matrix 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 df_dw_input(state_input &s, const input_ikfom &in) +// { +// Eigen::Matrix cov = Eigen::Matrix::Zero(); +// cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix(); +// cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); +// return cov; +// } + +Eigen::Matrix df_dx_output(state_output &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix()*MTK::hat(s.acc); + cov.template block<3, 3>(12, 18) = s.rot.normalized().toRotationMatrix(); + // Eigen::Matrix vec = Eigen::Matrix::Zero(); + // Eigen::Matrix 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 df_dw_output(state_output &s) +// { +// Eigen::Matrix cov = Eigen::Matrix::Zero(); +// cov.template block<3, 3>(12, 0) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(15, 3) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(18, 6) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(24, 9) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(27, 12) = Eigen::Matrix3d::Identity(); +// return cov; +// } + +vect3 SO3ToEuler(const SO3 &orient) +{ + Eigen::Matrix _ang; + Eigen::Vector4d q_data = orient.coeffs().transpose(); + //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; + double sqw = q_data[3]*q_data[3]; + double sqx = q_data[0]*q_data[0]; + double sqy = q_data[1]*q_data[1]; + double sqz = q_data[2]*q_data[2]; + double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor + double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; + + if (test > 0.49999*unit) { // singularity at north pole + + _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; + } + if (test < -0.49999*unit) { // singularity at south pole + _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; + } + + _ang << + std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), + std::asin (2*test/unit), + std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; +} + +void h_model_input(state_input &s, esekfom::dyn_share_modified &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]; + 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]; + + ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); //1.0); //, 3.0); // 2.236; + + 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 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3); + + if (p_body.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 = 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++) + { + 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.normalized() * p_body + s.offset_T_L_I; + p_imu_crossmat << SKEW_SYM_MATRX(point_imu); + V3D C(s.rot.conjugate().normalized() * norm_vec); + V3D A(p_imu_crossmat * C); + V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * 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.conjugate().normalized() * norm_vec); + V3D A(point_crossmat * C); + ekfom_data.h_x.block<1, 12>(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, esekfom::dyn_share_modified &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]; + 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]; + + ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); + + 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 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3); + + if (p_body.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 = 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++) + { + 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.normalized() * p_body + s.offset_T_L_I; + p_imu_crossmat << SKEW_SYM_MATRX(point_imu); + V3D C(s.rot.conjugate().normalized() * norm_vec); + V3D A(p_imu_crossmat * C); + V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * 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.conjugate().normalized() * norm_vec); + V3D A(point_crossmat * C); + // V3D A(point_crossmat * state.rot_end.transpose() * norm_vec); + ekfom_data.h_x.block<1, 12>(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 &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.normalized() * (kf_output.x_.offset_R_L_I.normalized() * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos; + } + else + { + p_global = kf_input.x_.rot.normalized() * (kf_input.x_.offset_R_L_I.normalized() * 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.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; + } + else + { + p_global = kf_input.x_.rot.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; + } + } + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; \ No newline at end of file diff --git a/src/Estimator.h b/src/Estimator.h new file mode 100755 index 0000000..ed189ba --- /dev/null +++ b/src/Estimator.h @@ -0,0 +1,118 @@ +#ifndef Estimator_H +#define Estimator_H + +#include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp> +#include "common_lib.h" +#include "parameters.h" +#include +#include +#include +#include +#include +#include + +extern PointCloudXYZI::Ptr normvec; //(new PointCloudXYZI(100000, 1)); +extern std::vector time_seq; +extern PointCloudXYZI::Ptr feats_down_body; +extern PointCloudXYZI::Ptr feats_down_world; +extern std::vector pbody_list; +extern std::vector Nearest_Points; +extern KD_TREE ikdtree; +extern std::vector pointSearchSqDis; +extern bool point_selected_surf[100000]; // = {0}; +extern std::vector crossmat_list; +extern int effct_feat_num; +extern int k; +extern int idx; +extern V3D angvel_avr, acc_avr; + +extern V3D Lidar_T_wrt_IMU; //(Zero3d); +extern M3D Lidar_R_wrt_IMU; //(Eye3d); + +typedef MTK::vect<3, double> vect3; +typedef MTK::SO3 SO3; +typedef MTK::S2 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 kf_input; +extern esekfom::esekf kf_output; +extern state_input state_in; +extern state_output state_out; +extern input_ikfom input_in; + +Eigen::Matrix process_noise_cov_input(); + +Eigen::Matrix 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 get_f_input(state_input &s, const input_ikfom &in); + +Eigen::Matrix get_f_output(state_output &s, const input_ikfom &in); + +Eigen::Matrix df_dx_input(state_input &s, const input_ikfom &in); + +// Eigen::Matrix df_dw_input(state_input &s, const input_ikfom &in); + +Eigen::Matrix df_dx_output(state_output &s, const input_ikfom &in); + +// Eigen::Matrix df_dw_output(state_output &s); + +vect3 SO3ToEuler(const SO3 &orient); + +void h_model_input(state_input &s, esekfom::dyn_share_modified &ekfom_data); + +void h_model_output(state_output &s, esekfom::dyn_share_modified &ekfom_data); + +void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified &ekfom_data); + +void pointBodyToWorld(PointType const * const pi, PointType * const po); + +const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);}; + +#endif \ No newline at end of file diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp new file mode 100755 index 0000000..ae71329 --- /dev/null +++ b/src/IMU_Processing.hpp @@ -0,0 +1,185 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#define MAX_INI_COUNT (100) + +class ImuProcess +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ImuProcess(); + ~ImuProcess(); + + void Reset(); + void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); + void Process(const MeasureGroup &meas, PointCloudXYZI::Ptr pcl_un_); + void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot); + + ofstream fout_imu; + + int lidar_type; + bool imu_en; + V3D mean_acc, gravity_; + bool imu_need_init_ = true; + bool b_first_frame_ = true; + bool gravity_align_ = false; + + private: + void IMU_init(const MeasureGroup &meas, int &N); + V3D mean_gyr; + int init_iter_num = 1; +}; + +ImuProcess::ImuProcess() + : b_first_frame_(true), imu_need_init_(true), gravity_align_(false) +{ + imu_en = true; + init_iter_num = 1; + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); +} + +ImuProcess::~ImuProcess() {} + +void ImuProcess::Reset() +{ + ROS_WARN("Reset ImuProcess"); + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + imu_need_init_ = true; + init_iter_num = 1; +} + +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; + ROS_ASSERT(meas.lidar != nullptr); + + + if (imu_need_init_) + { + + 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); + } + return; + } + if (!gravity_align_) + gravity_align_ = true; + *cur_pcl_un_ = *(meas.lidar); + return; + } + else + { + + if (!b_first_frame_) { + if (!gravity_align_) + gravity_align_ = true; + } + else{ + b_first_frame_ = false; + return; + } + + + *cur_pcl_un_ = *(meas.lidar); + return; + } +} + +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 **/ + + 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() / tmp_gravity.norm() / 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)); + } +} \ No newline at end of file diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp new file mode 100755 index 0000000..98ae6d0 --- /dev/null +++ b/src/laserMapping.cpp @@ -0,0 +1,1455 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +// #include +#include +#include +#include + +#include "IMU_Processing.hpp" +#include "parameters.h" +#include "Estimator.h" + +#define MAXN (720000) +#define PUBFRAME_PERIOD (20) + +const float MOV_THRESHOLD = 1.5f; + +mutex mtx_buffer; +condition_variable sig_buffer; + +string root_dir = ROOT_DIR; + +int feats_down_size = 0; +int time_log_counter = 0; +int scan_count = 0; +int publish_count = 0; + +int frame_ct = 0; +double time_update_last = 0.0; +double time_current = 0.0; +double time_predict_last_const = 0.0; +double t_last = 0.0; + +shared_ptr p_imu(new ImuProcess()); +bool init_map = false; +bool flg_first_scan = true; +PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI()); + +double T1[MAXN]; +double s_plot[MAXN]; +double s_plot2[MAXN]; +double s_plot3[MAXN]; +double s_plot11[MAXN]; + +double match_time = 0; +double solve_time = 0; +double propag_time = 0; +double update_time = 0; + +bool lidar_pushed = false; +bool flg_reset = false; +bool flg_exit = false; + +vector cub_needrm; + +deque lidar_buffer; +deque time_buffer; +deque imu_deque; + +PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_body_space(new PointCloudXYZI()); +PointCloudXYZI::Ptr init_feats_world(new PointCloudXYZI()); + +pcl::VoxelGrid downSizeFilterSurf; +pcl::VoxelGrid downSizeFilterMap; + +V3D euler_cur; + +MeasureGroup Measures; + +sensor_msgs::Imu imu_last, imu_next; +sensor_msgs::Imu::ConstPtr imu_last_ptr; +nav_msgs::Path path; +nav_msgs::Odometry odomAftMapped; +geometry_msgs::PoseStamped msg_body_pose; + + +void SigHandle(int sig) +{ + flg_exit = true; + ROS_WARN("catch sig %d", sig); + sig_buffer.notify_all(); +} + + +inline void dump_lio_state_to_log(FILE *fp) +{ + V3D rot_ang; + if (!use_imu_as_input) + { + rot_ang = SO3ToEuler(kf_output.x_.rot); + } + else + { + rot_ang = SO3ToEuler(kf_input.x_.rot); + } + + fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); + fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); + if (use_imu_as_input) + { + + fprintf(fp, "%lf %lf %lf ", kf_input.x_.pos(0), kf_input.x_.pos(1), kf_input.x_.pos(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.vel(0), kf_input.x_.vel(1), kf_input.x_.vel(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.bg(0), kf_input.x_.bg(1), kf_input.x_.bg(2)); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.ba(0), kf_input.x_.ba(1), kf_input.x_.ba(2)); + fprintf(fp, "%lf %lf %lf ", kf_input.x_.gravity(0), kf_input.x_.gravity(1), kf_input.x_.gravity(2)); + } + else + { + fprintf(fp, "%lf %lf %lf ", kf_output.x_.pos(0), kf_output.x_.pos(1), kf_output.x_.pos(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.vel(0), kf_output.x_.vel(1), kf_output.x_.vel(2)); + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.bg(0), kf_output.x_.bg(1), kf_output.x_.bg(2)); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.ba(0), kf_output.x_.ba(1), kf_output.x_.ba(2)); + fprintf(fp, "%lf %lf %lf ", kf_output.x_.gravity(0), kf_output.x_.gravity(1), kf_output.x_.gravity(2)); + } + fprintf(fp, "\r\n"); + fflush(fp); +} + + +void pointBodyLidarToIMU(PointType const *const pi, PointType *const po) +{ + + V3D p_body_lidar(pi->x, pi->y, pi->z); + V3D p_body_imu; + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + p_body_imu = kf_output.x_.offset_R_L_I.normalized() * p_body_lidar + kf_output.x_.offset_T_L_I; + } + else + { + p_body_imu = kf_input.x_.offset_R_L_I.normalized() * p_body_lidar + kf_input.x_.offset_T_L_I; + } + } + else + { + p_body_imu = Lidar_R_wrt_IMU * p_body_lidar + Lidar_T_wrt_IMU; + } + + po->x = p_body_imu(0); + po->y = p_body_imu(1); + po->z = p_body_imu(2); + + po->intensity = pi->intensity; +} + +int points_cache_size = 0; +void points_cache_collect() +{ + PointVector points_history; + ikdtree.acquire_removed_points(points_history); + points_cache_size = points_history.size(); +} + + +BoxPointType LocalMap_Points; +bool Localmap_Initialized = false; +void lasermap_fov_segment() +{ + cub_needrm.shrink_to_fit(); + + V3D pos_LiD; + if (use_imu_as_input) + { + pos_LiD = kf_input.x_.pos + kf_input.x_.rot.normalized() * Lidar_T_wrt_IMU; + } + else + { + pos_LiD = kf_output.x_.pos + kf_output.x_.rot.normalized() * Lidar_T_wrt_IMU; + } + + if (!Localmap_Initialized) + { + for (int i = 0; i < 3; i++) + { + LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; + LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; + } + Localmap_Initialized = true; + return; + } + + float dist_to_map_edge[3][2]; + bool need_move = false; + for (int i = 0; i < 3; i++) + { + dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); + dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) + need_move = true; + } + + if (!need_move) + return; + BoxPointType New_LocalMap_Points, tmp_boxpoints; + New_LocalMap_Points = LocalMap_Points; + float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1))); + for (int i = 0; i < 3; i++) + { + tmp_boxpoints = LocalMap_Points; + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) + { + New_LocalMap_Points.vertex_max[i] -= mov_dist; + New_LocalMap_Points.vertex_min[i] -= mov_dist; + tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; + cub_needrm.emplace_back(tmp_boxpoints); + } + else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) + { + New_LocalMap_Points.vertex_max[i] += mov_dist; + New_LocalMap_Points.vertex_min[i] += mov_dist; + tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; + cub_needrm.emplace_back(tmp_boxpoints); + } + } + LocalMap_Points = New_LocalMap_Points; + + points_cache_collect(); + if (cub_needrm.size() > 0) + int kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); +} + + +void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + // std::cout << "standard_pcl_cbk() run once!\n"; + + 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"); + + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + last_timestamp_lidar = msg->header.stamp.toSec(); + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + + double time_div = msg->header.stamp.toSec(); + + p_pre->process(msg, ptr); + + // std::cout << "ptr->points.size() = " << ptr->points.size() << std::endl; + + if (cut_frame) + { + + sort(ptr->points.begin(), ptr->points.end(), time_list); + + for (int i = 0; i < ptr->size(); i++) + { + + ptr_div->push_back(ptr->points[i]); + + if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) + { + if (ptr_div->size() < 1) + continue; + + PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + *ptr_div_i = *ptr_div; + + lidar_buffer.push_back(ptr_div_i); + + time_buffer.push_back(time_div); + time_div += ptr->points[i].curvature / double(1000); + ptr_div->clear(); + } + } + + if (!ptr_div->empty()) + { + lidar_buffer.push_back(ptr_div); + + time_buffer.push_back(time_div); + } + } + else if (con_frame) + { + + if (frame_ct == 0) + { + time_con = last_timestamp_lidar; + } + + if (frame_ct < con_frame_num) + { + 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()); + *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; +// } + +// last_timestamp_lidar = msg->header.stamp.toSec(); + +// PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); +// PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + +// p_pre->process(msg, ptr); +// double time_div = msg->header.stamp.toSec(); + +// if (cut_frame) +// { + +// sort(ptr->points.begin(), ptr->points.end(), time_list); + +// for (int i = 0; i < ptr->size(); i++) +// { + +// ptr_div->push_back(ptr->points[i]); +// if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) +// { +// if (ptr_div->size() < 1) +// continue; +// PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + +// *ptr_div_i = *ptr_div; + +// lidar_buffer.push_back(ptr_div_i); +// time_buffer.push_back(time_div); +// time_div += ptr->points[i].curvature / double(1000); +// ptr_div->clear(); +// } +// } +// if (!ptr_div->empty()) +// { +// lidar_buffer.push_back(ptr_div); + +// time_buffer.push_back(time_div); +// } +// } +// else if (con_frame) +// { +// if (frame_ct == 0) +// { + +// time_con = last_timestamp_lidar; +// } +// if (frame_ct < con_frame_num) +// { +// 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()); +// *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) +{ + + publish_count++; + + sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); + + msg->header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_lag_imu_to_lidar); + + double timestamp = msg->header.stamp.toSec(); + + mtx_buffer.lock(); + + if (timestamp < last_timestamp_imu) + { + ROS_ERROR("imu loop back, clear deque"); + + 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()) + { + + meas.lidar = lidar_buffer.front(); + meas.lidar_beg_time = time_buffer.front(); + time_buffer.pop_front(); + lidar_buffer.pop_front(); + + if (meas.lidar->points.size() < 1) + { + cout << "lose lidar" << std::endl; + return false; + } + + 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; + + return true; + } + return false; + } + + if (lidar_buffer.empty() || imu_deque.empty()) + { + return false; + } + + /*** push a lidar scan ***/ + if (!lidar_pushed) + { + + meas.lidar = lidar_buffer.front(); + + if (meas.lidar->points.size() < 1) + { + cout << "lose lidar" << endl; + lidar_buffer.pop_front(); + time_buffer.pop_front(); + return false; + } + + meas.lidar_beg_time = time_buffer.front(); + + 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; + } + + if (last_timestamp_imu < lidar_end_time) + { + return false; + } + + /*** 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(); + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_deque.front()->header.stamp.toSec(); + if (imu_time > lidar_end_time) + break; + meas.imu.emplace_back(imu_deque.front()); + imu_last = imu_next; + imu_last_ptr = imu_deque.front(); + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + } + else if (!init_map) + { + double imu_time = imu_deque.front()->header.stamp.toSec(); + meas.imu.shrink_to_fit(); + meas.imu.emplace_back(imu_last_ptr); + + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_deque.front()->header.stamp.toSec(); + if (imu_time > lidar_end_time) + break; + meas.imu.emplace_back(imu_deque.front()); + imu_last = imu_next; + imu_last_ptr = imu_deque.front(); + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + } + + lidar_buffer.pop_front(); + time_buffer.pop_front(); + lidar_pushed = false; + return true; +} + + +int process_increments = 0; +void map_incremental() +{ + PointVector PointToAdd; + PointVector PointNoNeedDownsample; + PointToAdd.reserve(feats_down_size); + PointNoNeedDownsample.reserve(feats_down_size); + + for (int i = 0; i < feats_down_size; i++) + { + if (!Nearest_Points[i].empty()) + { + const PointVector &points_near = Nearest_Points[i]; + bool need_add = true; + PointType downsample_result, mid_point; + mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min + 0.5 * filter_size_map_min; + /* If the nearest points is definitely outside the downsample box */ + if (fabs(points_near[0].x - mid_point.x) > 1.732 * filter_size_map_min || fabs(points_near[0].y - mid_point.y) > 1.732 * filter_size_map_min || fabs(points_near[0].z - mid_point.z) > 1.732 * filter_size_map_min) + { + PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); + continue; + } + /* Check if there is a point already in the downsample box */ + float dist = calc_dist(feats_down_world->points[i], mid_point); + for (int readd_i = 0; readd_i < points_near.size(); readd_i++) + { + /* Those points which are outside the downsample box should not be considered. */ + if (fabs(points_near[readd_i].x - mid_point.x) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].y - mid_point.y) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].z - mid_point.z) < 0.5 * filter_size_map_min) + { + need_add = false; + break; + } + } + if (need_add) + PointToAdd.emplace_back(feats_down_world->points[i]); + } + else + { + + PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); + } + } + int add_point_size = ikdtree.Add_Points(PointToAdd, true); + ikdtree.Add_Points(PointNoNeedDownsample, false); +} + +void publish_init_kdtree(const ros::Publisher &pubLaserCloudFullRes) +{ + int size_init_ikdtree = ikdtree.size(); + PointCloudXYZI::Ptr laserCloudInit(new PointCloudXYZI(size_init_ikdtree, 1)); + + sensor_msgs::PointCloud2 laserCloudmsg; + PointVector().swap(ikdtree.PCL_Storage); + ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); + + laserCloudInit->points = ikdtree.PCL_Storage; + pcl::toROSMsg(*laserCloudInit, laserCloudmsg); + + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); +} + + +PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); +PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); +void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes) +{ + if (scan_pub_en) + { + PointCloudXYZI::Ptr laserCloudFullRes(feats_down_body); + int size = laserCloudFullRes->points.size(); + + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + + laserCloudWorld->points[i].x = feats_down_world->points[i].x; + laserCloudWorld->points[i].y = feats_down_world->points[i].y; + laserCloudWorld->points[i].z = feats_down_world->points[i].z; + laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; + } + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); + + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; + } + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + if (pcd_save_en) + { + int size = feats_down_world->points.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + laserCloudWorld->points[i].x = feats_down_world->points[i].x; + laserCloudWorld->points[i].y = feats_down_world->points[i].y; + laserCloudWorld->points[i].z = feats_down_world->points[i].z; + laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; + } + + *pcl_wait_save += *laserCloudWorld; + + static int scan_wait_num = 0; + scan_wait_num++; + if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + pcd_index++; + string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd")); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << all_points_dir << endl; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + pcl_wait_save->clear(); + scan_wait_num = 0; + } + } +} + + +void publish_frame_body(const ros::Publisher &pubLaserCloudFull_body) +{ + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + pointBodyLidarToIMU(&feats_undistort->points[i], + &laserCloudIMUBody->points[i]); + } + + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "body"; + pubLaserCloudFull_body.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; +} + +template +void set_posestamp(T &out) +{ + if (!use_imu_as_input) + { + out.position.x = kf_output.x_.pos(0); + out.position.y = kf_output.x_.pos(1); + out.position.z = kf_output.x_.pos(2); + out.orientation.x = kf_output.x_.rot.coeffs()[0]; + out.orientation.y = kf_output.x_.rot.coeffs()[1]; + out.orientation.z = kf_output.x_.rot.coeffs()[2]; + out.orientation.w = kf_output.x_.rot.coeffs()[3]; + } + else + { + out.position.x = kf_input.x_.pos(0); + out.position.y = kf_input.x_.pos(1); + out.position.z = kf_input.x_.pos(2); + out.orientation.x = kf_input.x_.rot.coeffs()[0]; + out.orientation.y = kf_input.x_.rot.coeffs()[1]; + out.orientation.z = kf_input.x_.rot.coeffs()[2]; + out.orientation.w = kf_input.x_.rot.coeffs()[3]; + } +} + +void publish_odometry(const ros::Publisher &pubOdomAftMapped) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "aft_mapped"; + if (publish_odometry_without_downsample) + { + odomAftMapped.header.stamp = ros::Time().fromSec(time_current); + } + else + { + odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); + } + set_posestamp(odomAftMapped.pose.pose); + + pubOdomAftMapped.publish(odomAftMapped); + + static tf::TransformBroadcaster br; + tf::Transform transform; + tf::Quaternion q; + transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, + odomAftMapped.pose.pose.position.y, + odomAftMapped.pose.pose.position.z)); + q.setW(odomAftMapped.pose.pose.orientation.w); + q.setX(odomAftMapped.pose.pose.orientation.x); + q.setY(odomAftMapped.pose.pose.orientation.y); + q.setZ(odomAftMapped.pose.pose.orientation.z); + transform.setRotation(q); + br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped")); +} + +void publish_path(const ros::Publisher pubPath) +{ + set_posestamp(msg_body_pose.pose); + + msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); + msg_body_pose.header.frame_id = "camera_init"; + static int jjj = 0; + jjj++; + + { + path.poses.emplace_back(msg_body_pose); + pubPath.publish(path); + } +} + +int main(int argc, char **argv) +{ + + ros::init(argc, argv, "laserMapping"); + ros::NodeHandle nh("~"); + + readParameters(nh); + cout << "lidar_type: " << lidar_type << endl; + + path.header.stamp = ros::Time().fromSec(lidar_end_time); + path.header.frame_id = "camera_init"; + + int frame_num = 0; + double aver_time_consu = 0, + aver_time_icp = 0, + aver_time_match = 0, + aver_time_incre = 0, + aver_time_solve = 0, + aver_time_propag = 0; + std::time_t startTime, endTime; + + double FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); + double HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); + + memset(point_selected_surf, true, sizeof(point_selected_surf)); + + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + + Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT); + Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR); + + if (extrinsic_est_en) + { + + if (!use_imu_as_input) + { + kf_output.x_.offset_R_L_I = Lidar_R_wrt_IMU; + kf_output.x_.offset_T_L_I = Lidar_T_wrt_IMU; + } + + else + { + kf_input.x_.offset_R_L_I = Lidar_R_wrt_IMU; + kf_input.x_.offset_T_L_I = Lidar_T_wrt_IMU; + } + } + + p_imu->lidar_type = p_pre->lidar_type = lidar_type; + p_imu->imu_en = imu_en; + + kf_input.init_dyn_share_modified(get_f_input, df_dx_input, h_model_input); + kf_output.init_dyn_share_modified_2h(get_f_output, df_dx_output, h_model_output, h_model_IMU_output); + + Eigen::Matrix P_init = MD(24, 24)::Identity() * 0.01; + 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; + P_init.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001; + kf_input.change_P(P_init); + + Eigen::Matrix 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; + kf_input.change_P(P_init); + kf_output.change_P(P_init_output); + + Eigen::Matrix Q_input = process_noise_cov_input(); + Eigen::Matrix Q_output = process_noise_cov_output(); + + /*** debug record ***/ + FILE *fp; + string pos_log_dir = root_dir + "/Log/pos_log.txt"; + fp = fopen(pos_log_dir.c_str(), "w"); + + ofstream fout_out, fout_imu_pbp; + 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; + + /*** ROS subscribe initialization ***/ + + // ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : nh.subscribe(lid_topic, 200000, standard_pcl_cbk); + + ros::Subscriber sub_pcl = nh.subscribe(lid_topic, 200000, standard_pcl_cbk); + + ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); + + ros::Publisher pubLaserCloudFullRes = nh.advertise("/cloud_registered", 100000); + + ros::Publisher pubLaserCloudFullRes_body = nh.advertise("/cloud_registered_body", 100000); + + ros::Publisher pubLaserCloudEffect = nh.advertise("/cloud_effected", 100000); + + ros::Publisher pubLaserCloudMap = nh.advertise("/Laser_map", 100000); + + ros::Publisher pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 100000); + + ros::Publisher pubPath = nh.advertise("/path", 100000); + + ros::Publisher plane_pub = nh.advertise("/planner_normal", 1000); + + signal(SIGINT, SigHandle); + + ros::Rate rate(5000); + while (ros::ok()) + { + + if (flg_exit) + break; + + ros::spinOnce(); + + if (sync_packages(Measures) == false) + { + rate.sleep(); + continue; + } + + if (flg_first_scan) + { + first_lidar_time = Measures.lidar_beg_time; + flg_first_scan = false; + cout << "first lidar time" << first_lidar_time << endl; + } + + if (flg_reset) + { + ROS_WARN("reset when rosbag play back"); + p_imu->Reset(); + flg_reset = false; + continue; + } + + double t0, t1, t2, t3, t4, t5, match_start, solve_start; + match_time = 0; + solve_time = 0; + propag_time = 0; + update_time = 0; + t0 = omp_get_wtime(); + + p_imu->Process(Measures, feats_undistort); + + if (feats_undistort->empty() || feats_undistort == NULL) + { + continue; + } + + if (imu_en) + { + if (!p_imu->gravity_align_) + { + while (Measures.lidar_beg_time > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + if (non_station_start) + { + state_in.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.acc << VEC_FROM_ARRAY(gravity_init); + state_out.acc *= -1; + } + else + { + state_in.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + state_out.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + state_out.acc = p_imu->mean_acc * G_m_s2 / acc_norm; + } + if (gravity_align) + { + Eigen::Matrix3d rot_init; + p_imu->gravity_ << VEC_FROM_ARRAY(gravity); + p_imu->Set_init(state_in.gravity, rot_init); + state_in.gravity = state_out.gravity = p_imu->gravity_; + state_in.rot = state_out.rot = rot_init; + state_in.rot.normalize(); + state_out.rot.normalize(); + state_out.acc = -rot_init.transpose() * state_out.gravity; + } + kf_input.change_x(state_in); + kf_output.change_x(state_out); + } + } + else + { + if (!p_imu->gravity_align_) + { + state_in.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.acc << VEC_FROM_ARRAY(gravity_init); + state_out.acc *= -1; + } + } + + /*** Segment the map in lidar FOV ***/ + lasermap_fov_segment(); + + t1 = omp_get_wtime(); + if (space_down_sample) + { + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + } + else + { + feats_down_body = Measures.lidar; + + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + } + time_seq = time_compressing(feats_down_body); + feats_down_size = feats_down_body->points.size(); + + /*** initialize the map kdtree ***/ + if (!init_map) + { + if (ikdtree.Root_Node == nullptr) + + { + ikdtree.set_downsample_param(filter_size_map_min); + } + + feats_down_world->resize(feats_down_size); + for (int i = 0; i < feats_down_size; i++) + { + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + } + + for (size_t i = 0; i < feats_down_world->size(); i++) + { + init_feats_world->points.emplace_back(feats_down_world->points[i]); + } + + if (init_feats_world->size() < init_map_size) + continue; + + ikdtree.Build(init_feats_world->points); + init_map = true; + + publish_init_kdtree(pubLaserCloudMap); + continue; + } + + /*** ICP and Kalman filter update ***/ + + normvec->resize(feats_down_size); + feats_down_world->resize(feats_down_size); + + Nearest_Points.resize(feats_down_size); + + t2 = omp_get_wtime(); + + /*** iterated state estimation ***/ + + crossmat_list.reserve(feats_down_size); + pbody_list.reserve(feats_down_size); + + for (size_t i = 0; i < feats_down_body->size(); i++) + { + + V3D point_this(feats_down_body->points[i].x, + feats_down_body->points[i].y, + feats_down_body->points[i].z); + pbody_list[i] = point_this; + + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + + point_this = kf_output.x_.offset_R_L_I.normalized() * point_this + kf_output.x_.offset_T_L_I; + } + else + { + + point_this = kf_input.x_.offset_R_L_I.normalized() * point_this + kf_input.x_.offset_T_L_I; + } + } + else + { + point_this = Lidar_R_wrt_IMU * point_this + Lidar_T_wrt_IMU; + } + + + M3D point_crossmat; + point_crossmat << SKEW_SYM_MATRX(point_this); + crossmat_list[i]=point_crossmat; + } + + if (!use_imu_as_input) + { + bool imu_upda_cov = false; + effct_feat_num = 0; + + /**** point by point update ****/ + + double pcl_beg_time = Measures.lidar_beg_time; + idx = -1; + for (k = 0; k < time_seq.size(); k++) + { + + PointType &point_body = feats_down_body->points[idx + time_seq[k]]; + + time_current = point_body.curvature / 1000.0 + pcl_beg_time; + + if (is_first_frame) + { + if (imu_en) + { + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + + angvel_avr << imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z; + acc_avr << imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z; + } + is_first_frame = false; + imu_upda_cov = true; + time_update_last = time_current; + time_predict_last_const = time_current; + } + + if (imu_en) + { + bool imu_comes = time_current > imu_next.header.stamp.toSec(); + while (imu_comes) + { + imu_upda_cov = true; + angvel_avr << imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z; + acc_avr << imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z; + + /*** covariance update ***/ + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + double dt = imu_last.header.stamp.toSec() - time_predict_last_const; + kf_output.predict(dt, Q_output, input_in, true, false); + time_predict_last_const = imu_last.header.stamp.toSec(); + imu_comes = time_current > imu_next.header.stamp.toSec(); + + { + double dt_cov = imu_last.header.stamp.toSec() - time_update_last; + + if (dt_cov > 0.0) + { + time_update_last = imu_last.header.stamp.toSec(); + double propag_imu_start = omp_get_wtime(); + + kf_output.predict(dt_cov, Q_output, input_in, false, true); + + propag_time += omp_get_wtime() - propag_imu_start; + double solve_imu_start = omp_get_wtime(); + kf_output.update_iterated_dyn_share_IMU(); + solve_time += omp_get_wtime() - solve_imu_start; + } + } + } + } + + double dt = time_current - time_predict_last_const; + double propag_state_start = omp_get_wtime(); + if (!prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (dt_cov > 0.0) + { + kf_output.predict(dt_cov, Q_output, input_in, false, true); + time_update_last = time_current; + } + } + kf_output.predict(dt, Q_output, input_in, true, false); + propag_time += omp_get_wtime() - propag_state_start; + time_predict_last_const = time_current; + + double t_update_start = omp_get_wtime(); + + if (feats_down_size < 1) + { + ROS_WARN("No point, skip this scan!\n"); + idx += time_seq[k]; + continue; + } + if (!kf_output.update_iterated_dyn_share_modified()) + { + idx = idx + time_seq[k]; + continue; + } + + if (prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (!imu_en && (dt_cov >= imu_time_inte)) + { + double propag_cov_start = omp_get_wtime(); + kf_output.predict(dt_cov, Q_output, input_in, false, true); + imu_upda_cov = false; + time_update_last = time_current; + propag_time += omp_get_wtime() - propag_cov_start; + } + } + + solve_start = omp_get_wtime(); + + if (publish_odometry_without_downsample) + { + /******* Publish odometry *******/ + + publish_odometry(pubOdomAftMapped); + if (runtime_pos_log) + { + state_out = kf_output.x_; + euler_cur = SO3ToEuler(state_out.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() + << " " << state_out.omg.transpose() << " " << state_out.acc.transpose() << " " << state_out.gravity.transpose() << " " << state_out.bg.transpose() << " " << state_out.ba.transpose() << " " << feats_undistort->points.size() << endl; + } + } + + 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); + } + + solve_time += omp_get_wtime() - solve_start; + + update_time += omp_get_wtime() - t_update_start; + idx += time_seq[k]; + } + } + else + { + bool imu_prop_cov = false; + effct_feat_num = 0; + + double pcl_beg_time = Measures.lidar_beg_time; + idx = -1; + for (k = 0; k < time_seq.size(); k++) + { + PointType &point_body = feats_down_body->points[idx + time_seq[k]]; + time_current = point_body.curvature / 1000.0 + pcl_beg_time; + if (is_first_frame) + { + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + imu_prop_cov = true; + + is_first_frame = false; + t_last = time_current; + time_update_last = time_current; + + { + input_in.gyro << imu_last.angular_velocity.x, + imu_last.angular_velocity.y, + imu_last.angular_velocity.z; + + input_in.acc << imu_last.linear_acceleration.x, + imu_last.linear_acceleration.y, + imu_last.linear_acceleration.z; + + input_in.acc = input_in.acc * G_m_s2 / acc_norm; + } + } + + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + input_in.gyro << imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z; + input_in.acc << imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z; + + input_in.acc = input_in.acc * G_m_s2 / acc_norm; + double dt = imu_last.header.stamp.toSec() - t_last; + + double dt_cov = imu_last.header.stamp.toSec() - time_update_last; + if (dt_cov > 0.0) + { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = imu_last.header.stamp.toSec(); + } + kf_input.predict(dt, Q_input, input_in, true, false); + t_last = imu_last.header.stamp.toSec(); + imu_prop_cov = true; + } + + double dt = time_current - t_last; + t_last = time_current; + double propag_start = omp_get_wtime(); + + if (!prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (dt_cov > 0.0) + { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = time_current; + } + } + kf_input.predict(dt, Q_input, input_in, true, false); + + propag_time += omp_get_wtime() - propag_start; + + double t_update_start = omp_get_wtime(); + + if (feats_down_size < 1) + { + ROS_WARN("No point, skip this scan!\n"); + + idx += time_seq[k]; + continue; + } + if (!kf_input.update_iterated_dyn_share_modified()) + { + idx = idx + time_seq[k]; + continue; + } + + solve_start = omp_get_wtime(); + + if (publish_odometry_without_downsample) + { + /******* Publish odometry *******/ + + publish_odometry(pubOdomAftMapped); + if (runtime_pos_log) + { + state_in = kf_input.x_; + euler_cur = SO3ToEuler(state_in.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_in.pos.transpose() << " " << state_in.vel.transpose() + << " " << state_in.bg.transpose() << " " << state_in.ba.transpose() << " " << state_in.gravity.transpose() << " " << feats_undistort->points.size() << endl; + } + } + + 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); + } + solve_time += omp_get_wtime() - solve_start; + + update_time += omp_get_wtime() - t_update_start; + idx = idx + time_seq[k]; + } + } + + /******* Publish odometry downsample *******/ + if (!publish_odometry_without_downsample) + { + publish_odometry(pubOdomAftMapped); + } + + /*** add the feature points to map kdtree ***/ + t3 = omp_get_wtime(); + + if (feats_down_size > 4) + { + map_incremental(); + } + + t5 = omp_get_wtime(); + + /******* Publish points *******/ + + if (path_en) + publish_path(pubPath); + + if (scan_pub_en || pcd_save_en) + publish_frame_world(pubLaserCloudFullRes); + + if (scan_pub_en && scan_body_pub_en) + publish_frame_body(pubLaserCloudFullRes_body); + + /*** Debug variables Logging ***/ + if (runtime_pos_log) + { + frame_num++; + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; + { + aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + update_time / frame_num; + } + aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num; + aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + solve_time / frame_num; + aver_time_propag = aver_time_propag * (frame_num - 1) / frame_num + propag_time / frame_num; + T1[time_log_counter] = Measures.lidar_beg_time; + s_plot[time_log_counter] = t5 - t0; + s_plot2[time_log_counter] = feats_undistort->points.size(); + s_plot3[time_log_counter] = aver_time_consu; + time_log_counter++; + printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f propogate: %0.6f \n", t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, aver_time_propag); + if (!publish_odometry_without_downsample) + { + if (!use_imu_as_input) + { + state_out = kf_output.x_; + euler_cur = SO3ToEuler(state_out.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() + << " " << state_out.omg.transpose() << " " << state_out.acc.transpose() << " " << state_out.gravity.transpose() << " " << state_out.bg.transpose() << " " << state_out.ba.transpose() << " " << feats_undistort->points.size() << endl; + } + else + { + state_in = kf_input.x_; + euler_cur = SO3ToEuler(state_in.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_in.pos.transpose() << " " << state_in.vel.transpose() + << " " << state_in.bg.transpose() << " " << state_in.ba.transpose() << " " << state_in.gravity.transpose() << " " << feats_undistort->points.size() << endl; + } + } + dump_lio_state_to_log(fp); + } + + rate.sleep(); + } + + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + + if (pcl_wait_save->size() > 0 && pcd_save_en) + { + string file_name = string("scans.pcd"); + string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); + std::cout << "Saving map to file: " << all_points_dir << std::endl; + pcl::PCDWriter pcd_writer; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + } + fout_out.close(); + fout_imu_pbp.close(); + + return 0; +} diff --git a/src/parameters.cpp b/src/parameters.cpp new file mode 100755 index 0000000..8cba499 --- /dev/null +++ b/src/parameters.cpp @@ -0,0 +1,141 @@ +#include "parameters.h" + +bool is_first_frame = true; + +double lidar_end_time = 0.0; +double first_lidar_time = 0.0; +double time_con = 0.0; + +double last_timestamp_lidar = -1.0; +double last_timestamp_imu = -1.0; + +int pcd_index = 0; + +std::string lid_topic; +std::string imu_topic; + +bool prop_at_freq_of_imu; +bool check_satu; +bool con_frame; +bool cut_frame; + +bool use_imu_as_input; +bool space_down_sample; +bool publish_odometry_without_downsample; + +int init_map_size; +int con_frame_num; + +double match_s; +double satu_acc; +double satu_gyro; +double cut_frame_time_interval; + +float plane_thr; + +double filter_size_surf_min; +double filter_size_map_min; +double fov_deg; + +double cube_len; +float DET_RANGE; + +bool imu_en; +bool gravity_align; +bool non_station_start; + +double imu_time_inte; + +double laser_point_cov; +double acc_norm; + +double vel_cov; +double acc_cov_input; +double gyr_cov_input; + +double gyr_cov_output; +double acc_cov_output; +double b_gyr_cov; +double b_acc_cov; + +double imu_meas_acc_cov; +double imu_meas_omg_cov; + +int lidar_type; +int pcd_save_interval; + +std::vector gravity_init; +std::vector gravity; + +std::vector extrinT; +std::vector extrinR; + +bool runtime_pos_log; +bool pcd_save_en; +bool path_en; +bool extrinsic_est_en = true; + +bool scan_pub_en; +bool scan_body_pub_en; + +shared_ptr p_pre; +double time_lag_imu_to_lidar = 0.0; + +void readParameters(ros::NodeHandle &nh) +{ + p_pre.reset(new Preprocess()); + nh.param("prop_at_freq_of_imu", prop_at_freq_of_imu, 1); + nh.param("use_imu_as_input", use_imu_as_input, 1); + nh.param("check_satu", check_satu, 1); + nh.param("init_map_size", init_map_size, 100); + nh.param("space_down_sample", space_down_sample, 1); + nh.param("mapping/satu_acc", satu_acc, 3.0); + nh.param("mapping/satu_gyro", satu_gyro, 35.0); + nh.param("mapping/acc_norm", acc_norm, 1.0); + nh.param("mapping/plane_thr", plane_thr, 0.05f); + nh.param("point_filter_num", p_pre->point_filter_num, 2); + nh.param("common/lid_topic", lid_topic, "/livox/lidar"); + nh.param("common/imu_topic", imu_topic, "/livox/imu"); + nh.param("common/con_frame", con_frame, false); + nh.param("common/con_frame_num", con_frame_num, 1); + nh.param("common/cut_frame", cut_frame, false); + nh.param("common/cut_frame_time_interval", cut_frame_time_interval, 0.1); + nh.param("common/time_lag_imu_to_lidar", time_lag_imu_to_lidar, 0.0); + nh.param("filter_size_surf", filter_size_surf_min, 0.5); + nh.param("filter_size_map", filter_size_map_min, 0.5); + nh.param("cube_side_length", cube_len, 200); + nh.param("mapping/det_range", DET_RANGE, 300.f); + nh.param("mapping/fov_degree", fov_deg, 180); + nh.param("mapping/imu_en", imu_en, true); + nh.param("mapping/start_in_aggressive_motion", non_station_start, false); + nh.param("mapping/extrinsic_est_en", extrinsic_est_en, true); + nh.param("mapping/imu_time_inte", imu_time_inte, 0.005); + nh.param("mapping/lidar_meas_cov", laser_point_cov, 0.1); + nh.param("mapping/acc_cov_input", acc_cov_input, 0.1); + nh.param("mapping/vel_cov", vel_cov, 20); + nh.param("mapping/gyr_cov_input", gyr_cov_input, 0.1); + nh.param("mapping/gyr_cov_output", gyr_cov_output, 0.1); + nh.param("mapping/acc_cov_output", acc_cov_output, 0.1); + nh.param("mapping/b_gyr_cov", b_gyr_cov, 0.0001); + nh.param("mapping/b_acc_cov", b_acc_cov, 0.0001); + nh.param("mapping/imu_meas_acc_cov", imu_meas_acc_cov, 0.1); + nh.param("mapping/imu_meas_omg_cov", imu_meas_omg_cov, 0.1); + nh.param("preprocess/blind", p_pre->blind, 1.0); + nh.param("preprocess/lidar_type", lidar_type, 1); + nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); + nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); + nh.param("preprocess/timestamp_unit", p_pre->time_unit, 1); + nh.param("mapping/match_s", match_s, 81); + nh.param("mapping/gravity_align", gravity_align, true); + nh.param>("mapping/gravity", gravity, std::vector()); + nh.param>("mapping/gravity_init", gravity_init, std::vector()); + nh.param>("mapping/extrinsic_T", extrinT, std::vector()); + nh.param>("mapping/extrinsic_R", extrinR, std::vector()); + nh.param("odometry/publish_odometry_without_downsample", publish_odometry_without_downsample, false); + nh.param("publish/path_en", path_en, true); + nh.param("publish/scan_publish_en", scan_pub_en, 1); + nh.param("publish/scan_bodyframe_pub_en", scan_body_pub_en, 1); + nh.param("runtime_pos_log_enable", runtime_pos_log, 0); + nh.param("pcd_save/pcd_save_en", pcd_save_en, false); + nh.param("pcd_save/interval", pcd_save_interval, -1); +} diff --git a/src/parameters.h b/src/parameters.h new file mode 100755 index 0000000..88eee85 --- /dev/null +++ b/src/parameters.h @@ -0,0 +1,40 @@ +// #ifndef PARAM_H +// #define PARAM_H +#pragma once +#include +#include +#include +#include +#include "preprocess.h" + +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 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, gravity_align, non_station_start; +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 gravity_init, gravity; +extern std::vector extrinT; +extern std::vector extrinR; +extern bool runtime_pos_log, pcd_save_en, path_en; +extern bool scan_pub_en, scan_body_pub_en; +extern shared_ptr p_pre; +extern double time_lag_imu_to_lidar; + +void readParameters(ros::NodeHandle &n); diff --git a/src/preprocess.cpp b/src/preprocess.cpp new file mode 100755 index 0000000..ea1179c --- /dev/null +++ b/src/preprocess.cpp @@ -0,0 +1,856 @@ +#include "preprocess.h" + +#define RETURN0 0x00 +#define RETURN0AND1 0x10 + +Preprocess::Preprocess() + :lidar_type(AVIA), blind(0.01), point_filter_num(1) +{ + inf_bound = 10; + N_SCANS = 6; + SCAN_RATE = 10; + group_size = 8; + disA = 0.01; + disA = 0.1; // B? + p2l_ratio = 225; + limit_maxmid =6.25; + limit_midmin =6.25; + limit_maxmin = 3.24; + jump_up_limit = 170.0; + jump_down_limit = 8.0; + cos160 = 160.0; + edgea = 2; + edgeb = 0.1; + smallp_intersect = 172.5; + smallp_ratio = 1.2; + given_offset_time = false; + + jump_up_limit = cos(jump_up_limit/180*M_PI); + jump_down_limit = cos(jump_down_limit/180*M_PI); + cos160 = cos(cos160/180*M_PI); + smallp_intersect = cos(smallp_intersect/180*M_PI); +} + +Preprocess::~Preprocess() {} + +void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) +{ + lidar_type = lid_type; + blind = bld; + point_filter_num = pfilt_num; +} + +// void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +// { +// avia_handler(msg); +// *pcl_out = pl_surf; +// } + +void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + switch (time_unit) + { + case SEC: + time_unit_scale = 1.e3f; + break; + case MS: + time_unit_scale = 1.f; + break; + case US: + time_unit_scale = 1.e-3f; + break; + case NS: + time_unit_scale = 1.e-6f; + break; + default: + time_unit_scale = 1.f; + break; + } + + switch (lidar_type) + { + case OUST64: + oust64_handler(msg); + break; + + case VELO16: + velodyne_handler(msg); + break; + + case HESAIxt32: + hesai_handler(msg); + break; + + case UNILIDAR: + unilidar_handler(msg); + break; + + default: + printf("Error LiDAR Type"); + break; + } + + *pcl_out = pl_surf; +} + +// void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) +// { +// pl_surf.clear(); +// pl_corn.clear(); +// pl_full.clear(); +// double t1 = omp_get_wtime(); +// int plsize = msg->point_num; + +// pl_corn.reserve(plsize); +// pl_surf.reserve(plsize); +// pl_full.resize(plsize); + +// uint valid_num = 0; + +// for(uint i=1; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) +// { +// valid_num ++; +// if (valid_num % point_filter_num == 0) +// { +// pl_full[i].x = msg->points[i].x; +// pl_full[i].y = msg->points[i].y; +// pl_full[i].z = msg->points[i].z; +// pl_full[i].intensity = msg->points[i].reflectivity; +// pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms + +// if (i==0) pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; +// else pl_full[i].curvature = fabs(pl_full[i].curvature - pl_full[i-1].curvature) < 1.0 ? pl_full[i].curvature : pl_full[i-1].curvature + 0.004166667f; + +// if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) +// || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) +// || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7) +// && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) +// { +// pl_surf.push_back(pl_full[i]); +// } +// } +// } +// } + +// } + +void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + + + double time_stamp = msg->header.stamp.toSec(); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < (blind * blind)) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms + + pl_surf.points.push_back(added_pt); + } + + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].time > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + +} + + +void Preprocess::unilidar_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + + pl_surf.reserve(plsize); + + // std::cout << "plsize = " << plsize << ", given_offset_time = " << given_offset_time << std::endl; + int countElimnated = 0; + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + + added_pt.intensity = pl_orig.points[i].intensity; + + added_pt.curvature = pl_orig.points[i].time * time_unit_scale; + + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + else + { + countElimnated++; + } + } + + // std::cout << "pl_surf.size() = " << pl_surf.size() << ", countElimnated = " << countElimnated << std::endl; + +} + + +void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].timestamp > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + double time_head = pl_orig.points[0].timestamp; + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + +} + +void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) +{ + int plsize = pl.size(); + int plsize2; + if(plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + + while(types[head].range < blind) + { + head++; + } + + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for(uint i=head; i0.5) + if(last_state==1 && last_direct.norm()>0.1) + { + double mod = last_direct.transpose() * curr_direct; + if(mod>-0.707 && mod<0.707) + { + types[i].ftype = Edge_Plane; + } + else + { + types[i].ftype = Real_Plane; + } + } + + i = i_nex - 1; + last_state = 1; + } + else // if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for(uint i=head+3; i=Real_Plane) + { + continue; + } + + if(types[i-1].dista<1e-16 || types[i].dista<1e-16) + { + continue; + } + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for(int j=0; j<2; j++) + { + int m = -1; + if(j == 1) + { + m = 1; + } + + if(types[i+m].range < blind) + { + if(types[i].range > inf_bound) + { + types[i].edj[j] = Nr_inf; + } + else + { + types[i].edj[j] = Nr_blind; + } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if(types[i].angle[j] < jump_up_limit) + { + types[i].edj[j] = Nr_180; + } + else if(types[i].angle[j] > jump_down_limit) + { + types[i].edj[j] = Nr_zero; + } + } + + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + + } + else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) + { + if(types[i].ftype == Nor) + { + types[i].ftype = Wire; + } + } + } + + plsize2 = plsize-1; + double ratio; + for(uint i=head+1; i types[i].dista) + { + ratio = types[i-1].dista / types[i].dista; + } + else + { + ratio = types[i].dista / types[i-1].dista; + } + + if(types[i].intersect &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = disA*types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for(i_nex=i_cur; i_nex= pl.size()) || (i_nex >= pl.size())) break; + + if(types[i_nex].range < blind) + { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx*vx + vy*vy + vz*vz; + if(two_dis >= group_dis) + { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for(uint j=i_cur+1; j= pl.size()) || (i_cur >= pl.size())) break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1]*vz - vy*v1[2]; + v2[1] = v1[2]*vx - v1[0]*vz; + v2[2] = v1[0]*vy - vx*v1[1]; + + double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]; + if(lw > leng_wid) + { + leng_wid = lw; + } + } + + + if((two_dis*two_dis/leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for(uint j=0; j=limit_maxmid || dismid_min>=limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize-2]; + if(dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) +{ + if(nor_dir == 0) + { + if(types[i-1].rangeedgea*d2 || (d1-d2)>edgeb) + { + return false; + } + + return true; +} diff --git a/src/preprocess.h b/src/preprocess.h new file mode 100755 index 0000000..16dccec --- /dev/null +++ b/src/preprocess.h @@ -0,0 +1,181 @@ +#pragma once + +#include +#include +#include +// #include + +using namespace std; + +#define IS_VALID(a) ((abs(a)>1e8) ? true : false) + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud PointCloudXYZI; + +enum LID_TYPE{ + AVIA = 1, + VELO16, + OUST64, + HESAIxt32, + UNILIDAR +}; //{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}; + +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) +) + +/** + * @brief Unilidar Point Type + */ +namespace unilidar_ros { +struct Point +{ + PCL_ADD_POINT4D + PCL_ADD_INTENSITY + std::uint16_t ring; + float time; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; +} +POINT_CLOUD_REGISTER_POINT_STRUCT(unilidar_ros::Point, + (float, x, x)(float, y, y)(float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) + (float, time, time) +) + + +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(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 typess[128]; //maximum 128 line lidar + float time_unit_scale; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; + int 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 unilidar_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void give_feature(PointCloudXYZI &pl, vector &types); + void pub_func(PointCloudXYZI &pl, const ros::Time &ct); + int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); + bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); + bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); + + int group_size; + double disA, disB, inf_bound; + double limit_maxmid, limit_midmin, limit_maxmin; + double p2l_ratio; + double jump_up_limit, jump_down_limit; + double cos160; + double edgea, edgeb; + double smallp_intersect, smallp_ratio; + double vx, vy, vz; +};