diff --git a/.gitignore b/.gitignore index b0f3567..09ff613 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,7 @@ build Log/*.png Log/*.txt -Log/time.pdf +Log/*.csv +Log/*.pdf +.vscode/c_cpp_properties.json +.vscode/settings.json diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..b8a0efa --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "include/ikd-Tree"] + path = include/ikd-Tree + url = https://github.com/hku-mars/ikd-Tree.git + branch = fast_lio diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e1b95d..2bd67d3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,10 +15,26 @@ 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") -option(DEPLOY "Deploy on hardware" OFF) -if(DEPLOY) - add_definitions(-DDEPLOY) -endif(DEPLOY) +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}") @@ -43,8 +59,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(Eigen3 REQUIRED) find_package(PCL 1.8 REQUIRED) -find_package(OpenCV REQUIRED) - message(Eigen: ${EIGEN3_INCLUDE_DIR}) @@ -58,7 +72,6 @@ include_directories( add_message_files( FILES Pose6D.msg - States.msg ) generate_messages( @@ -68,19 +81,10 @@ generate_messages( catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime - DEPENDS EIGEN3 PCL OpenCV + DEPENDS EIGEN3 PCL INCLUDE_DIRS ) -# add_executable(imu_process src/IMU_Processing.cpp) -# target_link_libraries(imu_process ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(loam_feat_extract src/feature_extract.cpp) -target_link_libraries(loam_feat_extract ${catkin_LIBRARIES} ${PCL_LIBRARIES}) - -add_executable(loam_laserMapping src/laserMapping.cpp) -add_dependencies(loam_laserMapping ${catkin_EXPORTED_TARGETS}) -target_link_libraries(loam_laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${PYTHON_LIBRARIES}) -target_include_directories(loam_laserMapping PRIVATE ${PYTHON_INCLUDE_DIRS}) -# target_compile_definitions(loam_laserMapping PRIVATE ROOT_DIR="${CMAKE_CURRENT_SOURCE_DIR}") - +add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) +target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) +target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) \ No newline at end of file diff --git a/Log/fast_lio_time_log_analysis.m b/Log/fast_lio_time_log_analysis.m new file mode 100644 index 0000000..8a0b679 --- /dev/null +++ b/Log/fast_lio_time_log_analysis.m @@ -0,0 +1,135 @@ +clear +close all + +Color_red = [0.6350 0.0780 0.1840]; +Color_blue = [0 0.4470 0.7410]; +Color_orange = [0.8500 0.3250 0.0980]; +Color_green = [0.4660 0.6740 0.1880]; +Color_lightblue = [0.3010 0.7450 0.9330]; +Color_purple = [0.4940 0.1840 0.5560]; +Color_yellow = [0.9290 0.6940 0.1250]; + +fast_lio_ikdtree = csvread("./fast_lio_time_log.csv",1,0); +timestamp_ikd = fast_lio_ikdtree(:,1); +timestamp_ikd = timestamp_ikd - min(timestamp_ikd); +total_time_ikd = fast_lio_ikdtree(:,2)*1e3; +scan_num = fast_lio_ikdtree(:,3); +incremental_time_ikd = fast_lio_ikdtree(:,4)*1e3; +search_time_ikd = fast_lio_ikdtree(:,5)*1e3; +delete_size_ikd = fast_lio_ikdtree(:,6); +delete_time_ikd = fast_lio_ikdtree(:,7) * 1e3; +tree_size_ikd_st = fast_lio_ikdtree(:,8); +tree_size_ikd = fast_lio_ikdtree(:,9); +add_points = fast_lio_ikdtree(:,10); + +fast_lio_forest = csvread("fast_lio_time_log.csv",1,0); +fov_check_time_forest = fast_lio_forest(:,5)*1e3; +average_time_forest = fast_lio_forest(:,2)*1e3; +total_time_forest = fast_lio_forest(:,6)*1e3; +incremental_time_forest = fast_lio_forest(:,3)*1e3; +search_time_forest = fast_lio_forest(:,4)*1e3; +timestamp_forest = fast_lio_forest(:,1); + +% Use slide window to calculate average +L = 1; % Length of slide window +for i = 1:length(timestamp_ikd) + if (i 0); +search_time_ikd = search_time_ikd(index_ikd); +index_forest = find(search_time_forest > 0); +search_time_forest = search_time_forest(index_forest); + +t = nexttile; +hold on; +boxplot_data_ikd = [incremental_time_ikd,total_time_ikd]; +boxplot_data_forest = [incremental_time_forest,total_time_forest]; +Colors_ikd = [Color_blue;Color_blue;Color_blue]; +Colors_forest = [Color_orange;Color_orange;Color_orange]; +% xticks([3,8,13]) +h_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3); +h_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3); +h_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3); +h_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3); +ax2 = gca; +ax2.YAxis.Scale = 'log'; +xlim([0.5,6.0]) +ylim([0.0008,100]) +xticks([1.25 3.25 5.25]) +xticklabels({'Nearest Search',' Incremental Updates','Total Time'}); +yticks([1e-3,1e-2,1e-1,1e0,1e1,1e2]) +ax2.YAxis.FontSize = 12; +ax2.XAxis.FontSize = 14.5; +% ax.XAxis.FontWeight = 'bold'; +ylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman') +box_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')]; +for j=1:length(box_vars) + if (j<=3) + Color = Color_blue; + else + Color = Color_orange; + end + patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color); +end +Lg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman'); +grid on +set(gca,'YMinorGrid','off') +nexttile; +hold on; +grid on; +box on; +set(gca,'FontSize',12,'FontName','Times New Roman') +plot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2); +plot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2); +plot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2); +lg = legend("\alpha_{bal}", "\alpha_{del}",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman') +title("Re-balancing Criterion",'FontSize',16,'FontName','Times New Roman') +xlabel("time/s",'FontSize',16,'FontName','Times New Roman') +yl = ylabel("\alpha",'FontSize',15, 'Position',[285.7 0.4250 -1]) +xlim([32,390]); +ylim([0,0.85]); +ax3 = gca; +ax3.YAxis.FontSize = 12; +ax3.XAxis.FontSize = 12; +% print('./Figures/fastlio_exp_combine','-depsc','-r1200') +% exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector') + diff --git a/Log/guide.md b/Log/guide.md index a620293..8ff3fc1 100644 --- a/Log/guide.md +++ b/Log/guide.md @@ -1 +1 @@ -Here saved the debug records which can be drew by the ../tools/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h. +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/tools/plot.py b/Log/plot.py similarity index 62% rename from tools/plot.py rename to Log/plot.py index eb8ef7d..4416188 100644 --- a/tools/plot.py +++ b/Log/plot.py @@ -3,47 +3,55 @@ import numpy as np import matplotlib.pyplot as plt -fig, axs = plt.subplots(5) + +#######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) -a_pre=np.loadtxt('Log/mat_pre.txt') -a_out=np.loadtxt('Log/mat_out.txt') +a_pre=np.loadtxt('mat_pre.txt') +a_out=np.loadtxt('mat_out.txt') time=a_pre[:,0] -axs[0].set_title('Attitude') -axs[1].set_title('Translation') -axs[2].set_title('Velocity') -axs[3].set_title('bg') -axs[4].set_title('ba') +axs[0,0].set_title('Attitude') +axs[1,0].set_title('Translation') +axs[2,0].set_title('Extrins-R') +axs[3,0].set_title('Extrins-T') +axs[0,1].set_title('Velocity') +axs[1,1].set_title('bg') +axs[2,1].set_title('ba') +axs[3,1].set_title('Gravity') for i in range(1,4): - for j in range(5): - axs[j].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) - axs[j].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) + 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####### -for i in range(5): + +#### Draw IMU data +fig, axs = plt.subplots(2) +imu=np.loadtxt('imu.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() - - -# #### Draw IMU data -# fig, axs = plt.subplots(2) -# imu=np.loadtxt('Log/imu_0.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() # #### Draw time calculation +# plt.figure(3) # fig = plt.figure() # font1 = {'family' : 'Times New Roman', # 'weight' : 'normal', @@ -79,8 +87,8 @@ for i in range(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("Log/time.pdf", dpi=1200) +# # # print(time_se) +# # # print(a_out3[:,2]) +# plt.grid() +# plt.savefig("time.pdf", dpi=1200) plt.show() diff --git a/README.md b/README.md index c515065..fa004e8 100644 --- a/README.md +++ b/README.md @@ -1,38 +1,51 @@ **Noted: Ubuntu 16.04 and lower is not supported** -## FAST-LIO 2.0 -Will Launch **Soon**. - -New features: -1. Faster and Better; -2. Higher Frequency; -3. More LiDAR support (Horizon and Ouster 64); -4. Support ARM based embeded platforms. ## FAST-LIO **FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues: 1. Fast iterated Kalman filter for odometry optimization; 2. Automaticaly initialized at most steady environments; 3. Parallel KD-Tree Search to decrease the computation; -4. Robust feature extraction; -**Developers** +## FAST-LIO 2.0 (2021-07-04 Update) + + +
+ + +
-[Wei Xu 徐威](https://github.com/XW-HKU): Laser mapping and pose optimization; +**Related video:** -[Zheng Liu 刘政](https://github.com/Zale-Liu): Features extraction. +[FAST-LIO2](https://youtu.be/2OvjGnxszf8) + +[FAST-LIO1](https://youtu.be/iYCY6T79oNU) + +**New features:** +1. Incremental mapping using ikd-Tree, achieve faster speed and over 100Hz LiDAR rate. +2. Direct odometry on Raw LiDAR points (feature extraction can be closed), achieving better accuracy. +3. Since no need for feature extraction, FAST-LIO2 can support different LiDAR Types including spinning (Velodyne, Ouster) and solid-state (Avia, horizon) LiDARs. And most of LiDARs can be easily supported. +4. Support external IMU. +5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry 4B with 8G RAM. + +**Contributors** + +[Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu) To know more about the details, please refer to our related paper:) -**Our related paper**: our related papers are now available on arxiv: +**Related papers**: + +our related papers are now available on arxiv: + +[FAST-LIO2: Fast Direct LiDAR-inertial Odometry (Currently Uavailable)]() [FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196) -**Our related video**: https://youtu.be/iYCY6T79oNU -
+ ## 1. Prerequisites ### 1.1 **Ubuntu** and **ROS** @@ -45,18 +58,16 @@ PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/ Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). -OpenCV >= 3.2, Follow [openCV Installation](https://opencv.org/releases/). - ### 1.3. **livox_ros_driver** Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). - ## 2. Build Clone the repository and catkin_make: ``` cd ~/catkin_ws/src git clone https://github.com/XW-HKU/fast_lio.git + git submodule update --init cd .. catkin_make source devel/setup.bash @@ -65,10 +76,11 @@ Clone the repository and catkin_make: - If you want to use a custom build of PCL, add the following line to ~/.bashrc ```export PCL_ROOT={CUSTOM_PCL_PATH}``` ## 3. Directly run -### 3.1 For indoor environments (support maximum 50hz frame rate) +### 3.1 For Avia Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then ``` .... + cd ~/catkin_ws roslaunch fast_lio mapping_avia.launch roslaunch livox_ros_driver livox_lidar_msg.launch @@ -76,14 +88,6 @@ Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installat *Remarks:* - If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage. -### 3.2 For outdoor environments -Connect to your PC to Livox Avia LiDAR following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then -``` - .... - roslaunch fast_lio mapping_avia_outdoor.launch - roslaunch livox_ros_driver livox_lidar_msg.launch - -``` ## 4. Rosbag Example ### 4.1 Indoor rosbag (Livox Avia LiDAR) diff --git a/config/avia.yaml b/config/avia.yaml new file mode 100644 index 0000000..8176780 --- /dev/null +++ b/config/avia.yaml @@ -0,0 +1,20 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + +preprocess: + lidar_type: 1 # Livox Avia LiDAR + scan_line: 6 + blind: 1 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 90 + det_range: 450.0 + extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] \ No newline at end of file diff --git a/config/horizon.yaml b/config/horizon.yaml new file mode 100644 index 0000000..ec691e0 --- /dev/null +++ b/config/horizon.yaml @@ -0,0 +1,20 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + +preprocess: + lidar_type: 1 # Livox Avia LiDAR + scan_line: 6 + blind: 1 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 100 + det_range: 260.0 + extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] \ No newline at end of file diff --git a/config/ouster64.yaml b/config/ouster64.yaml new file mode 100644 index 0000000..98598b8 --- /dev/null +++ b/config/ouster64.yaml @@ -0,0 +1,20 @@ +common: + lid_topic: "/os_cloud_node/points" + imu_topic: "/os_cloud_node/imu" + +preprocess: + lidar_type: 3 # Ouster2-64 LiDAR + scan_line: 64 + blind: 1 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 150.0 + extrinsic_T: [ 0.0, 0.0, 0.0 ] + extrinsic_R: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] \ No newline at end of file diff --git a/config/velody16.yaml b/config/velody16.yaml new file mode 100644 index 0000000..5cee477 --- /dev/null +++ b/config/velody16.yaml @@ -0,0 +1,20 @@ +common: + lid_topic: "/velodyne_points" + imu_topic: "/imu/data" + +preprocess: + lidar_type: 2 # VLP-16 LiDAR + scan_line: 32 + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 100.0 + extrinsic_T: [ 0, 0, 0.28] #ULHK #[ -0.5, 1.4, 1.5 ] #utbm + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] \ No newline at end of file diff --git a/doc/real_exp_2.png b/doc/real_exp_2.png new file mode 100644 index 0000000..90bf038 Binary files /dev/null and b/doc/real_exp_2.png differ diff --git a/doc/real_experiment2.gif b/doc/real_experiment2.gif new file mode 100644 index 0000000..bec4701 Binary files /dev/null and b/doc/real_experiment2.gif differ diff --git a/doc/ulhkwh_fastlio.gif b/doc/ulhkwh_fastlio.gif new file mode 100644 index 0000000..6005ca2 Binary files /dev/null and b/doc/ulhkwh_fastlio.gif differ diff --git a/include/IKFoM_toolkit/esekfom/esekfom.hpp b/include/IKFoM_toolkit/esekfom/esekfom.hpp new file mode 100755 index 0000000..8a0c194 --- /dev/null +++ b/include/IKFoM_toolkit/esekfom/esekfom.hpp @@ -0,0 +1,2005 @@ +/* + * 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 "../mtk/types/vect.hpp" +#include "../mtk/types/SOn.hpp" +#include "../mtk/types/S2.hpp" +#include "../mtk/startIdx.hpp" +#include "../mtk/build_manifold.hpp" +#include "util.hpp" + +//#define USE_sparse + + +namespace esekfom { + +using namespace Eigen; + +//used for iterated error state EKF update +//for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. +//applied for measurement as a manifold. +template +struct share_datastruct +{ + bool valid; + bool converge; + M z; + Eigen::Matrix h_v; + Eigen::Matrix h_x; + Eigen::Matrix R; +}; + +//used for iterated error state EKF update +//for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. +//applied for measurement as an Eigen matrix whose dimension is changing +template +struct dyn_share_datastruct +{ + bool valid; + bool converge; + Eigen::Matrix z; + Eigen::Matrix h; + Eigen::Matrix h_v; + Eigen::Matrix h_x; + Eigen::Matrix R; +}; + +//used for iterated error state EKF update +//for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. +//applied for measurement as a dynamic manifold whose dimension or type is changing +template +struct dyn_runtime_share_datastruct +{ + bool valid; + bool converge; + //Z z; + Eigen::Matrix h_v; + Eigen::Matrix h_x; + Eigen::Matrix R; +}; + +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 measurement measurementModel(state &, bool &); + typedef measurement measurementModel_share(state &, share_datastruct &); + typedef Eigen::Matrix measurementModel_dyn(state &, bool &); + //typedef Eigen::Matrix measurementModel_dyn_share(state &, dyn_share_datastruct &); + typedef void measurementModel_dyn_share(state &, dyn_share_datastruct &); + typedef Eigen::Matrix measurementMatrix1(state &, bool&); + typedef Eigen::Matrix measurementMatrix1_dyn(state &, bool&); + typedef Eigen::Matrix measurementMatrix2(state &, bool&); + typedef Eigen::Matrix measurementMatrix2_dyn(state &, bool&); + typedef Eigen::Matrix measurementnoisecovariance; + typedef Eigen::Matrix measurementnoisecovariance_dyn; + + esekf(const state &x = state(), + const cov &P = cov::Identity()): x_(x), P_(P){ + #ifdef USE_sparse + SparseMatrix ref(n, n); + ref.setIdentity(); + l_ = ref; + f_x_2 = ref; + f_x_1 = ref; + #endif + }; + + //receive system-specific models and their differentions. + //for measurement as a manifold. + void init(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel h_in, measurementMatrix1 h_x_in, measurementMatrix2 h_v_in, int maximum_iteration, scalar_type limit_vector[n]) + { + f = f_in; + f_x = f_x_in; + f_w = f_w_in; + h = h_in; + h_x = h_x_in; + h_v = h_v_in; + + maximum_iter = maximum_iteration; + for(int i=0; i f_w_ = f_w(x_, i_in); + Matrix f_w_final; + state x_before = x_; + x_.oplus(f_, dt); + + 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)); + #ifdef USE_sparse + res_temp_SO3 = res.toRotationMatrix(); + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + f_x_1.coeffRef(idx + i, idx + j) = res_temp_SO3(i, j); + } + } + #else + F_x1.template block<3, 3>(idx, idx) = res.toRotationMatrix(); + #endif + 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)); + } + for(int i = 0; i < process_noise_dof; i++){ + f_w_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_. template block<3, 1>(dim, i)); + } + } + + + Matrix res_temp_S2; + Matrix res_temp_S2_; + MTK::vect<3, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_S2(i) = f_(dim + i) * dt; + } + MTK::vect<2, scalar_type> vec = MTK::vect<2, scalar_type>::Zero(); + MTK::SO3 res; + res.w() = MTK::exp(res.vec(), seg_S2, scalar_type(1/2)); + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_before.S2_Mx(Mx, vec, idx); + #ifdef USE_sparse + res_temp_S2_ = Nx * res.toRotationMatrix() * Mx; + for(int i = 0; i < 2; i++){ + for(int j = 0; j < 2; j++){ + f_x_1.coeffRef(idx + i, idx + j) = res_temp_S2_(i, j); + } + } + #else + F_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx; + #endif + + Eigen::Matrix x_before_hat; + x_before.S2_hat(x_before_hat, idx); + res_temp_S2 = -Nx * res.toRotationMatrix() * x_before_hat*MTK::A_matrix(seg_S2).transpose(); + + for(int i = 0; i < n; i++){ + f_x_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_x_. template block<3, 1>(dim, i)); + + } + for(int i = 0; i < process_noise_dof; i++){ + f_w_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_w_. template block<3, 1>(dim, i)); + } + } + + #ifdef USE_sparse + f_x_1.makeCompressed(); + spMt f_x2 = f_x_final.sparseView(); + spMt f_w1 = f_w_final.sparseView(); + spMt xp = f_x_1 + f_x2 * dt; + P_ = xp * P_ * xp.transpose() + (f_w1 * dt) * Q * (f_w1 * dt).transpose(); + #else + F_x1 += f_x_final * dt; + P_ = (F_x1) * P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose(); + #endif + } + + //iterated error state EKF update for measurement as a manifold. + void update_iterated(measurement& z, measurementnoisecovariance &R) { + + if(!(is_same())){ + std::cerr << "the scalar type of measurment must be the same as the state" << std::endl; + std::exit(100); + } + int t = 0; + bool converg = true; + bool valid = true; + state x_propagated = x_; + cov P_propagated = P_; + + for(int i=-1; i h_x_ = h_x(x_, valid); + Matrix h_v_ = h_v(x_, valid); + #endif + if(! valid) + { + continue; + } + + P_ = P_propagated; + + Matrix 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) = dx(idx+i); + } + + res_temp_SO3 = A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + 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 (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + 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; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + 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(); + } + } + + Matrix K_; + if(n > l) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + measurementnoisecovariance b = measurementnoisecovariance::Identity(); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + measurementnoisecovariance R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + measurementnoisecovariance R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + Matrix innovation; + z.boxminus(innovation, h(x_, valid)); + cov K_x = K_ * h_x_; + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + + converg = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + converg = false; + break; + } + } + + if(converg) t++; + + if(t > 1 || i == maximum_iter - 1) + { + 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)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. 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(); + } + } + + 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)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. 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(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > l) + { + P_ = L_ - K_ * h_x_ * P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as a manifold. + //calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. + void update_iterated_share() { + + if(!(is_same())){ + std::cerr << "the scalar type of measurment must be the same as the state" << std::endl; + std::exit(100); + } + + int t = 0; + share_datastruct _share; + _share.valid = true; + _share.converge = true; + state x_propagated = x_; + cov P_propagated = P_; + + for(int i=-1; i h_x_ = _share.h_x; + Matrix h_v_ = _share.h_v; + #endif + if(! _share.valid) + { + continue; + } + + P_ = P_propagated; + + Matrix 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) = dx(idx+i); + } + + res_temp_SO3 = A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + 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 (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + 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; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + 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(); + } + } + + Matrix K_; + if(n > l) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + measurementnoisecovariance b = measurementnoisecovariance::Identity(); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + measurementnoisecovariance R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + measurementnoisecovariance R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + Matrix innovation; + z.boxminus(innovation, h); + cov K_x = K_ * h_x_; + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + + _share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + _share.converge = false; + break; + } + } + + if(_share.converge) t++; + + if(t > 1 || i == maximum_iter - 1) + { + 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)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. 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(); + } + } + + 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)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. 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(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > l) + { + P_ = L_ - K_ * h_x_ * P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing. + void update_iterated_dyn(Eigen::Matrix z, measurementnoisecovariance_dyn R) { + + int t = 0; + bool valid = true; + bool converg = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise = R.rows(); + for(int i=-1; i h_x_ = h_x_dyn(x_, valid); + Matrix h_v_ = h_v_dyn(x_, valid); + #endif + Matrix h_ = h_dyn(x_, valid); + dof_Measurement = h_.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! valid) + { + continue; + } + + P_ = P_propagated; + Matrix 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) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + 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 (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + 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; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + 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(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + cov K_x = K_ * h_x_; + Matrix dx_ = K_ * (z - h_) + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + converg = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + converg = false; + break; + } + } + if(converg) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + 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 = MTK::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)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. 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(); + } + } + + 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)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. 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(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x_*P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + //iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing. + //calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. + void update_iterated_dyn_share() { + + int t = 0; + dyn_share_datastruct dyn_share; + dyn_share.valid = true; + dyn_share.converge = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise; + for(int i=-1; i h = h_dyn_share (x_, dyn_share); + Matrix z = dyn_share.z; + Matrix h = dyn_share.h; + #ifdef USE_sparse + spMt h_x = dyn_share.h_x.sparseView(); + spMt h_v = dyn_share.h_v.sparseView(); + spMt R_ = dyn_share.R.sparseView(); + #else + Matrix R = dyn_share.R; + Matrix h_x = dyn_share.h_x; + Matrix h_v = dyn_share.h_v; + #endif + dof_Measurement = h_x.rows(); + dof_Measurement_noise = dyn_share.R.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! (dyn_share.valid)) + { + continue; + } + + P_ = P_propagated; + Matrix 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) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + 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 (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + 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; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + 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(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x * P_ * h_x.transpose(); + spMt R_temp = h_v * R_ * h_v.transpose(); + K_temp += R_temp; + K_ = P_ * h_x.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x.transpose() * R_in * h_x; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v*R*h_v.transpose()).inverse(); + K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in; + #endif + } + + cov K_x = K_ * h_x; + Matrix dx_ = K_ * (z - h) + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + dyn_share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + dyn_share.converge = false; + break; + } + } + if(dyn_share.converge) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + 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 = MTK::A_matrix(seg_SO3).transpose(); + for(int i = 0; i < int(n); i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. 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(); + } + } + + 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)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. 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(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x*P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing. + //the measurement and the measurement model are received in a dynamic manner. + template + void update_iterated_dyn_runtime(measurement_runtime z, measurementnoisecovariance_dyn R, measurementModel_runtime h_runtime) { + + int t = 0; + bool valid = true; + bool converg = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise; + for(int i=-1; i h_x_ = h_x_dyn(x_, valid); + Matrix h_v_ = h_v_dyn(x_, valid); + #endif + measurement_runtime h_ = h_runtime(x_, valid); + dof_Measurement = measurement_runtime::DOF; + dof_Measurement_noise = R.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! valid) + { + continue; + } + + P_ = P_propagated; + Matrix 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) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + 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 (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + 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; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + 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(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + cov K_x = K_ * h_x_; + Eigen::Matrix innovation; + z.boxminus(innovation, h_); + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + converg = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + converg = false; + break; + } + } + if(converg) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + 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 = MTK::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)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. 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(); + } + } + + 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)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. 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(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x_*P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing. + //the measurement and the measurement model are received in a dynamic manner. + //calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. + template + void update_iterated_dyn_runtime_share(measurement_runtime z, measurementModel_dyn_runtime_share h) { + + int t = 0; + dyn_runtime_share_datastruct dyn_share; + dyn_share.valid = true; + dyn_share.converge = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise; + for(int i=-1; i R = dyn_share.R; + Matrix h_x = dyn_share.h_x; + Matrix h_v = dyn_share.h_v; + #endif + dof_Measurement = measurement_runtime::DOF; + dof_Measurement_noise = dyn_share.R.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! (dyn_share.valid)) + { + continue; + } + + P_ = P_propagated; + Matrix 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) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + 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 (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + 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; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + 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(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x * P_ * h_x.transpose(); + spMt R_temp = h_v * R_ * h_v.transpose(); + K_temp += R_temp; + K_ = P_ * h_x.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in =R_in_temp.sparseView(); + spMt K_temp = h_x.transpose() * R_in * h_x; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v*R*h_v.transpose()).inverse(); + K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in; + #endif + } + cov K_x = K_ * h_x; + Eigen::Matrix innovation; + z.boxminus(innovation, h_); + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + dyn_share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + dyn_share.converge = false; + break; + } + } + if(dyn_share.converge) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + 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 = MTK::A_matrix(seg_SO3).transpose(); + for(int i = 0; i < int(n); i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. 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(); + } + } + + 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)); + } + if(n > dof_Measurement) + { + 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)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. 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(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x * P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update modified for one specific system. + void update_iterated_dyn_share_modified(double R, double &solve_time) { + + dyn_share_datastruct dyn_share; + dyn_share.valid = true; + dyn_share.converge = true; + int t = 0; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + + Matrix K_h; + Matrix K_x; + + vectorized_state dx_new = vectorized_state::Zero(); + for(int i=-1; i h = h_dyn_share(x_, dyn_share); + #ifdef USE_sparse + spMt h_x_ = dyn_share.h_x.sparseView(); + #else + Eigen::Matrix h_x_ = dyn_share.h_x; + #endif + double solve_start = omp_get_wtime(); + dof_Measurement = h_x_.rows(); + vectorized_state dx; + x_.boxminus(dx, x_propagated); + dx_new = dx; + + if(! dyn_share.valid) + { + continue; + } + + P_ = P_propagated; + + Matrix 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) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + 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 (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + 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; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + 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(); + } + } + //Matrix K_; + //Matrix K_h; + //Matrix K_x; + + /* + if(n > dof_Measurement) + { + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse()/R; + } + else + { + K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); + } + */ + + if(n > dof_Measurement) + { + //#ifdef USE_sparse + //Matrix K_temp = h_x * P_ * h_x.transpose(); + //spMt R_temp = h_v * R_ * h_v.transpose(); + //K_temp += R_temp; + Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; + /* + h_x_cur.col(0) = h_x_.col(0); + h_x_cur.col(1) = h_x_.col(1); + h_x_cur.col(2) = h_x_.col(2); + h_x_cur.col(3) = h_x_.col(3); + h_x_cur.col(4) = h_x_.col(4); + h_x_cur.col(5) = h_x_.col(5); + h_x_cur.col(6) = h_x_.col(6); + h_x_cur.col(7) = h_x_.col(7); + h_x_cur.col(8) = h_x_.col(8); + h_x_cur.col(9) = h_x_.col(9); + h_x_cur.col(10) = h_x_.col(10); + h_x_cur.col(11) = h_x_.col(11); + */ + + Matrix K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose()/R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse()/R; + K_h = K_ * dyn_share.h; + K_x = K_ * h_x_cur; + //#else + // K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); + //#endif + } + else + { + #ifdef USE_sparse + //Eigen::Matrix b = Eigen::Matrix::Identity(); + //Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + spMt A = h_x_.transpose() * h_x_; + cov P_temp = (P_/R).inverse(); + P_temp. template block<12, 12>(0, 0) += A; + P_temp = P_temp.inverse(); + /* + Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + h_x_cur.col(0) = h_x_.col(0); + h_x_cur.col(1) = h_x_.col(1); + h_x_cur.col(2) = h_x_.col(2); + h_x_cur.col(3) = h_x_.col(3); + h_x_cur.col(4) = h_x_.col(4); + h_x_cur.col(5) = h_x_.col(5); + h_x_cur.col(6) = h_x_.col(6); + h_x_cur.col(7) = h_x_.col(7); + h_x_cur.col(8) = h_x_.col(8); + h_x_cur.col(9) = h_x_.col(9); + h_x_cur.col(10) = h_x_.col(10); + h_x_cur.col(11) = h_x_.col(11); + */ + K_ = P_temp. template block(0, 0) * h_x_.transpose(); + K_x = cov::Zero(); + K_x. template block(0, 0) = P_inv. template block(0, 0) * HTH; + /* + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in =R_in_temp.sparseView(); + spMt K_temp = h_x.transpose() * R_in * h_x; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x.transpose() * R_in; + */ + #else + cov P_temp = (P_/R).inverse(); + //Eigen::Matrix h_T = h_x_.transpose(); + Eigen::Matrix HTH = h_x_.transpose() * h_x_; + P_temp. template block<12, 12>(0, 0) += HTH; + /* + Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + //std::cout << "line 1767" << std::endl; + h_x_cur.col(0) = h_x_.col(0); + h_x_cur.col(1) = h_x_.col(1); + h_x_cur.col(2) = h_x_.col(2); + h_x_cur.col(3) = h_x_.col(3); + h_x_cur.col(4) = h_x_.col(4); + h_x_cur.col(5) = h_x_.col(5); + h_x_cur.col(6) = h_x_.col(6); + h_x_cur.col(7) = h_x_.col(7); + h_x_cur.col(8) = h_x_.col(8); + h_x_cur.col(9) = h_x_.col(9); + h_x_cur.col(10) = h_x_.col(10); + h_x_cur.col(11) = h_x_.col(11); + */ + cov P_inv = P_temp.inverse(); + //std::cout << "line 1781" << std::endl; + K_h = P_inv. template block(0, 0) * h_x_.transpose() * dyn_share.h; + //std::cout << "line 1780" << std::endl; + //cov HTH_cur = cov::Zero(); + //HTH_cur. template block<12, 12>(0, 0) = HTH; + K_x.setZero(); // = cov::Zero(); + K_x. template block(0, 0) = P_inv. template block(0, 0) * HTH; + //K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); + #endif + } + + //K_x = K_ * h_x_; + Matrix dx_ = K_h + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + dyn_share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + dyn_share.converge = false; + break; + } + } + if(dyn_share.converge) t++; + + if(!t && i == maximum_iter - 2) + { + dyn_share.converge = true; + } + + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + //std::cout << "iteration time" << t << "," << i << std::endl; + 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 = MTK::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)); + } + // if(n > dof_Measurement) + // { + // 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)); + // } + // } + // else + // { + for(int i = 0; i < 12; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. 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(); + } + } + + 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)); + } + // if(n > dof_Measurement) + // { + // 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)); + // } + // } + // else + // { + for(int i = 0; i < 12; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. 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(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + // if(n > dof_Measurement) + // { + // Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + // h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; + // /* + // h_x_cur.col(0) = h_x_.col(0); + // h_x_cur.col(1) = h_x_.col(1); + // h_x_cur.col(2) = h_x_.col(2); + // h_x_cur.col(3) = h_x_.col(3); + // h_x_cur.col(4) = h_x_.col(4); + // h_x_cur.col(5) = h_x_.col(5); + // h_x_cur.col(6) = h_x_.col(6); + // h_x_cur.col(7) = h_x_.col(7); + // h_x_cur.col(8) = h_x_.col(8); + // h_x_cur.col(9) = h_x_.col(9); + // h_x_cur.col(10) = h_x_.col(10); + // h_x_cur.col(11) = h_x_.col(11); + // */ + // P_ = L_ - K_*h_x_cur * P_; + // } + // else + //{ + P_ = L_ - K_x.template block(0, 0) * P_.template block<12, n>(0, 0); + //} + solve_time += omp_get_wtime() - solve_start; + return; + } + solve_time += omp_get_wtime() - solve_start; + } + } + + void change_x(state &input_state) + { + x_ = input_state; + if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size())) + { + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + } + } + + void change_P(cov &input_cov) + { + P_ = input_cov; + } + + const state& get_x() const { + return x_; + } + const cov& get_P() const { + return P_; + } +private: + state x_; + 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; + + measurementModel *h; + measurementMatrix1 *h_x; + measurementMatrix2 *h_v; + + measurementModel_dyn *h_dyn; + measurementMatrix1_dyn *h_x_dyn; + measurementMatrix2_dyn *h_v_dyn; + + measurementModel_share *h_share; + measurementModel_dyn_share *h_dyn_share; + + int maximum_iter = 0; + scalar_type limit[n]; + + template + T check_safe_update( T _temp_vec ) + { + T temp_vec = _temp_vec; + if ( std::isnan( temp_vec(0, 0) ) ) + { + temp_vec.setZero(); + return temp_vec; + } + double angular_dis = temp_vec.block( 0, 0, 3, 1 ).norm() * 57.3; + double pos_dis = temp_vec.block( 3, 0, 3, 1 ).norm(); + if ( angular_dis >= 20 || pos_dis > 1 ) + { + printf( "Angular dis = %.2f, pos dis = %.2f\r\n", angular_dis, pos_dis ); + temp_vec.setZero(); + } + return temp_vec; + } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace esekfom + +#endif // ESEKFOM_EKF_HPP diff --git a/include/IKFoM_toolkit/esekfom/util.hpp b/include/IKFoM_toolkit/esekfom/util.hpp new file mode 100755 index 0000000..ab39fc4 --- /dev/null +++ b/include/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_toolkit/mtk/build_manifold.hpp b/include/IKFoM_toolkit/mtk/build_manifold.hpp new file mode 100755 index 0000000..2cdb106 --- /dev/null +++ b/include/IKFoM_toolkit/mtk/build_manifold.hpp @@ -0,0 +1,229 @@ +// 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_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_SUBVARLIST(seq, S2state, SO3state) \ +BOOST_PP_FOR_1( \ + ( \ + BOOST_PP_SEQ_SIZE(seq), \ + BOOST_PP_SEQ_HEAD(seq), \ + BOOST_PP_SEQ_TAIL(seq) (~), \ + 0,\ + 0,\ + S2state,\ + SO3state ),\ + MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) + +#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ + MTK::SubManifold id; +#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \ + MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ + 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) \ + 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)) + +#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) a + +#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state +#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \ + 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) + +#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;\ + MTK_SUBVARLIST(entries, S2_state, SO3_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 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_toolkit/mtk/src/SubManifold.hpp b/include/IKFoM_toolkit/mtk/src/SubManifold.hpp new file mode 100755 index 0000000..a1b13de --- /dev/null +++ b/include/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_toolkit/mtk/src/mtkmath.hpp b/include/IKFoM_toolkit/mtk/src/mtkmath.hpp new file mode 100755 index 0000000..e3420d1 --- /dev/null +++ b/include/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"); + + // 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_toolkit/mtk/src/vectview.hpp b/include/IKFoM_toolkit/mtk/src/vectview.hpp new file mode 100755 index 0000000..5025071 --- /dev/null +++ b/include/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_toolkit/mtk/startIdx.hpp b/include/IKFoM_toolkit/mtk/startIdx.hpp new file mode 100755 index 0000000..4dc2958 --- /dev/null +++ b/include/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_toolkit/mtk/types/S2.hpp b/include/IKFoM_toolkit/mtk/types/S2.hpp new file mode 100755 index 0000000..789ebd9 --- /dev/null +++ b/include/IKFoM_toolkit/mtk/types/S2.hpp @@ -0,0 +1,316 @@ +// 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.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.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 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.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 * 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_toolkit/mtk/types/SOn.hpp b/include/IKFoM_toolkit/mtk/types/SOn.hpp new file mode 100755 index 0000000..31005e6 --- /dev/null +++ b/include/IKFoM_toolkit/mtk/types/SOn.hpp @@ -0,0 +1,317 @@ +// 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(); + } + +}; + + +/** + * 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 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_toolkit/mtk/types/vect.hpp b/include/IKFoM_toolkit/mtk/types/vect.hpp new file mode 100755 index 0000000..0e5b9ce --- /dev/null +++ b/include/IKFoM_toolkit/mtk/types/vect.hpp @@ -0,0 +1,461 @@ +// 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 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 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; + } +}; + +/** + * 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 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 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_toolkit/mtk/types/wrapped_cv_mat.hpp b/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp new file mode 100755 index 0000000..b6643f1 --- /dev/null +++ b/include/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/common_lib.h b/include/common_lib.h index d5e92ac..9f05b19 100644 --- a/include/common_lib.h +++ b/include/common_lib.h @@ -5,66 +5,76 @@ #include #include #include -#include #include #include #include #include #include +using namespace std; +using namespace Eigen; -// #define DEBUG_PRINT -// #define USE_ikdtree +#define USE_IKFOM #define PI_M (3.14159265358) -#define G_m_s2 (9.8099) // Gravaty const in GuangDong/China -#define DIM_OF_STATES (18) // Dimension of states (Let Dim(SO(3)) = 3) -#define DIM_OF_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) +#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 INIT_COV (1) +#define NUM_MATCH_POINTS (5) +#define MAX_MEAS_DIM (10000) #define VEC_FROM_ARRAY(v) v[0],v[1],v[2] #define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] #define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) - - -#define DEBUG_FILE_DIR(name) (std::string(std::string(ROOT_DIR) + "Log/"+ name)) +#define STD_VEC_FROM_EIGEN(mat) vector (mat.data(), mat.data() + mat.rows() * mat.cols()) +#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) typedef fast_lio::Pose6D Pose6D; typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud PointCloudXYZI; +typedef vector> PointVector; +typedef Vector3d V3D; +typedef Matrix3d M3D; +typedef Vector3f V3F; +typedef Matrix3f M3F; -Eigen::Matrix3d Eye3d(Eigen::Matrix3d::Identity()); -Eigen::Matrix3f Eye3f(Eigen::Matrix3f::Identity()); -Eigen::Vector3d Zero3d(0, 0, 0); -Eigen::Vector3f Zero3f(0, 0, 0); -// Eigen::Vector3d Lidar_offset_to_IMU(0.05512, 0.02226, 0.0297); // Horizon -Eigen::Vector3d Lidar_offset_to_IMU(0.04165, 0.02326, -0.0284); // Avia +#define MD(a,b) Matrix +#define VD(a) Matrix +#define MF(a,b) Matrix +#define VF(a) Matrix + +M3D Eye3d(M3D::Identity()); +M3F Eye3f(M3F::Identity()); +V3D Zero3d(0, 0, 0); +V3F Zero3f(0, 0, 0); struct MeasureGroup // Lidar data and imu dates for the curent process { MeasureGroup() { + lidar_beg_time = 0.0; this->lidar.reset(new PointCloudXYZI()); }; double lidar_beg_time; PointCloudXYZI::Ptr lidar; - std::deque imu; + deque imu; }; struct StatesGroup { StatesGroup() { - this->rot_end = Eigen::Matrix3d::Identity(); + this->rot_end = M3D::Identity(); this->pos_end = Zero3d; this->vel_end = Zero3d; this->bias_g = Zero3d; this->bias_a = Zero3d; this->gravity = Zero3d; - this->cov = Eigen::Matrix::Identity() * INIT_COV; + this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV; + this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001; }; StatesGroup(const StatesGroup& b) { @@ -89,8 +99,7 @@ struct StatesGroup return *this; }; - - StatesGroup operator+(const Eigen::Matrix &state_add) + StatesGroup operator+(const Matrix &state_add) { StatesGroup a; a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); @@ -103,7 +112,7 @@ struct StatesGroup return a; }; - StatesGroup& operator+=(const Eigen::Matrix &state_add) + StatesGroup& operator+=(const Matrix &state_add) { this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); this->pos_end += state_add.block<3,1>(3,0); @@ -114,10 +123,10 @@ struct StatesGroup return *this; }; - Eigen::Matrix operator-(const StatesGroup& b) + Matrix operator-(const StatesGroup& b) { - Eigen::Matrix a; - Eigen::Matrix3d rotd(b.rot_end.transpose() * this->rot_end); + Matrix a; + M3D rotd(b.rot_end.transpose() * this->rot_end); a.block<3,1>(0,0) = Log(rotd); a.block<3,1>(3,0) = this->pos_end - b.pos_end; a.block<3,1>(6,0) = this->vel_end - b.vel_end; @@ -127,13 +136,20 @@ struct StatesGroup return a; }; - Eigen::Matrix3d rot_end; // the estimated attitude (rotation matrix) at the end lidar point - Eigen::Vector3d pos_end; // the estimated position at the end lidar point (world frame) - Eigen::Vector3d vel_end; // the estimated velocity at the end lidar point (world frame) - Eigen::Vector3d bias_g; // gyroscope bias - Eigen::Vector3d bias_a; // accelerator bias - Eigen::Vector3d gravity; // the estimated gravity acceleration - Eigen::Matrix cov; // states covariance + void resetpose() + { + this->rot_end = M3D::Identity(); + this->pos_end = Zero3d; + this->vel_end = Zero3d; + } + + M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point + V3D pos_end; // the estimated position at the end lidar point (world frame) + V3D vel_end; // the estimated velocity at the end lidar point (world frame) + V3D bias_g; // gyroscope bias + V3D bias_a; // accelerator bias + V3D gravity; // the estimated gravity acceleration + Matrix cov; // states covariance }; template @@ -149,8 +165,8 @@ T deg2rad(T degrees) } template -auto set_pose6d(const double t, const Eigen::Matrix &a, const Eigen::Matrix &g, \ - const Eigen::Matrix &v, const Eigen::Matrix &p, const Eigen::Matrix &R) +auto set_pose6d(const double t, const Matrix &a, const Matrix &g, \ + const Matrix &v, const Matrix &p, const Matrix &R) { Pose6D rot_kp; rot_kp.offset_time = t; @@ -162,10 +178,81 @@ auto set_pose6d(const double t, const Eigen::Matrix &a, const Eigen::Ma rot_kp.pos[i] = p(i); for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); } - // Eigen::Map(rot_kp.rot, 3,3) = R; - return std::move(rot_kp); + return move(rot_kp); } +/* 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; + } + } -#endif + normvec.normalize(); + return true; +} + +float calc_dist(PointType p1, PointType p2){ + float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); + return d; +} + +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 b/include/ikd-Tree new file mode 160000 index 0000000..3d115a4 --- /dev/null +++ b/include/ikd-Tree @@ -0,0 +1 @@ +Subproject commit 3d115a41377243420a74fc15dd7cf7ef337730df diff --git a/include/so3_math.h b/include/so3_math.h index 40d1aed..8530c3c 100644 --- a/include/so3_math.h +++ b/include/so3_math.h @@ -3,11 +3,17 @@ #include #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) { diff --git a/include/use-ikfom.hpp b/include/use-ikfom.hpp new file mode 100644 index 0000000..fdd8f87 --- /dev/null +++ b/include/use-ikfom.hpp @@ -0,0 +1,126 @@ +#ifndef USE_IKFOM_H +#define USE_IKFOM_H + +#include + +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_ikfom, +((vect3, pos)) +((SO3, rot)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +((vect3, vel)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +); + +MTK_BUILD_MANIFOLD(input_ikfom, +((vect3, acc)) +((vect3, gyro)) +); + +MTK_BUILD_MANIFOLD(process_noise_ikfom, +((vect3, ng)) +((vect3, na)) +((vect3, nbg)) +((vect3, nba)) +); + +MTK::get_cov::type process_noise_cov() +{ + MTK::get_cov::type cov = MTK::get_cov::type::Zero(); + MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);// 0.03 + MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05 + MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 + MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +//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(state_ikfom &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + vect3 a_inertial = s.rot * (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.grav[i]; + } + return res; +} + +Eigen::Matrix df_dx(state_ikfom &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.toRotationMatrix()*MTK::hat(acc_); + cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); + Eigen::Matrix vec = Eigen::Matrix::Zero(); + Eigen::Matrix grav_matrix; + s.S2_Mx(grav_matrix, vec, 21); + cov.template block<3, 2>(12, 21) = grav_matrix; + cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); + return cov; +} + + +Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.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; +} + +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); + // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw + return euler_ang; +} + +#endif \ No newline at end of file diff --git a/launch/gdb_debug_example.launch b/launch/gdb_debug_example.launch new file mode 100644 index 0000000..5070aa1 --- /dev/null +++ b/launch/gdb_debug_example.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mapping_avia.launch b/launch/mapping_avia.launch index 99e9768..2f56ef7 100644 --- a/launch/mapping_avia.launch +++ b/launch/mapping_avia.launch @@ -1,41 +1,22 @@ + - + - - - - - - - - - - - - - - - - - - + + + - + + + + + + + - - - - - - - - - - - - - - + + + diff --git a/launch/mapping_avia_outdoor.launch b/launch/mapping_avia_outdoor.launch deleted file mode 100644 index dd2e3fe..0000000 --- a/launch/mapping_avia_outdoor.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/mapping_horizon.launch b/launch/mapping_horizon.launch new file mode 100644 index 0000000..0d0cd31 --- /dev/null +++ b/launch/mapping_horizon.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/mapping_ouster64.launch b/launch/mapping_ouster64.launch new file mode 100644 index 0000000..3969273 --- /dev/null +++ b/launch/mapping_ouster64.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mapping_velody16.launch b/launch/mapping_velody16.launch new file mode 100644 index 0000000..990a425 --- /dev/null +++ b/launch/mapping_velody16.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/replay.launch b/launch/replay.launch deleted file mode 100644 index 53eb801..0000000 --- a/launch/replay.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/msg/States.msg b/msg/States.msg deleted file mode 100644 index f1b5712..0000000 --- a/msg/States.msg +++ /dev/null @@ -1,9 +0,0 @@ -Header header # timestamp of the first lidar in a frame -float64[] rot_end # the estimated attitude (rotation matrix) at the end lidar point -float64[] pos_end # the estimated position at the end lidar point (world frame) -float64[] vel_end # the estimated velocity at the end lidar point (world frame) -float64[] bias_gyr # gyroscope bias -float64[] bias_acc # accelerator bias -float64[] gravity # the estimated gravity acceleration -float64[] cov # states covariance -# Pose6D[] IMUpose # 6D pose at each imu measurements \ No newline at end of file diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index b2ef4ba..553f107 100644 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -5,30 +5,19 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /Axes1 - - /odometry1/odomPath1 - /mapping1 - /mapping1/surround1 - /mapping1/currPoints1 - - /mapping1/PointCloud21 - - /mapping1/PointCloud22 - - /Debug1/PointCloud21 - - /Debug1/PointCloud22 - - /Debug1/PointCloud23 + - /mapping1/currPoints1/Autocompute Value Bounds1 + - /Odometry1 - /Odometry1/Odometry1 - /Odometry1/Odometry1/Shape1 - - /Odometry1/Odometry2 - - /Odometry1/Odometry2/Shape1 - - /Odometry1/Odometry3 - - /Odometry1/Odometry3/Shape1 - - /Features1/PointCloud21 - - /Features1/PointCloud22 - - /Features1/PointCloud23 - - /Axes2 - - /Odometry2/Shape1 - - /Path1 - Splitter Ratio: 0.520588219165802 - Tree Height: 839 + - /Odometry1/Odometry1/Covariance1 + - /Odometry1/Odometry1/Covariance1/Position1 + - /Odometry1/Odometry1/Covariance1/Orientation1 + - /MarkerArray1/Namespaces1 + Splitter Ratio: 0.6432291865348816 + Tree Height: 1146 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -47,7 +36,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: currPoints Preferences: PromptSaveOnExit: true Toolbars: @@ -56,7 +45,7 @@ Visualization Manager: Class: "" Displays: - Alpha: 1 - Cell Size: 1 + Cell Size: 1000 Class: rviz/Grid Color: 160; 160; 164 Enabled: false @@ -70,93 +59,16 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 160 + Plane Cell Count: 40 Reference Frame: Value: false - Class: rviz/Axes - Enabled: true + Enabled: false Length: 0.699999988079071 Name: Axes Radius: 0.05999999865889549 Reference Frame: - Value: true - - Class: rviz/Group - Displays: - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 170; 0 - Enabled: false - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: gtPathlc - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /path_gt - Unreliable: false - Value: false - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 170; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: gtPathLidar - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /path_gt_lidar - Unreliable: false - Value: true - Enabled: false - Name: GT - - Class: rviz/Group - Displays: - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 85; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.10000000149011612 - Name: odomPath - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /laser_odom_path - Unreliable: false - Value: true - Enabled: false - Name: odometry + Value: false - Class: rviz/Group Displays: - Alpha: 1 @@ -168,20 +80,20 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor + Color: 238; 238; 236 + Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 15 + Max Intensity: 202 Min Color: 0; 0; 0 Min Intensity: 0 Name: surround Position Transformer: XYZ Queue Size: 1 Selectable: false - Size (Pixels): 2 + Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Points Topic: /cloud_registered @@ -189,22 +101,22 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Alpha: 0.10000000149011612 + - Alpha: 0.07000000029802322 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 7.485495090484619 - Min Value: -0.6337304711341858 - Value: true + Max Value: 15 + Min Value: -5 + Value: false Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor - Decay Time: 1e+9 + Decay Time: 1000 Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 90.00662994384766 + Max Intensity: 0 Min Color: 0; 0; 0 Min Intensity: 0 Name: currPoints @@ -220,36 +132,6 @@ Visualization Manager: Use rainbow: true Value: true - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5.01795768737793 - Min Value: -0.3809538185596466 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - 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: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.05999999865889549 - Style: Flat Squares - Topic: /cloud_effected - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - Value: false - - Alpha: 0.800000011920929 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -258,21 +140,21 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz/PointCloud2 - Color: 239; 41; 41 + Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 151 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 1 + Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.20000000298023224 + Size (m): 0.10000000149011612 Style: Flat Squares Topic: /Laser_map Unreliable: false @@ -281,171 +163,9 @@ Visualization Manager: Value: false Enabled: true Name: mapping - - 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: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 100000 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15.00274658203125 - Min Color: 0; 0; 0 - Min Intensity: -0.0028979615308344364 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Spheres - Topic: /laser_cloud_sharp - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.305999994277954 - Min Value: -0.6200000047683716 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 98; 255; 245 - Color Transformer: FlatColor - Decay Time: 1e+6 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 353 - Min Color: 0; 0; 0 - Min Intensity: 2 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /laser_cloud_flat - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 100000 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 324 - Min Color: 0; 0; 0 - Min Intensity: 91 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /laser_cloud_less_sharp - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: false - Name: Debug - 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: 1000 - Name: Odometry - Position Tolerance: 0.009999999776482582 - Shape: - Alpha: 1 - Axes Length: 0.5 - Axes Radius: 0.009999999776482582 - Color: 0; 255; 0 - Head Length: 0 - Head Radius: 0 - Shaft Length: 0.05000000074505806 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: /re_local_pose - Unreliable: false - Value: true - - 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: 10000 - Name: Odometry - Position Tolerance: 0.0010000000474974513 - Shape: - Alpha: 1 - Axes Length: 0.20000000298023224 - Axes Radius: 0.05000000074505806 - Color: 255; 85; 0 - Head Length: 0 - Head Radius: 0 - Shaft Length: 0.05000000074505806 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: /aft_mapped_to_init - Unreliable: false - Value: true - - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: @@ -468,195 +188,123 @@ Visualization Manager: Position Tolerance: 0.0010000000474974513 Shape: Alpha: 1 - Axes Length: 0.20000000298023224 - Axes Radius: 0.05000000074505806 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 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: /re_local_pose + Topic: /aft_mapped_to_init Unreliable: false Value: true - Enabled: false + Enabled: true Name: Odometry - - 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: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 38.0005989074707 - Min Color: 0; 0; 0 - Min Intensity: 1.000100016593933 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 7 - Size (m): 0.20000000298023224 - Style: Points - Topic: /laser_cloud_flat - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 78; 154; 6 - Max Intensity: 0 - Min Color: 78; 154; 6 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Flat Squares - Topic: /livox_undistort - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 155 - Min Color: 255; 255; 255 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Flat Squares - Topic: /livox_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: false - Value: false - Enabled: false - Name: Features - Class: rviz/Axes Enabled: true Length: 0.699999988079071 Name: Axes - Radius: 0.05999999865889549 - Reference Frame: aft_mapped + Radius: 0.10000000149011612 + Reference Frame: Value: true - - Angle Tolerance: 0.10000000149011612 - 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: false - Keep: 10000 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 0.20000000298023224 - Axes Radius: 0.05000000074505806 - Color: 25; 255; 255 - Head Length: 0 - Head Radius: 0 - Shaft Length: 0.10000000149011612 - Shaft Radius: 0.10000000149011612 - Value: Arrow - Topic: /aft_mapped_to_init - Unreliable: false - Value: false - Alpha: 0 Buffer Length: 2 Class: rviz/Path - Color: 25; 255; 0 + Color: 25; 255; 255 Enabled: true Head Diameter: 0 Head Length: 0 Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 + Line Style: Billboards + Line Width: 0.20000000298023224 Name: Path Offset: X: 0 Y: 0 Z: 0 - Pose Color: 25; 255; 0 - Pose Style: Arrows + Pose Color: 25; 255; 255 + Pose Style: None Radius: 0.029999999329447746 - Shaft Diameter: 0.15000000596046448 - Shaft Length: 0.15000000596046448 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 Topic: /path Unreliable: false Value: true - - Class: rviz/Image + - 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 - Image Topic: /image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw + 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 + Max Intensity: 248 + Min Color: 138; 226; 52 + Min Intensity: 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 + - 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: 30 + Frame Rate: 10 Name: root Tools: - Class: rviz/Interact @@ -678,36 +326,34 @@ Visualization Manager: Value: true Views: Current: - Class: rviz/Orbit - Distance: 49.2824821472168 + Class: rviz/ThirdPersonFollower + Distance: 207.86837768554688 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 9.622546195983887 - Y: -18.686565399169922 - Z: -4.2439751625061035 + X: -66.09013366699219 + Y: 46.88283920288086 + Z: 3.041166019102093e-5 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8597958087921143 - Target Frame: - Value: Orbit (rviz) - Yaw: 2.7072019577026367 + Pitch: 0.8397973775863647 + Target Frame: camera_init + Value: ThirdPersonFollower (rviz) + Yaw: 1.4935526847839355 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 1385 Hide Left Dock: false Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000017900000384fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002700000384000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001aa000001e20000001600fffffffb0000000a0049006d00610067006501000002730000027f00000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074900000052fc0100000002fb0000000800540069006d0065010000000000000749000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005ca0000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016b000004b7fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004b7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bd00000052fc0100000002fb0000000800540069006d00650100000000000009bd000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000084c000004b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -716,6 +362,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1865 - X: 55 - Y: 24 + Width: 2493 + X: 67 + Y: 27 diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp index 0bf2aab..cd383bc 100644 --- a/src/IMU_Processing.hpp +++ b/src/IMU_Processing.hpp @@ -21,12 +21,12 @@ #include #include #include -#include #include +#include "use-ikfom.hpp" /// *************Preconfiguration -#define MAX_INI_COUNT (200) +#define MAX_INI_COUNT (20) const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; @@ -38,116 +38,140 @@ class ImuProcess ImuProcess(); ~ImuProcess(); - - void Process(const MeasureGroup &meas, StatesGroup &state, PointCloudXYZI::Ptr pcl_un_); + void Reset(); - void IMU_Initial(const MeasureGroup &meas, StatesGroup &state, int &N); - - // Eigen::Matrix3d Exp(const Eigen::Vector3d &ang_vel, const double &dt); - - void IntegrateGyr(const std::vector &v_imu); - - void UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_in_out); - - ros::NodeHandle nh; - - void Integrate(const sensor_msgs::ImuConstPtr &imu); void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); + void set_extrinsic(const V3D &transl, const M3D &rot); + void set_extrinsic(const V3D &transl); + void set_extrinsic(const MD(4,4) &T); + void set_gyr_cov(const V3D &scaler); + void set_acc_cov(const V3D &scaler); + void set_gyr_bias_cov(const V3D &b_g); + void set_acc_bias_cov(const V3D &b_a); + Eigen::Matrix Q; + void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); - double scale_gravity; - - Eigen::Vector3d angvel_last; - Eigen::Vector3d acc_s_last; - - Eigen::Matrix cov_proc_noise; - - Eigen::Vector3d cov_acc; - Eigen::Vector3d cov_gyr; - - std::ofstream fout; + ofstream fout_imu; + V3D cov_acc; + V3D cov_gyr; + V3D cov_acc_scale; + V3D cov_gyr_scale; + V3D cov_bias_gyr; + V3D cov_bias_acc; + double first_lidar_time; private: - /*** Whether is the first frame, init for first frame ***/ - bool b_first_frame_ = true; - bool imu_need_init_ = true; + void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); + void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); - int init_iter_num = 1; - Eigen::Vector3d mean_acc; - Eigen::Vector3d mean_gyr; - - /*** Undistorted pointcloud ***/ PointCloudXYZI::Ptr cur_pcl_un_; - - //// For timestamp usage sensor_msgs::ImuConstPtr last_imu_; - - /*** For gyroscope integration ***/ + deque v_imu_; + vector IMUpose; + vector v_rot_pcl_; + M3D Lidar_R_wrt_IMU; + V3D Lidar_T_wrt_IMU; + V3D mean_acc; + V3D mean_gyr; + V3D angvel_last; + V3D acc_s_last; double start_timestamp_; - /// Making sure the equal size: v_imu_ and v_rot_ - std::deque v_imu_; - std::vector v_rot_pcl_; - std::vector IMUpose; + double last_lidar_end_time_; + int init_iter_num = 1; + bool b_first_frame_ = true; + bool imu_need_init_ = true; }; ImuProcess::ImuProcess() - : b_first_frame_(true), imu_need_init_(true), last_imu_(nullptr), start_timestamp_(-1) + : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) { - Eigen::Quaterniond q(0, 1, 0, 0); - Eigen::Vector3d t(0, 0, 0); init_iter_num = 1; - scale_gravity = 1.0; - cov_acc = Eigen::Vector3d(0.1, 0.1, 0.1); - cov_gyr = Eigen::Vector3d(0.1, 0.1, 0.1); - mean_acc = Eigen::Vector3d(0, 0, -1.0); - mean_gyr = Eigen::Vector3d(0, 0, 0); - angvel_last = Zero3d; - cov_proc_noise = Eigen::Matrix::Zero(); - // Lidar_offset_to_IMU = Eigen::Vector3d(0.0, 0.0, -0.0); - // fout.open(DEBUG_FILE_DIR("imu.txt"),std::ios::out); + Q = process_noise_cov(); + cov_acc = V3D(0.1, 0.1, 0.1); + cov_gyr = V3D(0.1, 0.1, 0.1); + cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); + cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + angvel_last = Zero3d; + Lidar_T_wrt_IMU = Zero3d; + Lidar_R_wrt_IMU = Eye3d; + last_imu_.reset(new sensor_msgs::Imu()); } -ImuProcess::~ImuProcess() {fout.close();} +ImuProcess::~ImuProcess() {} void ImuProcess::Reset() { ROS_WARN("Reset ImuProcess"); - scale_gravity = 1.0; - angvel_last = Zero3d; - cov_proc_noise = Eigen::Matrix::Zero(); - - cov_acc = Eigen::Vector3d(0.1, 0.1, 0.1); - cov_gyr = Eigen::Vector3d(0.1, 0.1, 0.1); - mean_acc = Eigen::Vector3d(0, 0, -1.0); - mean_gyr = Eigen::Vector3d(0, 0, 0); - - imu_need_init_ = true; - b_first_frame_ = true; - init_iter_num = 1; - - last_imu_ = nullptr; - - //gyr_int_.Reset(-1, nullptr); - start_timestamp_ = -1; + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + angvel_last = Zero3d; + imu_need_init_ = true; + start_timestamp_ = -1; + init_iter_num = 1; v_imu_.clear(); IMUpose.clear(); - + last_imu_.reset(new sensor_msgs::Imu()); cur_pcl_un_.reset(new PointCloudXYZI()); - fout.close(); - } -void ImuProcess::IMU_Initial(const MeasureGroup &meas, StatesGroup &state_inout, int &N) +void ImuProcess::set_extrinsic(const MD(4,4) &T) +{ + Lidar_T_wrt_IMU = T.block<3,1>(0,3); + Lidar_R_wrt_IMU = T.block<3,3>(0,0); +} + +void ImuProcess::set_extrinsic(const V3D &transl) +{ + Lidar_T_wrt_IMU = transl; + Lidar_R_wrt_IMU.setIdentity(); +} + +void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) +{ + Lidar_T_wrt_IMU = transl; + Lidar_R_wrt_IMU = rot; +} + +void ImuProcess::set_gyr_cov(const V3D &scaler) +{ + cov_gyr_scale = scaler; +} + +void ImuProcess::set_acc_cov(const V3D &scaler) +{ + cov_acc_scale = scaler; +} + +void ImuProcess::set_gyr_bias_cov(const V3D &b_g) +{ + cov_bias_gyr = b_g; +} + +void ImuProcess::set_acc_bias_cov(const V3D &b_a) +{ + cov_bias_acc = b_a; +} + +void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, 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); - Eigen::Vector3d cur_acc, cur_gyr; + V3D cur_acc, cur_gyr; if (b_first_frame_) { Reset(); N = 1; b_first_frame_ = false; + const auto &imu_acc = meas.imu.front()->linear_acceleration; + const auto &gyr_acc = meas.imu.front()->angular_velocity; + mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; + mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + first_lidar_time = meas.lidar_beg_time; + // cout<<"init acc norm: "<::cov init_P = kf_state.get_P(); + init_P.setIdentity(); + init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; + init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; + init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; + init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; + init_P(21,21) = init_P(22,22) = 0.00001; + kf_state.change_P(init_P); + last_imu_ = meas.imu.back(); } -void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out) +void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out) { /*** add the imu of the last frame-tail to the of current frame-head ***/ auto v_imu = meas.imu; @@ -183,27 +222,30 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout /*** sort point clouds by offset time ***/ pcl_out = *(meas.lidar); - std::sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); + sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000); - std::cout<<"[ IMU Process ]: Process lidar from "<::Identity()); - Eigen::MatrixXd cov_w(Eigen::Matrix::Zero()); + V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu; + M3D R_imu; + double dt = 0; - for (auto it_imu = v_imu.begin(); it_imu != (v_imu.end() - 1); it_imu++) + + input_ikfom in; + for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++) { auto &&head = *(it_imu); auto &&tail = *(it_imu + 1); + if (tail->header.stamp.toSec() < last_lidar_end_time_) continue; + angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); @@ -211,71 +253,48 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); - angvel_avr -= state_inout.bias_g; - acc_avr = acc_avr * G_m_s2 / scale_gravity - state_inout.bias_a; + // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; - #ifdef DEBUG_PRINT - // fout<header.stamp.toSec()<<" "<header.stamp.toSec() - head->header.stamp.toSec(); + acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; + + if(head->header.stamp.toSec() < last_lidar_end_time_) + { + dt = tail->header.stamp.toSec() - last_lidar_end_time_; + // dt = tail->header.stamp.toSec() - pcl_beg_time; + } + else + { + dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); + } - /* covariance propagation */ - Eigen::Matrix3d acc_avr_skew; - Eigen::Matrix3d Exp_f = Exp(angvel_avr, dt); - acc_avr_skew<(0,0) = Exp(angvel_avr, - dt); - F_x.block<3,3>(0,9) = - Eye3d * dt; - // F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt; - F_x.block<3,3>(3,6) = Eye3d * dt; - F_x.block<3,3>(6,0) = - R_imu * acc_avr_skew * dt; - F_x.block<3,3>(6,12) = - R_imu * dt; - F_x.block<3,3>(6,15) = Eye3d * dt; - - Eigen::Matrix3d cov_acc_diag(Eye3d), cov_gyr_diag(Eye3d); - cov_acc_diag.diagonal() = cov_acc; - cov_gyr_diag.diagonal() = cov_gyr; - cov_w.block<3,3>(0,0).diagonal() = cov_gyr * dt * dt * 10000; - cov_w.block<3,3>(3,3) = R_imu * cov_gyr_diag * R_imu.transpose() * dt * dt * 10000; - cov_w.block<3,3>(6,6) = R_imu * cov_acc_diag * R_imu.transpose() * dt * dt * 10000; - cov_w.block<3,3>(9,9).diagonal() = Eigen::Vector3d(0.0001, 0.0001, 0.0001) * dt * dt; // bias gyro covariance - cov_w.block<3,3>(12,12).diagonal() = Eigen::Vector3d(0.0001, 0.0001, 0.0001) * dt * dt; // bias acc covariance - - state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w; - - /* propogation of IMU attitude */ - R_imu = R_imu * Exp_f; - - /* Specific acceleration (global frame) of IMU */ - acc_imu = R_imu * acc_avr + state_inout.gravity; - - /* propogation of IMU */ - pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt; - - /* velocity of IMU */ - vel_imu = vel_imu + acc_imu * dt; + in.acc = acc_avr; + in.gyro = angvel_avr; + Q.block<3, 3>(0, 0).diagonal() = cov_gyr; + Q.block<3, 3>(3, 3).diagonal() = cov_acc; + Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; + Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc; + kf_state.predict(dt, Q, in); /* save the poses at each IMU measurements */ - angvel_last = angvel_avr; - acc_s_last = acc_imu; + imu_state = kf_state.get_x(); + angvel_last = angvel_avr - imu_state.bg; + acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); + for(int i=0; i<3; i++) + { + acc_s_last[i] += imu_state.grav[i]; + } double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; - // std::cout<<"acc "< imu_end_time ? 1.0 : -1.0; + dt = note * (pcl_end_time - imu_end_time); + kf_state.predict(dt, Q, in); + + imu_state = kf_state.get_x(); + last_imu_ = meas.imu.back(); + last_lidar_end_time_ = pcl_end_time; /*** undistort each lidar point (backward propagation) ***/ auto it_pcl = pcl_out.points.end() - 1; @@ -284,11 +303,11 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout auto head = it_kp - 1; auto tail = it_kp; R_imu<rot); - acc_imu<acc); - // std::cout<<"head imu acc: "<vel); pos_imu<pos); - angvel_avr<gyr); + acc_imu<acc); + angvel_avr<gyr); for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) { @@ -298,13 +317,13 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout * Note: Compensation direction is INVERSE of Frame's moving direction * So if we want to compensate a point at timestamp-i to the frame-e * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ - Eigen::Matrix3d R_i(R_imu * Exp(angvel_avr, dt)); - Eigen::Vector3d T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt + R_i * Lidar_offset_to_IMU - pos_liD_e); - - Eigen::Vector3d P_i(it_pcl->x, it_pcl->y, it_pcl->z); - Eigen::Vector3d P_compensate = state_inout.rot_end.transpose() * (R_i * P_i + T_ei); - - /// save Undistorted points and their rotation + M3D R_i(R_imu * Exp(angvel_avr, dt)); + + V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); + V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); + V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate! + + // save Undistorted points and their rotation it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2); @@ -314,54 +333,48 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout } } -void ImuProcess::Process(const MeasureGroup &meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_) +void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) { double t1,t2,t3; t1 = omp_get_wtime(); - if(meas.imu.empty()) {std::cout<<"no imu data"< MAX_INI_COUNT) { + cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); imu_need_init_ = false; - // std::cout<<"mean acc: "<("/livox_undistort", 100); - // sensor_msgs::PointCloud2 pcl_out_msg; - // pcl::toROSMsg(*cur_pcl_un_, pcl_out_msg); - // pcl_out_msg.header.stamp = ros::Time().fromSec(meas.lidar_beg_time); - // pcl_out_msg.header.frame_id = "/livox"; - // pub_UndistortPcl.publish(pcl_out_msg); - // } - - /// Record last measurements - last_imu_ = meas.imu.back(); - t3 = omp_get_wtime(); - std::cout<<"[ IMU Process ]: Time: "< -#include -#include -#include - -// Feature will be updated in next version - -using namespace std; - -#define IS_VALID(a) ((abs(a)>1e8) ? true : false) - -typedef pcl::PointXYZINormal PointType; - -ros::Publisher pub_full, pub_surf, pub_corn; - -enum LID_TYPE{MID, HORIZON, VELO16, OUST64}; - -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; - } -}; - -const double rad2deg = 180*M_1_PI; - -int lidar_type; -double blind, inf_bound; -int N_SCANS; -int group_size; -double disA, disB; -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; -int point_filter_num; - -void mid_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); -void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); -void velo16_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); -void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); -void give_feature(pcl::PointCloud &pl, vector &types, pcl::PointCloud &pl_corn, pcl::PointCloud &pl_surf); -void pub_func(pcl::PointCloud &pl, ros::Publisher pub, const ros::Time &ct); -int plane_judge(const pcl::PointCloud &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); -bool small_plane(const pcl::PointCloud &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); -bool edge_jump_judge(const pcl::PointCloud &pl, vector &types, uint i, Surround nor_dir); - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "feature_extract"); - ros::NodeHandle n; - - n.param("lidar_type", lidar_type, 0); - n.param("blind", blind, 0.5); - n.param("inf_bound", inf_bound, 10); - n.param("N_SCANS", N_SCANS, 1); - n.param("group_size", group_size, 8); - n.param("disA", disA, 0.01); - n.param("disB", disB, 0.1); - n.param("p2l_ratio", p2l_ratio, 400); - n.param("limit_maxmid", limit_maxmid, 9); - n.param("limit_midmin", limit_midmin, 16); - n.param("limit_maxmin", limit_maxmin, 3.24); - n.param("jump_up_limit", jump_up_limit, 175.0); - n.param("jump_down_limit", jump_down_limit, 5.0); - n.param("cos160", cos160, 160.0); - n.param("edgea", edgea, 3); - n.param("edgeb", edgeb, 0.05); - n.param("smallp_intersect", smallp_intersect, 170.0); - n.param("smallp_ratio", smallp_ratio, 1.2); - n.param("point_filter_num", point_filter_num, 4); - - 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); - - ros::Subscriber sub_points; - - switch(lidar_type) - { - case MID: - printf("MID40\n"); - sub_points = n.subscribe("/livox/lidar", 1000, mid_handler); - // sub_points = n.subscribe("/livox/lidar_1LVDG1S006J5GZ3", 1000, mid_handler); - break; - - case HORIZON: - printf("HORIZON\n"); - sub_points = n.subscribe("/livox/lidar", 1000, horizon_handler); - break; - - case VELO16: - printf("VELO16\n"); - sub_points = n.subscribe("/velodyne_points", 1000, velo16_handler); - break; - - case OUST64: - printf("OUST64\n"); - sub_points = n.subscribe("/os1_cloud_node/points", 1000, oust64_handler); - break; - - default: - printf("Lidar type is wrong.\n"); - exit(0); - break; - } - - pub_full = n.advertise("/laser_cloud", 100); - pub_surf = n.advertise("/laser_cloud_flat", 100); - pub_corn = n.advertise("/laser_cloud_sharp", 100); - - ros::spin(); - return 0; -} - - -double vx, vy, vz; -void mid_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pcl::PointCloud pl; - pcl::fromROSMsg(*msg, pl); - - pcl::PointCloud pl_corn, pl_surf; - vector types; - uint plsize = pl.size()-1; - pl_corn.reserve(plsize); pl_surf.reserve(plsize); - types.resize(plsize+1); - - for(uint i=0; iheader.stamp); - pub_func(pl_surf, pub_surf, msg->header.stamp); - pub_func(pl_corn, pub_corn, msg->header.stamp); -} - -void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) -{ - double t1 = omp_get_wtime(); - vector> pl_buff(N_SCANS); - vector> typess(N_SCANS); - pcl::PointCloud pl_full, pl_corn, pl_surf; - - uint plsize = msg->point_num; - - pl_corn.reserve(plsize); pl_surf.reserve(plsize); - pl_full.resize(plsize); - - for(int i=0; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10) - && (!IS_VALID(msg->points[i].x)) && (!IS_VALID(msg->points[i].y)) && (!IS_VALID(msg->points[i].z))) - { - 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 - - if((std::abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) - || (std::abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) - || (std::abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) - { - pl_buff[msg->points[i].line].push_back(pl_full[i]); - } - } - } - - if(pl_buff[0].size() <= 7) {return;} - - for(int j=0; j &pl = pl_buff[j]; - vector &types = typess[j]; - plsize = pl.size(); - types.resize(plsize); - plsize--; - for(uint i=0; itimebase); - pub_func(pl_full, pub_full, msg->header.stamp); - pub_func(pl_surf, pub_surf, msg->header.stamp); - pub_func(pl_corn, pub_corn, msg->header.stamp); - std::cout<<"[~~~~~~~~~~~~ Feature Extract ]: time: "<< omp_get_wtime() - t1<<" "<header.stamp.toSec()< pl_orig; - pcl::fromROSMsg(*msg, pl_orig); - uint plsize = pl_orig.size(); - - vector> pl_buff(N_SCANS); - vector> typess(N_SCANS); - pcl::PointCloud pl_corn, pl_surf, pl_full; - - int scanID; - int last_stat = -1; - int idx = 0; - - for(int i=0; i=N_SCANS || scanID<0) - { - continue; - } - - if(orders[scanID] <= last_stat) - { - idx++; - } - - pl_buff[scanID][idx].x = ap.x; - pl_buff[scanID][idx].y = ap.y; - pl_buff[scanID][idx].z = ap.z; - pl_buff[scanID][idx].intensity = ap.intensity; - typess[scanID][idx].range = leng; - last_stat = orders[scanID]; - } - - // for(int i=0; iheader.stamp); - // } - - - // for(int i=0; i<10; i++) - // { - // printf("%f %f %f\n", pl_buff[0][i].x, pl_buff[0][i].y, pl_buff[0][i].z); - // } - // cout << endl; - - idx++; - - for(int j=0; j &pl = pl_buff[j]; - vector &types = typess[j]; - pl.erase(pl.begin()+idx, pl.end()); - types.erase(types.begin()+idx, types.end()); - plsize = idx - 1; - for(uint i=0; iheader.stamp); - pub_func(pl_surf, pub_surf, msg->header.stamp); - pub_func(pl_corn, pub_corn, msg->header.stamp); - -} - - -void velo16_handler1(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pcl::PointCloud pl_orig; - pcl::fromROSMsg(*msg, pl_orig); - - pcl::PointCloud pl_corn, pl_surf, pl_full; - vector> pl_buff(N_SCANS); - vector> typess(N_SCANS); - - uint plsize = pl_orig.size(); - pl_corn.reserve(plsize); - pl_surf.reserve(plsize); - pl_full.reserve(plsize); - - for(int i=0; i blind) - { - if(stat == 0) - { - stat = 1; - idx++; - } - - double ang = atan(ap.z / leng)*rad2deg; - scanID = int((ang + 15) / 2 + 0.5); - if(scanID>=N_SCANS || scanID <0) - { - continue; - } - pl_buff[scanID][idx] = ap; - typess[scanID][idx].range = leng; - pl_full.push_back(ap); - } - else - { - stat = 0; - } - } - - // idx = 0; - // int last_stat = -1; - // for(uint i=0; i=N_SCANS || scanID<0) - // { - // continue; - // } - - // if(orders[scanID] <= last_stat) - // { - // idx++; - // } - - // pl_buff[scanID][idx].x = ap.x; - // pl_buff[scanID][idx].y = ap.y; - // pl_buff[scanID][idx].z = ap.z; - // pl_buff[scanID][idx].intensity = ap.intensity; - - // last_stat = orders[scanID]; - // } - - idx++; - - for(int j=0; j &pl = pl_buff[j]; - vector &types = typess[j]; - pl.erase(pl.begin()+idx, pl.end()); - types.erase(types.begin()+idx, types.end()); - plsize = idx - 1; - for(uint i=0; iheader.stamp); - pub_func(pl_surf, pub_surf, msg->header.stamp); - pub_func(pl_corn, pub_corn, msg->header.stamp); -} - -void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) -{ - pcl::PointCloud pl_orig; - pcl::fromROSMsg(*msg, pl_orig); - - vector> pl_buff(N_SCANS); - vector> typess(N_SCANS); - pcl::PointCloud pl_corn, pl_surf; - - uint plsize = pl_orig.size(); - - pl_corn.reserve(plsize); pl_surf.reserve(plsize); - for(int i=0; i &pl = pl_buff[j]; - vector &types = typess[j]; - plsize = pl.size() - 1; - types.resize(plsize+1); - for(uint i=0; iheader.stamp); - pub_func(pl_surf, pub_surf, msg->header.stamp); - pub_func(pl_corn, pub_corn, msg->header.stamp); -} - - -void give_feature(pcl::PointCloud &pl, vector &types, pcl::PointCloud &pl_corn, pcl::PointCloud &pl_surf) -{ - uint plsize = pl.size(); - uint 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; - } - else if(plane_type == 0) - { - if(last_state == 1) - { - uint i_nex_tem; - uint j; - for(j=last_i+1; j<=last_i_nex; j++) - { - uint i_nex_tem2 = i_nex_tem; - Eigen::Vector3d curr_direct2; - - uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); - - if(ttem != 1) - { - i_nex_tem = i_nex_tem2; - break; - } - curr_direct = curr_direct2; - } - - if(j == last_i+1) - { - last_state = 0; - } - else - { - for(uint k=last_i_nex; k<=i_nex_tem; k++) - { - if(k != i_nex_tem) - { - types[k].ftype = Real_Plane; - } - else - { - types[k].ftype = Poss_Plane; - } - } - i = i_nex_tem-1; - i_nex = i_nex_tem; - i2 = j-1; - last_state = 1; - } - - } - } - - last_i = i2; - last_i_nex = i_nex; - last_direct = curr_direct; - - } - - plsize2 = plsize > 3 ? plsize - 3 : 0; - for(uint i=head+3; i=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 &pl, ros::Publisher pub, const ros::Time &ct) -{ - pl.height = 1; pl.width = pl.size(); - sensor_msgs::PointCloud2 output; - pcl::toROSMsg(pl, output); - output.header.frame_id = "livox"; - output.header.stamp = ct; - pub.publish(output); -} - -int plane_judge(const pcl::PointCloud &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) -{ - double group_dis = disA*types[i_cur].range + disB; - group_dis = group_dis * group_dis; - // i_nex = i_cur; - - double two_dis; - vector disarr; - disarr.reserve(20); - - for(i_nex=i_cur; i_nex= 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 edge_jump_judge(const pcl::PointCloud &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/laserMapping.cpp b/src/laserMapping.cpp index 2ef885a..d275ea1 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -43,117 +43,141 @@ #include #include #include -#include -#include - #include "IMU_Processing.hpp" #include #include -#include #include #include #include #include #include -#include #include #include #include #include -#include #include +#include +#include "preprocess.h" +#include +#define INIT_TIME (0.1) +#define LASER_POINT_COV (0.001) +#define MAXN (720000) +#define PUBFRAME_PERIOD (20) -#ifdef DEPLOY -#include "matplotlibcpp.h" -namespace plt = matplotlibcpp; -#endif +/*** Time Log Variables ***/ +double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0; +double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN]; +double match_time = 0, solve_time = 0, solve_const_H_time = 0; +int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0; +bool runtime_pos_log = false; +/**************************/ -#define INIT_TIME (0) -#define LASER_POINT_COV (0.0015) -#define NUM_MATCH_POINTS (5) +float res_last[100000] = {0.0}; +float DET_RANGE = 300.0f; +const float MOV_THRESHOLD = 1.5f; -std::string root_dir = ROOT_DIR; +mutex mtx_buffer; +condition_variable sig_buffer; -int iterCount = 0; -int NUM_MAX_ITERATIONS = 0; -int FOV_RANGE = 3; // range of FOV = FOV_RANGE * cube_len -int laserCloudCenWidth = 24; -int laserCloudCenHeight = 24; -int laserCloudCenDepth = 24; +string root_dir = ROOT_DIR; +string map_file_path, lid_topic, imu_topic; -int laserCloudValidNum = 0; -int laserCloudSelNum = 0; +double res_mean_last = 0.05, total_residual = 0.0; +double last_timestamp_lidar = 0, last_timestamp_imu = -1.0; +double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001; +double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; +double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0; +int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0; +int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0; +bool point_selected_surf[100000] = {0}; +bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited; +bool dense_map_en = true; -const int laserCloudWidth = 48; -const int laserCloudHeight = 48; -const int laserCloudDepth = 48; -const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; +vector> pointSearchInd_surf; +vector cub_needrm; +vector Nearest_Points; +vector extrinT(3, 0.0); +vector extrinR(9, 0.0); +deque time_buffer; +deque lidar_buffer; +deque imu_buffer; -std::vector T1, T2, s_plot, s_plot2, s_plot3, s_plot4, s_plot5, s_plot6; - -/// IMU relative variables -std::mutex mtx_buffer; -std::condition_variable sig_buffer; -bool lidar_pushed = false; -bool flg_exit = false; -bool flg_reset = false; -bool flg_map_inited = false; - -/// Buffers for measurements -double cube_len = 0.0; -double lidar_end_time = 0.0; -double last_timestamp_lidar = -1; -double last_timestamp_imu = -1; -double HALF_FOV_COS = 0.0; -double res_mean_last = 0.05; -double total_distance = 0.0; -auto position_last = Zero3d; -double copy_time, readd_time; - -std::deque lidar_buffer; -std::deque imu_buffer; - -//surf feature in map PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); -PointCloudXYZI::Ptr cube_points_add(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); +PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr _featsArray; -//all points -PointCloudXYZI::Ptr laserCloudFullRes2(new PointCloudXYZI()); -PointCloudXYZI::Ptr featsArray[laserCloudNum]; -bool _last_inFOV[laserCloudNum]; -bool cube_updated[laserCloudNum]; -int laserCloudValidInd[laserCloudNum]; -pcl::PointCloud::Ptr laserCloudFullResColor(new pcl::PointCloud()); +pcl::VoxelGrid downSizeFilterSurf; +pcl::VoxelGrid downSizeFilterMap; -#ifdef USE_ikdtree KD_TREE ikdtree; -std::vector cub_needrm; -std::vector cub_needad; -#else -pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN()); -#endif -Eigen::Vector3f XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); -Eigen::Vector3f XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); +V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); +V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); +V3D euler_cur; +V3D position_last(Zero3d); +V3D Lidar_T_wrt_IMU(Zero3d); +M3D Lidar_R_wrt_IMU(Eye3d); -//estimator inputs and output; +/*** EKF inputs and output ***/ MeasureGroup Measures; -StatesGroup state; +esekfom::esekf kf; +state_ikfom state_point; +vect3 pos_lid; + +nav_msgs::Path path; +nav_msgs::Odometry odomAftMapped; +geometry_msgs::Quaternion geoQuat; +geometry_msgs::PoseStamped msg_body_pose; + +shared_ptr p_pre(new Preprocess()); +shared_ptr p_imu(new ImuProcess()); void SigHandle(int sig) { - flg_exit = true; - ROS_WARN("catch sig %d", sig); - sig_buffer.notify_all(); + flg_exit = true; + ROS_WARN("catch sig %d", sig); + sig_buffer.notify_all(); } -//project lidar frame to world +inline void dump_lio_state_to_log(FILE *fp) +{ + V3D rot_ang(Log(state_point.rot.toRotationMatrix())); + fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); + fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle + fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega + fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc + fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2)); // Bias_g + fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2)); // Bias_a + fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a + fprintf(fp, "\r\n"); + fflush(fp); +} + +void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + + void pointBodyToWorld(PointType const * const pi, PointType * const po) { - Eigen::Vector3d p_body(pi->x, pi->y, pi->z); - Eigen::Vector3d p_global(state.rot_end * (p_body + Lidar_offset_to_IMU) + state.pos_end); - + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); @@ -161,404 +185,128 @@ void pointBodyToWorld(PointType const * const pi, PointType * const po) } template -void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &po) +void pointBodyToWorld(const Matrix &pi, Matrix &po) { - Eigen::Vector3d p_body(pi[0], pi[1], pi[2]); - Eigen::Vector3d p_global(state.rot_end * (p_body + Lidar_offset_to_IMU) + state.pos_end); + V3D p_body(pi[0], pi[1], pi[2]); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + po[0] = p_global(0); po[1] = p_global(1); po[2] = p_global(2); } -void RGBpointBodyToWorld(PointType const * const pi, pcl::PointXYZI * const po) +void RGBpointBodyToWorld(PointType const * const pi, PointType * const po) { - Eigen::Vector3d p_body(pi->x, pi->y, pi->z); - Eigen::Vector3d p_global(state.rot_end * (p_body + Lidar_offset_to_IMU) + state.pos_end); - + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + po->x = p_global(0); po->y = p_global(1); po->z = p_global(2); po->intensity = pi->intensity; - - float intensity = pi->intensity; - intensity = intensity - std::floor(intensity); - - int reflection_map = intensity*10000; - - // //std::cout<<"DEBUG reflection_map "<r = 0; - // po->g = green & 0xff; - // po->b = 0xff; - // } - // else if (reflection_map < 90) - // { - // int blue = (((90 - reflection_map) * 255) / 60); - // po->r = 0x0; - // po->g = 0xff; - // po->b = blue & 0xff; - // } - // else if (reflection_map < 150) - // { - // int red = ((reflection_map-90) * 255 / 60); - // po->r = red & 0xff; - // po->g = 0xff; - // po->b = 0x0; - // } - // else - // { - // int green = (((255-reflection_map) * 255) / (255-150)); - // po->r = 0xff; - // po->g = green & 0xff; - // po->b = 0; - // } } -int cube_ind(const int &i, const int &j, const int &k) +void points_cache_collect() { - return (i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k); -} - -bool CenterinFOV(Eigen::Vector3f cube_p) -{ - Eigen::Vector3f dis_vec = state.pos_end.cast() - cube_p; - float squaredSide1 = dis_vec.transpose() * dis_vec; - - if(squaredSide1 < 0.4 * cube_len * cube_len) return true; - - dis_vec = XAxisPoint_world.cast() - cube_p; - float squaredSide2 = dis_vec.transpose() * dis_vec; - - float ang_cos = fabs(squaredSide1 <= 3) ? 1.0 : - (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1)); - - return ((ang_cos > HALF_FOV_COS)? true : false); -} - -bool CornerinFOV(Eigen::Vector3f cube_p) -{ - Eigen::Vector3f dis_vec = state.pos_end.cast() - cube_p; - float squaredSide1 = dis_vec.transpose() * dis_vec; - - dis_vec = XAxisPoint_world.cast() - cube_p; - float squaredSide2 = dis_vec.transpose() * dis_vec; - - float ang_cos = fabs(squaredSide1 <= 3) ? 1.0 : - (LIDAR_SP_LEN * LIDAR_SP_LEN + squaredSide1 - squaredSide2) / (2 * LIDAR_SP_LEN * sqrt(squaredSide1)); - - return ((ang_cos > HALF_FOV_COS)? true : false); + PointVector points_history; + ikdtree.acquire_removed_points(points_history); + for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]); } +BoxPointType LocalMap_Points; +bool Localmap_Initialized = false; void lasermap_fov_segment() { - laserCloudValidNum = 0; - - pointBodyToWorld(XAxisPoint_body, XAxisPoint_world); - - int centerCubeI = int((state.pos_end(0) + 0.5 * cube_len) / cube_len) + laserCloudCenWidth; - int centerCubeJ = int((state.pos_end(1) + 0.5 * cube_len) / cube_len) + laserCloudCenHeight; - int centerCubeK = int((state.pos_end(2) + 0.5 * cube_len) / cube_len) + laserCloudCenDepth; - - if (state.pos_end(0) + 0.5 * cube_len < 0) centerCubeI--; - if (state.pos_end(1) + 0.5 * cube_len < 0) centerCubeJ--; - if (state.pos_end(2) + 0.5 * cube_len < 0) centerCubeK--; - - bool last_inFOV_flag = 0; - int cube_index = 0; - - T2.push_back(Measures.lidar_beg_time); - double t_begin = omp_get_wtime(); - - std::cout<<"centerCubeIJK: "<= 1; i--) { - featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i-1, j, k)]; - _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i-1, j, k)]; - } - - featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer; - _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag; - laserCloudCubeSurfPointer->clear(); - } - } - centerCubeI++; - laserCloudCenWidth++; - } - - while (centerCubeI >= laserCloudWidth - (FOV_RANGE + 1)) { - for (int j = 0; j < laserCloudHeight; j++) { - for (int k = 0; k < laserCloudDepth; k++) { - int i = 0; - - PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)]; - last_inFOV_flag = _last_inFOV[cube_index]; - - for (; i >= 1; i--) { - featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i+1, j, k)]; - _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i+1, j, k)]; - } - - featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer; - _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag; - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeI--; - laserCloudCenWidth--; - } - - while (centerCubeJ < (FOV_RANGE + 1)) { - for (int i = 0; i < laserCloudWidth; i++) { - for (int k = 0; k < laserCloudDepth; k++) { - int j = laserCloudHeight - 1; - - PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)]; - last_inFOV_flag = _last_inFOV[cube_index]; - - for (; i >= 1; i--) { - featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j-1, k)]; - _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j-1, k)]; - } - - featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer; - _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag; - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeJ++; - laserCloudCenHeight++; - } - - while (centerCubeJ >= laserCloudHeight - (FOV_RANGE + 1)) { - for (int i = 0; i < laserCloudWidth; i++) { - for (int k = 0; k < laserCloudDepth; k++) { - int j = 0; - PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)]; - last_inFOV_flag = _last_inFOV[cube_index]; - - for (; i >= 1; i--) { - featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j+1, k)]; - _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j+1, k)]; - } - - featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer; - _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag; - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeJ--; - laserCloudCenHeight--; - } - - while (centerCubeK < (FOV_RANGE + 1)) { - for (int i = 0; i < laserCloudWidth; i++) { - for (int j = 0; j < laserCloudHeight; j++) { - int k = laserCloudDepth - 1; - PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)]; - last_inFOV_flag = _last_inFOV[cube_index]; - - for (; i >= 1; i--) { - featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j, k-1)]; - _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j, k-1)]; - } - - featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer; - _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag; - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeK++; - laserCloudCenDepth++; - } - - while (centerCubeK >= laserCloudDepth - (FOV_RANGE + 1)) - { - for (int i = 0; i < laserCloudWidth; i++) - { - for (int j = 0; j < laserCloudHeight; j++) - { - int k = 0; - PointCloudXYZI::Ptr laserCloudCubeSurfPointer = featsArray[cube_ind(i, j, k)]; - last_inFOV_flag = _last_inFOV[cube_index]; - - for (; i >= 1; i--) { - featsArray[cube_ind(i, j, k)] = featsArray[cube_ind(i, j, k+1)]; - _last_inFOV[cube_ind(i, j, k)] = _last_inFOV[cube_ind(i, j, k+1)]; - } - - featsArray[cube_ind(i, j, k)] = laserCloudCubeSurfPointer; - _last_inFOV[cube_ind(i, j, k)] = last_inFOV_flag; - laserCloudCubeSurfPointer->clear(); - } - } - - centerCubeK--; - laserCloudCenDepth--; - } - - cube_points_add->clear(); - featsFromMap->clear(); - bool now_inFOV[laserCloudNum] = {false}; - - // std::cout<<"centerCubeIJK: "<= 0 && i < laserCloudWidth && - j >= 0 && j < laserCloudHeight && - k >= 0 && k < laserCloudDepth) - { - Eigen::Vector3f center_p(cube_len * (i - laserCloudCenWidth), \ - cube_len * (j - laserCloudCenHeight), \ - cube_len * (k - laserCloudCenDepth)); - - float check1, check2; - float squaredSide1, squaredSide2; - float ang_cos = 1; - bool &last_inFOV = _last_inFOV[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; - bool inFOV = CenterinFOV(center_p); - - for (int ii = -1; (ii <= 1) && (!inFOV); ii += 2) - { - for (int jj = -1; (jj <= 1) && (!inFOV); jj += 2) - { - for (int kk = -1; (kk <= 1) && (!inFOV); kk += 2) - { - Eigen::Vector3f corner_p(cube_len * ii, cube_len * jj, cube_len * kk); - corner_p = center_p + 0.5 * corner_p; - - inFOV = CornerinFOV(corner_p); - } - } - } - - now_inFOV[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = inFOV; - - #ifdef USE_ikdtree - /*** readd cubes and points ***/ - if (inFOV) - { - int center_index = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; - *cube_points_add += *featsArray[center_index]; - featsArray[center_index]->clear(); - if (!last_inFOV) - { - BoxPointType cub_points; - for(int i = 0; i < 3; i++) - { - cub_points.vertex_max[i] = center_p[i] + 0.5 * cube_len; - cub_points.vertex_min[i] = center_p[i] - 0.5 * cube_len; - } - cub_needad.push_back(cub_points); - laserCloudValidInd[laserCloudValidNum] = center_index; - laserCloudValidNum ++; - // std::cout<<"readd center: "< 0) ikdtree.Delete_Point_Boxes(cub_needrm); - // s_plot4.push_back(omp_get_wtime() - t_begin); t_begin = omp_get_wtime(); - if(cub_needad.size() > 0) ikdtree.Add_Point_Boxes(cub_needad); - // s_plot5.push_back(omp_get_wtime() - t_begin); t_begin = omp_get_wtime(); - if(cube_points_add->points.size() > 0) ikdtree.Add_Points(cube_points_add->points, true); -#endif - s_plot6.push_back(omp_get_wtime() - t_begin); - readd_time = omp_get_wtime() - t_begin - copy_time; + points_cache_collect(); + double delete_begin = omp_get_wtime(); + if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); + kdtree_delete_time = omp_get_wtime() - delete_begin; } -void feat_points_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) +void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) { mtx_buffer.lock(); - // std::cout<<"got feature"<header.stamp.toSec() < last_timestamp_lidar) { ROS_ERROR("lidar loop back, clear buffer"); lidar_buffer.clear(); } - // ROS_INFO("get point cloud at time: %.6f", msg->header.stamp.toSec()); - lidar_buffer.push_back(msg); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lidar_buffer.push_back(ptr); + time_buffer.push_back(msg->header.stamp.toSec()); last_timestamp_lidar = 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(); + scan_count ++; + double preprocess_start_time = omp_get_wtime(); + if (msg->header.stamp.toSec() < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + lidar_buffer.clear(); + } + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lidar_buffer.push_back(ptr); + time_buffer.push_back(msg->header.stamp.toSec()); + last_timestamp_lidar = 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)); double timestamp = msg->header.stamp.toSec(); @@ -575,7 +323,6 @@ void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) last_timestamp_imu = timestamp; imu_buffer.push_back(msg); - // std::cout<<"got imu: "<header.stamp.toSec(); + meas.lidar = lidar_buffer.front(); + if(meas.lidar->points.size() <= 1) + { + lidar_buffer.pop_front(); + return false; + } + meas.lidar_beg_time = time_buffer.front(); lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); lidar_pushed = true; } @@ -607,100 +358,376 @@ bool sync_packages(MeasureGroup &meas) while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) { imu_time = imu_buffer.front()->header.stamp.toSec(); - if(imu_time > lidar_end_time + 0.02) break; + if(imu_time > lidar_end_time) break; meas.imu.push_back(imu_buffer.front()); imu_buffer.pop_front(); } lidar_buffer.pop_front(); + time_buffer.pop_front(); lidar_pushed = false; - // if (meas.imu.empty()) return false; - // std::cout<<"[IMU Sycned]: "<points[i]), &(feats_down_world->points[i])); + /* decide if need add to map */ + if (!Nearest_Points[i].empty() && flg_EKF_inited) + { + const PointVector &points_near = Nearest_Points[i]; + bool need_add = true; + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + float dist = calc_dist(feats_down_world->points[i],mid_point); + if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min){ + PointNoNeedDownsample.push_back(feats_down_world->points[i]); + continue; + } + for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i ++) + { + if (points_near.size() < NUM_MATCH_POINTS) break; + if (calc_dist(points_near[readd_i], mid_point) < dist) + { + need_add = false; + break; + } + } + if (need_add) PointToAdd.push_back(feats_down_world->points[i]); + } + else + { + PointToAdd.push_back(feats_down_world->points[i]); + } + } + + double st_time = omp_get_wtime(); + add_point_size = ikdtree.Add_Points(PointToAdd, true); + ikdtree.Add_Points(PointNoNeedDownsample, false); + add_point_size = PointToAdd.size() + PointNoNeedDownsample.size(); + kdtree_incremental_time = omp_get_wtime() - st_time; +} + +PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); +void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) +{ + PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); + int size = laserCloudFullRes->points.size(); + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], \ + &laserCloudWorld->points[i]); + } + + *pcl_wait_pub += *laserCloudWorld; + if(1) + { + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*pcl_wait_pub, laserCloudmsg); + laserCloudmsg.header.stamp = ros::Time::now(); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; + pcl_wait_pub->clear(); + } +} + +void publish_effect_world(const ros::Publisher & pubLaserCloudEffect) +{ + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(effct_feat_num, 1)); + for (int i = 0; i < effct_feat_num; i++) + { + RGBpointBodyToWorld(&laserCloudOri->points[i], \ + &laserCloudWorld->points[i]); + } + sensor_msgs::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = ros::Time::now(); + laserCloudFullRes3.header.frame_id = "camera_init"; + pubLaserCloudEffect.publish(laserCloudFullRes3); +} + +void publish_map(const ros::Publisher & pubLaserCloudMap) +{ + sensor_msgs::PointCloud2 laserCloudMap; + pcl::toROSMsg(*featsFromMap, laserCloudMap); + laserCloudMap.header.stamp = ros::Time::now(); + laserCloudMap.header.frame_id = "camera_init"; + pubLaserCloudMap.publish(laserCloudMap); +} + +template +void set_posestamp(T & out) +{ + out.pose.position.x = state_point.pos(0); + out.pose.position.y = state_point.pos(1); + out.pose.position.z = state_point.pos(2); + out.pose.orientation.x = geoQuat.x; + out.pose.orientation.y = geoQuat.y; + out.pose.orientation.z = geoQuat.z; + out.pose.orientation.w = geoQuat.w; + +} + +void publish_odometry(const ros::Publisher & pubOdomAftMapped) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "aft_mapped"; + odomAftMapped.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar); + set_posestamp(odomAftMapped.pose); + pubOdomAftMapped.publish(odomAftMapped); + auto P = kf.get_P(); + for (int i = 0; i < 6; i ++) + { + int k = i < 3 ? i + 3 : i - 3; + odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3); + odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4); + odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5); + odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0); + odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1); + odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2); + } + + 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); + msg_body_pose.header.stamp = ros::Time::now(); + msg_body_pose.header.frame_id = "camera_init"; + + /*** if path is too large, the rvis will crash ***/ + static int jjj = 0; + jjj++; + if (jjj % 10 == 0) + { + path.poses.push_back(msg_body_pose); + pubPath.publish(path); + } +} + +void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct &ekfom_data) +{ + double match_start = omp_get_wtime(); + laserCloudOri->clear(); + corr_normvect->clear(); + total_residual = 0.0; + + /** closest surface search and residual computation **/ + #ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); + #pragma omp parallel for + #endif + for (int i = 0; i < feats_down_size; i++) + { + PointType &point_body = feats_down_body->points[i]; + PointType &point_world = feats_down_world->points[i]; + + /* transform to world frame */ + V3D p_body(point_body.x, point_body.y, point_body.z); + V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos); + point_world.x = p_global(0); + point_world.y = p_global(1); + point_world.z = p_global(2); + point_world.intensity = point_body.intensity; + + vector pointSearchSqDis(NUM_MATCH_POINTS); + + auto &points_near = Nearest_Points[i]; + + if (ekfom_data.converge) + { + /** Find the closest surfaces in the map **/ + ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis); + point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false : true; + } + + if (!point_selected_surf[i]) continue; + + VF(4) pabcd; + point_selected_surf[i] = false; + if (esti_plane(pabcd, points_near, 0.1f)) + { + float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3); + float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm()); + + if (s > 0.9) + { + point_selected_surf[i] = true; + normvec->points[i].x = pabcd(0); + normvec->points[i].y = pabcd(1); + normvec->points[i].z = pabcd(2); + normvec->points[i].intensity = pd2; + res_last[i] = abs(pd2); + } + } + } + + effct_feat_num = 0; + + for (int i = 0; i < feats_down_size; i++) + { + if (point_selected_surf[i]) + { + laserCloudOri->points[effct_feat_num] = feats_down_body->points[i]; + corr_normvect->points[effct_feat_num] = normvec->points[i]; + total_residual += res_last[i]; + effct_feat_num ++; + } + } + + res_mean_last = total_residual / effct_feat_num; + match_time += omp_get_wtime() - match_start; + double solve_start_ = omp_get_wtime(); + + /*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/ + ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //23 + ekfom_data.h.resize(effct_feat_num); + + for (int i = 0; i < effct_feat_num; i++) + { + const PointType &laser_p = laserCloudOri->points[i]; + V3D point_this_be(laser_p.x, laser_p.y, laser_p.z); + M3D point_be_crossmat; + point_be_crossmat << SKEW_SYM_MATRX(point_this_be); + V3D point_this = s.offset_R_L_I * point_this_be + s.offset_T_L_I; + M3D point_crossmat; + point_crossmat<points[i]; + V3D norm_vec(norm_p.x, norm_p.y, norm_p.z); + + /*** calculate the Measuremnt Jacobian matrix H ***/ + V3D C(s.rot.conjugate() *norm_vec); + V3D A(point_crossmat * C); + V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec); + ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); + + /*** Measuremnt: distance to the closest surface/corner ***/ + ekfom_data.h(i) = -norm_p.intensity; + } + solve_time += omp_get_wtime() - solve_start_; +} + int main(int argc, char** argv) { ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh; - ros::Subscriber sub_pcl = nh.subscribe("/laser_cloud_flat", 20000, feat_points_cbk); - ros::Subscriber sub_imu = nh.subscribe("/livox/imu", 20000, imu_cbk); - ros::Publisher pubLaserCloudFullRes = nh.advertise - ("/cloud_registered", 100); - ros::Publisher pubLaserCloudEffect = nh.advertise - ("/cloud_effected", 100); - ros::Publisher pubLaserCloudMap = nh.advertise - ("/Laser_map", 100); - ros::Publisher pubOdomAftMapped = nh.advertise - ("/aft_mapped_to_init", 10); - ros::Publisher pubPath = nh.advertise - ("/path", 10); -#ifdef DEPLOY - ros::Publisher mavros_pose_publisher = nh.advertise("/mavros/vision_pose/pose", 10); -#endif - geometry_msgs::PoseStamped msg_body_pose; - nav_msgs::Path path; + nh.param("dense_map_enable",dense_map_en,1); + nh.param("max_iteration",NUM_MAX_ITERATIONS,4); + nh.param("map_file_path",map_file_path,""); + nh.param("common/lid_topic",lid_topic,"/livox/lidar"); + nh.param("common/imu_topic", imu_topic,"/livox/imu"); + nh.param("filter_size_corner",filter_size_corner_min,0.5); + 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/gyr_cov",gyr_cov,0.1); + nh.param("mapping/acc_cov",acc_cov,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("preprocess/blind", p_pre->blind, 0.01); + nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); + nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); + nh.param("point_filter_num", p_pre->point_filter_num, 2); + nh.param("feature_extract_enable", p_pre->feature_enabled, 0); + nh.param("runtime_pos_log_enable", runtime_pos_log, 0); + nh.param>("mapping/extrinsic_T", extrinT, vector()); + nh.param>("mapping/extrinsic_R", extrinR, vector()); + cout<<"p_pre->lidar_type "<lidar_type< G, H_T_H, I_STATE; - G.setZero(); - H_T_H.setZero(); - I_STATE.setIdentity(); + double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0; + bool flg_EKF_converged, EKF_stop_flg = 0; + + FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); + HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); - nav_msgs::Odometry odomAftMapped; - - cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); - cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); - cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); - cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0)); - - PointType pointOri, pointSel, coeff; - PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); - PointCloudXYZI::Ptr feats_down(new PointCloudXYZI()); - PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI()); - PointCloudXYZI::Ptr coeffSel(new PointCloudXYZI()); - pcl::VoxelGrid downSizeFilterSurf; - pcl::VoxelGrid downSizeFilterMap; - - /*** variables initialize ***/ - ros::param::get("~dense_map_enable",dense_map_en); - ros::param::get("~max_iteration",NUM_MAX_ITERATIONS); - ros::param::get("~map_file_path",map_file_path); - ros::param::get("~fov_degree",fov_deg); - ros::param::get("~filter_size_corner",filter_size_corner_min); - ros::param::get("~filter_size_surf",filter_size_surf_min); - ros::param::get("~filter_size_map",filter_size_map_min); - ros::param::get("~cube_side_length",cube_len); - - HALF_FOV_COS = std::cos((fov_deg + 10.0) * 0.5 * PI_M / 180.0); - - for (int i = 0; i < laserCloudNum; i++) - { - featsArray[i].reset(new PointCloudXYZI()); - } + _featsArray.reset(new PointCloudXYZI()); + memset(point_selected_surf, true, sizeof(point_selected_surf)); + memset(res_last, -1000.0f, sizeof(res_last)); downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + memset(point_selected_surf, true, sizeof(point_selected_surf)); + memset(res_last, -1000.0f, sizeof(res_last)); - std::shared_ptr p_imu(new ImuProcess()); + Lidar_T_wrt_IMU<set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); + p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); + p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); + p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); + p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); + + double epsi[23] = {0.001}; + fill(epsi, epsi+23, 0.001); + kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi); /*** debug record ***/ - std::ofstream fout_pre, fout_out; - fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),std::ios::out); - fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),std::ios::out); - if (fout_pre && fout_out) - std::cout << "~~~~"<lidar_type == AVIA ? \ + nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ + 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 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); //------------------------------------------------------------------------------------------------------ signal(SIGINT, SigHandle); ros::Rate rate(5000); @@ -709,7 +736,7 @@ int main(int argc, char** argv) { if (flg_exit) break; ros::spinOnce(); - while(sync_packages(Measures)) + if(sync_packages(Measures)) { if (flg_reset) { @@ -719,606 +746,175 @@ int main(int argc, char** argv) continue; } - double t0,t1,t2,t3,t4,t5,match_start, match_time, solve_start, solve_time, pca_time, svd_time; + double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time; + match_time = 0; + kdtree_search_time = 0.0; solve_time = 0; - pca_time = 0; + solve_const_H_time = 0; svd_time = 0; t0 = omp_get_wtime(); - p_imu->Process(Measures, state, feats_undistort); - StatesGroup state_propagat(state); + p_imu->Process(Measures, kf, feats_undistort); + state_point = kf.get_x(); + pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; if (feats_undistort->empty() || (feats_undistort == NULL)) { first_lidar_time = Measures.lidar_beg_time; - std::cout<<"not ready for odometry"<first_lidar_time = first_lidar_time; + cout<<"FAST-LIO not ready"<points.size(); /*** initialize the map kdtree ***/ - if((feats_down->points.size() > 1) && (ikdtree.Root_Node == nullptr)) - { - // std::vector points_init = feats_down->points; - ikdtree.set_downsample_param(filter_size_map_min); - ikdtree.Build(feats_down->points); - flg_map_inited = true; - continue; - } - if(ikdtree.Root_Node == nullptr) { - flg_map_inited = false; - std::cout<<"~~~~~~~Initiallize Map iKD-Tree Failed!"< 5) + { + ikdtree.set_downsample_param(filter_size_map_min); + feats_down_world->resize(feats_down_size); + for(int i = 0; i < feats_down_size; i++) + { + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + } + ikdtree.Build(feats_down_world->points); + } continue; } - int featsFromMapNum = ikdtree.size(); - #else - if(featsFromMap->points.empty()) - { - downSizeFilterMap.setInputCloud(feats_down); - } - else - { - downSizeFilterMap.setInputCloud(featsFromMap); - } - downSizeFilterMap.filter(*featsFromMap); - int featsFromMapNum = featsFromMap->points.size(); - #endif - int feats_down_size = feats_down->points.size(); - std::cout<<"[ mapping ]: Raw feature num: "<points.size()<<" downsamp num "<points.size()<<" downsamp "< res_last(feats_down_size, 1000.0); // initial + normvec->resize(feats_down_size); + feats_down_world->resize(feats_down_size); - if (featsFromMapNum >= 5) + V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I); + fout_pre< Nearest_Points(feats_down_size); - #else - kdtreeSurfFromMap->setInputCloud(featsFromMap); - - #endif + PointVector ().swap(ikdtree.PCL_Storage); + ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); + featsFromMap->clear(); + featsFromMap->points = ikdtree.PCL_Storage; + } - std::vector point_selected_surf(feats_down_size, true); - std::vector> pointSearchInd_surf(feats_down_size); - - int rematch_num = 0; - bool rematch_en = 0; - flg_EKF_converged = 0; - deltaR = 0.0; - deltaT = 0.0; - t2 = omp_get_wtime(); - - for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) - { - match_start = omp_get_wtime(); - laserCloudOri->clear(); - coeffSel->clear(); + pointSearchInd_surf.resize(feats_down_size); + Nearest_Points.resize(feats_down_size); + int rematch_num = 0; + bool nearest_search_en = true; // - /** closest surface search and residual computation **/ - omp_set_num_threads(4); - #pragma omp parallel for - for (int i = 0; i < feats_down_size; i++) - { - PointType &pointOri_tmpt = feats_down->points[i]; - PointType &pointSel_tmpt = feats_down_updated->points[i]; - - /* transform to world frame */ - pointBodyToWorld(&pointOri_tmpt, &pointSel_tmpt); - std::vector pointSearchSqDis_surf; - #ifdef USE_ikdtree - auto &points_near = Nearest_Points[i]; - #else - auto &points_near = pointSearchInd_surf[i]; - #endif - - if (iterCount == 0 || rematch_en) - { - point_selected_surf[i] = true; - /** Find the closest surfaces in the map **/ - #ifdef USE_ikdtree - ikdtree.Nearest_Search(pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf); - #else - kdtreeSurfFromMap->nearestKSearch(pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf); - #endif - float max_distance = pointSearchSqDis_surf[NUM_MATCH_POINTS - 1]; - - if (max_distance > 3) - { - point_selected_surf[i] = false; - } - } - - if (point_selected_surf[i] == false) continue; - - // match_time += omp_get_wtime() - match_start; - - double pca_start = omp_get_wtime(); - - /// PCA (using minimum square method) - cv::Mat matA0(NUM_MATCH_POINTS, 3, CV_32F, cv::Scalar::all(0)); - cv::Mat matB0(NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all(-1)); - cv::Mat matX0(NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all(0)); - - for (int j = 0; j < NUM_MATCH_POINTS; j++) - { - #ifdef USE_ikdtree - matA0.at(j, 0) = points_near[j].x; - matA0.at(j, 1) = points_near[j].y; - matA0.at(j, 2) = points_near[j].z; - #else - matA0.at(j, 0) = featsFromMap->points[points_near[j]].x; - matA0.at(j, 1) = featsFromMap->points[points_near[j]].y; - matA0.at(j, 2) = featsFromMap->points[points_near[j]].z; - #endif - } - - //matA0*matX0=matB0 - //AX+BY+CZ+D = 0 <=> AX+BY+CZ=-D <=> (A/D)X+(B/D)Y+(C/D)Z = -1 - //(X,Y,Z)<=>mat_a0 - //A/D, B/D, C/D <=> mat_x0 + t2 = omp_get_wtime(); - cv::solve(matA0, matB0, matX0, cv::DECOMP_QR); //TODO + /*** iterated state estimation ***/ + double t_update_start = omp_get_wtime(); + double solve_H_time = 0; + kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); + state_point = kf.get_x(); + euler_cur = SO3ToEuler(state_point.rot); + pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; + geoQuat.x = state_point.rot.coeffs()[0]; + geoQuat.y = state_point.rot.coeffs()[1]; + geoQuat.z = state_point.rot.coeffs()[2]; + geoQuat.w = state_point.rot.coeffs()[3]; - float pa = matX0.at(0, 0); - float pb = matX0.at(1, 0); - float pc = matX0.at(2, 0); - float pd = 1; + double t_update_end = omp_get_wtime(); - //ps is the norm of the plane norm_vec vector - //pd is the distance from point to plane - float ps = sqrt(pa * pa + pb * pb + pc * pc); - pa /= ps; - pb /= ps; - pc /= ps; - pd /= ps; - - bool planeValid = true; - for (int j = 0; j < NUM_MATCH_POINTS; j++) - { - #ifdef USE_ikdtree - if (fabs(pa * points_near[j].x + - pb * points_near[j].y + - pc * points_near[j].z + pd) > 0.1) - #else - if (fabs(pa * featsFromMap->points[points_near[j]].x + - pb * featsFromMap->points[points_near[j]].y + - pc * featsFromMap->points[points_near[j]].z + pd) > 0.1) - #endif - { - planeValid = false; - point_selected_surf[i] = false; - break; - } - } - - if (planeValid) - { - //loss fuction - float pd2 = pa * pointSel_tmpt.x + pb * pointSel_tmpt.y + pc * pointSel_tmpt.z + pd; - //if(fabs(pd2) > 0.1) continue; - float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel_tmpt.x * pointSel_tmpt.x + pointSel_tmpt.y * pointSel_tmpt.y + pointSel_tmpt.z * pointSel_tmpt.z)); - - if ((s > 0.85))// && ((std::abs(pd2) - res_last[i]) < 3 * res_mean_last)) - { - // if(std::abs(pd2) > 5 * res_mean_last) - // { - // point_selected_surf[i] = false; - // res_last[i] = 0.0; - // continue; - // } - point_selected_surf[i] = true; - coeffSel_tmpt->points[i].x = pa; - coeffSel_tmpt->points[i].y = pb; - coeffSel_tmpt->points[i].z = pc; - coeffSel_tmpt->points[i].intensity = pd2; - - // if(i%50==0) std::cout<<"s: "<points.size(); i++) - { - if (point_selected_surf[i] && (res_last[i] <= 2.0)) - { - laserCloudOri->push_back(feats_down->points[i]); - coeffSel->push_back(coeffSel_tmpt->points[i]); - total_residual += res_last[i]; - laserCloudSelNum ++; - } - } - - res_mean_last = total_residual / laserCloudSelNum; - std::cout << "[ mapping ]: Effective feature num: "<points[i]; - Eigen::Vector3d point_this(laser_p.x, laser_p.y, laser_p.z); - point_this += Lidar_offset_to_IMU; - Eigen::Matrix3d point_crossmat; - point_crossmat<points[i]; - Eigen::Vector3d norm_vec(norm_p.x, norm_p.y, norm_p.z); - - /*** calculate the Measuremnt Jacobian matrix H ***/ - Eigen::Vector3d A(point_crossmat * state.rot_end.transpose() * norm_vec); - Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z; - - /*** Measuremnt: distance to the closest surface/corner ***/ - meas_vec(i) = - norm_p.intensity; - } - - Eigen::Vector3d rot_add, t_add, v_add, bg_add, ba_add, g_add; - Eigen::Matrix solution; - Eigen::MatrixXd K(DIM_OF_STATES, laserCloudSelNum); - - /*** Iterative Kalman Filter Update ***/ - if (!flg_EKF_inited) - { - /*** only run in initialization period ***/ - Eigen::MatrixXd H_init(Eigen::Matrix::Zero()); - Eigen::MatrixXd z_init(Eigen::Matrix::Zero()); - H_init.block<3,3>(0,0) = Eigen::Matrix3d::Identity(); - H_init.block<3,3>(3,3) = Eigen::Matrix3d::Identity(); - H_init.block<3,3>(6,15) = Eigen::Matrix3d::Identity(); - z_init.block<3,1>(0,0) = - Log(state.rot_end); - z_init.block<3,1>(0,0) = - state.pos_end; - - auto H_init_T = H_init.transpose(); - auto &&K_init = state.cov * H_init_T * (H_init * state.cov * H_init_T + 0.0001 * Eigen::Matrix::Identity()).inverse(); - solution = K_init * z_init; - - solution.block<9,1>(0,0).setZero(); - state += solution; - state.cov = (Eigen::MatrixXd::Identity(DIM_OF_STATES, DIM_OF_STATES) - K_init * H_init) * state.cov; - } - else - { - auto &&Hsub_T = Hsub.transpose(); - H_T_H.block<6,6>(0,0) = Hsub_T * Hsub; - Eigen::Matrix &&K_1 = \ - (H_T_H + (state.cov / LASER_POINT_COV).inverse()).inverse(); - K = K_1.block(0,0) * Hsub_T; - - // solution = K * meas_vec; - // state += solution; - - auto vec = state_propagat - state; - // solution = K * (meas_vec - Hsub * vec.block<6,1>(0,0)); - // state = state_propagat + solution; - - solution = K * meas_vec + vec - K * Hsub * vec.block<6,1>(0,0); - state += solution; - - rot_add = solution.block<3,1>(0,0); - t_add = solution.block<3,1>(3,0); - - flg_EKF_converged = false; - - if ((rot_add.norm() * 57.3 < 0.01) \ - && (t_add.norm() * 100 < 0.015)) - { - flg_EKF_converged = true; - } - - deltaR = rot_add.norm() * 57.3; - deltaT = t_add.norm() * 100; - } - - euler_cur = RotMtoEuler(state.rot_end); - - #ifdef DEBUG_PRINT - std::cout<<"update: R"<= 2 || (iterCount == NUM_MAX_ITERATIONS - 1)) - { - if (flg_EKF_inited) - { - /*** Covariance Update ***/ - G.block(0,0) = K * Hsub; - state.cov = (I_STATE - G) * state.cov; - total_distance += (state.pos_end - position_last).norm(); - position_last = state.pos_end; - - std::cout<<"position: "<= 0 && cubeI < laserCloudWidth && - cubeJ >= 0 && cubeJ < laserCloudHeight && - cubeK >= 0 && cubeK < laserCloudDepth) - { - int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; - featsArray[cubeInd]->push_back(pointSel); - - } - } - - // omp_set_num_threads(4); - // #pragma omp parallel for - for (int i = 0; i < feats_down_size; i++) - { - /* transform to world frame */ - pointBodyToWorld(&(feats_down->points[i]), &(feats_down_updated->points[i])); - } - t4 = omp_get_wtime(); - ikdtree.Add_Points(feats_down_updated->points, true); - #else - bool cube_updated[laserCloudNum] = {0}; - for (int i = 0; i < feats_down_size; i++) - { - PointType &pointSel = feats_down_updated->points[i]; - - int cubeI = int((pointSel.x + 0.5 * cube_len) / cube_len) + laserCloudCenWidth; - int cubeJ = int((pointSel.y + 0.5 * cube_len) / cube_len) + laserCloudCenHeight; - int cubeK = int((pointSel.z + 0.5 * cube_len) / cube_len) + laserCloudCenDepth; - - if (pointSel.x + 0.5 * cube_len < 0) cubeI--; - if (pointSel.y + 0.5 * cube_len < 0) cubeJ--; - if (pointSel.z + 0.5 * cube_len < 0) cubeK--; - - if (cubeI >= 0 && cubeI < laserCloudWidth && - cubeJ >= 0 && cubeJ < laserCloudHeight && - cubeK >= 0 && cubeK < laserCloudDepth) { - int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; - featsArray[cubeInd]->push_back(pointSel); - cube_updated[cubeInd] = true; - } - } - for (int i = 0; i < laserCloudValidNum; i++) - { - int ind = laserCloudValidInd[i]; - - if(cube_updated[ind]) - { - downSizeFilterMap.setInputCloud(featsArray[ind]); - downSizeFilterMap.filter(*featsArray[ind]); - } - } - #endif - t5 = omp_get_wtime(); - } - - /******* Publish current frame points in world coordinates: *******/ - laserCloudFullRes2->clear(); - *laserCloudFullRes2 = dense_map_en ? (*feats_undistort) : (* feats_down); - - int laserCloudFullResNum = laserCloudFullRes2->points.size(); - - pcl::PointXYZI temp_point; - laserCloudFullResColor->clear(); - { - for (int i = 0; i < laserCloudFullResNum; i++) - { - RGBpointBodyToWorld(&laserCloudFullRes2->points[i], &temp_point); - laserCloudFullResColor->push_back(temp_point); - } - - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*laserCloudFullResColor, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time::now();//.fromSec(last_timestamp_lidar); - laserCloudFullRes3.header.frame_id = "/camera_init"; - pubLaserCloudFullRes.publish(laserCloudFullRes3); - } - - /******* Publish Effective points *******/ - { - laserCloudFullResColor->clear(); - pcl::PointXYZI temp_point; - for (int i = 0; i < laserCloudSelNum; i++) - { - RGBpointBodyToWorld(&laserCloudOri->points[i], &temp_point); - laserCloudFullResColor->push_back(temp_point); - } - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*laserCloudFullResColor, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time::now();//.fromSec(last_timestamp_lidar); - laserCloudFullRes3.header.frame_id = "/camera_init"; - pubLaserCloudEffect.publish(laserCloudFullRes3); - } - - /******* Publish Maps: *******/ - sensor_msgs::PointCloud2 laserCloudMap; - pcl::toROSMsg(*featsFromMap, laserCloudMap); - laserCloudMap.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar); - laserCloudMap.header.frame_id = "/camera_init"; - pubLaserCloudMap.publish(laserCloudMap); - - /******* Publish Odometry ******/ - geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw - (euler_cur(0), euler_cur(1), euler_cur(2)); - odomAftMapped.header.frame_id = "/camera_init"; - odomAftMapped.child_frame_id = "/aft_mapped"; - odomAftMapped.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar); - odomAftMapped.pose.pose.orientation.x = geoQuat.x; - odomAftMapped.pose.pose.orientation.y = geoQuat.y; - odomAftMapped.pose.pose.orientation.z = geoQuat.z; - odomAftMapped.pose.pose.orientation.w = geoQuat.w; - odomAftMapped.pose.pose.position.x = state.pos_end(0); - odomAftMapped.pose.pose.position.y = state.pos_end(1); - odomAftMapped.pose.pose.position.z = state.pos_end(2); - - 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" ) ); + /******* Publish odometry *******/ + publish_odometry(pubOdomAftMapped); + /*** add the feature points to map kdtree ***/ + t3 = omp_get_wtime(); + map_incremental(); + t5 = omp_get_wtime(); - msg_body_pose.header.stamp = ros::Time::now(); - msg_body_pose.header.frame_id = "/camera_odom_frame"; - msg_body_pose.pose.position.x = state.pos_end(0); - msg_body_pose.pose.position.y = state.pos_end(1); - msg_body_pose.pose.position.z = state.pos_end(2); - msg_body_pose.pose.orientation.x = geoQuat.x; - msg_body_pose.pose.orientation.y = geoQuat.y; - msg_body_pose.pose.orientation.z = geoQuat.z; - msg_body_pose.pose.orientation.w = geoQuat.w; - #ifdef DEPLOY - mavros_pose_publisher.publish(msg_body_pose); - #endif + /******* Publish points *******/ + publish_frame_world(pubLaserCloudFullRes); + publish_path(pubPath); + // publish_effect_world(pubLaserCloudEffect); + publish_map(pubLaserCloudMap); - /******* Publish Path ********/ - msg_body_pose.header.frame_id = "/camera_init"; - path.poses.push_back(msg_body_pose); - pubPath.publish(path); - - /*** save debug variables ***/ - frame_num ++; - aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; - // aver_time_consu = aver_time_consu * 0.8 + (t5 - t0) * 0.2; - T1.push_back(Measures.lidar_beg_time); - s_plot.push_back(aver_time_consu); - s_plot2.push_back(t5 - t3); - s_plot3.push_back(match_time); - s_plot4.push_back(float(feats_down_size/10000.0)); - s_plot5.push_back(t5 - t0); - - // std::cout<<"[ mapping ]: time: copy map "<points.size(); + s_plot3[time_log_counter] = kdtree_incremental_time; + s_plot4[time_log_counter] = kdtree_search_time; + s_plot5[time_log_counter] = kdtree_delete_counter; + s_plot6[time_log_counter] = kdtree_delete_time; + s_plot7[time_log_counter] = kdtree_size_st; + s_plot8[time_log_counter] = kdtree_size_end; + s_plot9[time_log_counter] = aver_time_consu; + s_plot10[time_log_counter] = add_point_size; + 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 construct H: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu,aver_time_icp, aver_time_const_H_time); + ext_euler = SO3ToEuler(state_point.offset_R_L_I); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<points.size()< 0 && corner_points.size() > 0) - // { - // pcl::PCDWriter pcd_writer; - // std::cout << "saving..."; - // pcd_writer.writeBinary(surf_filename, surf_points); - // pcd_writer.writeBinary(corner_filename, corner_points); - // } - - #ifdef DEPLOY - if (!T1.empty()) + PointCloudXYZI surf_points, corner_points; + surf_points = *featsFromMap; + fout_out.close(); + fout_pre.close(); + if (surf_points.size() > 0 && corner_points.size() > 0) { - // plt::named_plot("add new frame",T1,s_plot2); - // plt::named_plot("search and pca",T1,s_plot3); - // plt::named_plot("newpoints number",T1,s_plot4); - plt::named_plot("total time",T1,s_plot5); - plt::named_plot("average time",T1,s_plot); - // plt::named_plot("readd",T2,s_plot6); - plt::legend(); - plt::show(); - plt::pause(0.5); - plt::close(); + pcl::PCDWriter pcd_writer; + cout << "saving..."; + pcd_writer.writeBinary(surf_filename, surf_points); + pcd_writer.writeBinary(corner_filename, corner_points); + } + *******************************************/ + if (runtime_pos_log) + { + vector t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7; + FILE *fp2; + string log_dir = root_dir + "/Log/fast_lio_time_log.csv"; + fp2 = fopen(log_dir.c_str(),"w"); + fprintf(fp2,"time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n"); + for (int i = 0;ipoint_num; + // cout<<"plsie: "<points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)) + { + 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 + + bool is_new = false; + if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) + || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) + || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) + { + pl_buff[msg->points[i].line].push_back(pl_full[i]); + } + } + } + static int count = 0; + static double time = 0.0; + count ++; + double t0 = omp_get_wtime(); + for(int j=0; j &pl = pl_buff[j]; + plsize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(plsize); + plsize--; + for(uint i=0; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)) + { + 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 + + 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 > 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); + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (uint i = 0; i < plsize; i++) + { + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + if (range < blind) continue; + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) + yaw_angle -= 360.0; + if (yaw_angle <= -180.0) + yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t / 1e6; + if(pl_orig.points[i].ring < N_SCANS) + { + pl_buff[pl_orig.points[i].ring].push_back(added_pt); + } + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + 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) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) + yaw_angle -= 360.0; + if (yaw_angle <= -180.0) + yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t / 1e6; + + pl_surf.points.push_back(added_pt); + } + } + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +#define MAX_LINE_NUM 64 + +void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + pl_surf.reserve(plsize); + + bool is_first[MAX_LINE_NUM]; + double yaw_fp[MAX_LINE_NUM]={0}; // yaw of first scan point + double omega_l=3.61; // scan angular velocity + float yaw_last[MAX_LINE_NUM]={0.0}; // yaw of last scan point + float time_last[MAX_LINE_NUM]={0.0}; // last offset time + + if (pl_orig.points[plsize - 1].t > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + memset(is_first, true, sizeof(is_first)); + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + if(feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + int layer = pl_orig.points[i].ring; + if (layer >= N_SCANS) continue; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = pl_orig.points[i].t / 1000.0; // units: ms + + if (!given_offset_time) + { + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer]=yaw_angle; + is_first[layer]=false; + added_pt.curvature = 0.0; + yaw_last[layer]=yaw_angle; + time_last[layer]=added_pt.curvature; + continue; + } + + if (yaw_angle <= yaw_fp[layer]) + { + added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l; + } + else + { + added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l; + } + + if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer]=added_pt.curvature; + } + + pl_buff[layer].points.push_back(added_pt); + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + if (linesize < 2) continue; + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< blind) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, pl_orig.points[i].t); + } + } + } + } + + + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_surf, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +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; + } + // else if(plane_type == 0) + // { + // if(last_state == 1) + // { + // uint i_nex_tem; + // uint j; + // for(j=last_i+1; j<=last_i_nex; j++) + // { + // uint i_nex_tem2 = i_nex_tem; + // Eigen::Vector3d curr_direct2; + + // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); + + // if(ttem != 1) + // { + // i_nex_tem = i_nex_tem2; + // break; + // } + // curr_direct = curr_direct2; + // } + + // if(j == last_i+1) + // { + // last_state = 0; + // } + // else + // { + // for(uint k=last_i_nex; k<=i_nex_tem; k++) + // { + // if(k != i_nex_tem) + // { + // types[k].ftype = Real_Plane; + // } + // else + // { + // types[k].ftype = Poss_Plane; + // } + // } + // i = i_nex_tem-1; + // i_nex = i_nex_tem; + // i2 = j-1; + // last_state = 1; + // } + + // } + // } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for(uint i=head+3; i=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; +} \ No newline at end of file diff --git a/src/preprocess.h b/src/preprocess.h new file mode 100644 index 0000000..1027517 --- /dev/null +++ b/src/preprocess.h @@ -0,0 +1,122 @@ +#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}; //{1, 2, 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; + uint32_t t; + 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) + (uint32_t, t, t) + (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 + int lidar_type, point_filter_num, N_SCANS;; + double blind; + bool feature_enabled, given_offset_time; + ros::Publisher pub_full, pub_surf, pub_corn; + + + private: + void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); + void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void velodyne_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; +}; \ No newline at end of file diff --git a/tools/plot_state.py b/tools/plot_state.py deleted file mode 100644 index e69de29..0000000