fast_lio2 released!
parent
6e1fa94351
commit
1087a72497
|
@ -1,4 +1,7 @@
|
|||
build
|
||||
Log/*.png
|
||||
Log/*.txt
|
||||
Log/time.pdf
|
||||
Log/*.csv
|
||||
Log/*.pdf
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/settings.json
|
||||
|
|
|
@ -0,0 +1,4 @@
|
|||
[submodule "include/ikd-Tree"]
|
||||
path = include/ikd-Tree
|
||||
url = https://github.com/hku-mars/ikd-Tree.git
|
||||
branch = fast_lio
|
|
@ -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})
|
|
@ -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<L)
|
||||
average_time_ikd(i) = mean(total_time_ikd(1:i));
|
||||
else
|
||||
average_time_ikd(i) = mean(total_time_ikd(i-L+1:i));
|
||||
end
|
||||
end
|
||||
for i = 1:length(timestamp_forest)
|
||||
if (i<L)
|
||||
average_time_forest(i) = mean(total_time_forest(1:i));
|
||||
else
|
||||
average_time_forest(i) = mean(total_time_forest(i-L+1:i));
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
f = figure;
|
||||
set(gcf,'Position',[80 433 600 640])
|
||||
tiled_handler = tiledlayout(3,1);
|
||||
tiled_handler.TileSpacing = 'compact';
|
||||
tiled_handler.Padding = 'compact';
|
||||
nexttile;
|
||||
hold on;
|
||||
set(gca,'FontSize',12,'FontName','Times New Roman')
|
||||
plot(timestamp_ikd, average_time_ikd,'-','Color',Color_blue,'LineWidth',1.2);
|
||||
plot(timestamp_forest, average_time_forest,'--','Color',Color_orange,'LineWidth',1.2);
|
||||
lg = legend("ikd-Tree", "ikd-Forest",'location',[0.1314 0.8559 0.2650 0.0789],'fontsize',14,'fontname','Times New Roman')
|
||||
title("Time Performance on FAST-LIO",'FontSize',16,'FontName','Times New Roman')
|
||||
xlabel("time/s",'FontSize',16,'FontName','Times New Roman')
|
||||
yl = ylabel("Run Time/ms",'FontSize',15,'Position',[285.7 5.5000 -1]);
|
||||
xlim([32,390]);
|
||||
ylim([0,23]);
|
||||
ax1 = gca;
|
||||
ax1.YAxis.FontSize = 12;
|
||||
ax1.XAxis.FontSize = 12;
|
||||
grid on
|
||||
box on
|
||||
% print('./Figures/fastlio_exp_average','-depsc','-r600')
|
||||
|
||||
|
||||
index_ikd = find(search_time_ikd > 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')
|
||||
|
|
@ -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.
|
||||
|
|
|
@ -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()
|
60
README.md
60
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)
|
||||
<!--  -->
|
||||
<!-- [](https://youtu.be/2OvjGnxszf8) -->
|
||||
<div align="left">
|
||||
<img src="doc/real_experiment2.gif" width=49% />
|
||||
<img src="doc/ulhkwh_fastlio.gif" width = 49% >
|
||||
</div>
|
||||
|
||||
[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
|
||||
|
||||
<div align="center">
|
||||
<!-- <div align="center">
|
||||
<img src="doc/results/HKU_HW.png" width = 49% >
|
||||
<img src="doc/results/HKU_MB_001.png" width = 49% >
|
||||
</div>
|
||||
</div> -->
|
||||
|
||||
## 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)
|
||||
|
||||
|
|
|
@ -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]
|
|
@ -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]
|
|
@ -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]
|
|
@ -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]
|
Binary file not shown.
After Width: | Height: | Size: 2.0 MiB |
Binary file not shown.
After Width: | Height: | Size: 50 MiB |
Binary file not shown.
After Width: | Height: | Size: 34 MiB |
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,82 @@
|
|||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef __MEKFOM_UTIL_HPP__
|
||||
#define __MEKFOM_UTIL_HPP__
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include "../mtk/src/mtkmath.hpp"
|
||||
namespace esekfom {
|
||||
|
||||
template <typename T1, typename T2>
|
||||
class is_same {
|
||||
public:
|
||||
operator bool() {
|
||||
return false;
|
||||
}
|
||||
};
|
||||
template<typename T1>
|
||||
class is_same<T1, T1> {
|
||||
public:
|
||||
operator bool() {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class is_double {
|
||||
public:
|
||||
operator bool() {
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
template<>
|
||||
class is_double<double> {
|
||||
public:
|
||||
operator bool() {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
static T
|
||||
id(const T &x)
|
||||
{
|
||||
return x;
|
||||
}
|
||||
|
||||
} // namespace esekfom
|
||||
|
||||
#endif // __MEKFOM_UTIL_HPP__
|
|
@ -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 <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/build_manifold.hpp
|
||||
* @brief Macro to automatically construct compound manifolds.
|
||||
*
|
||||
*/
|
||||
#ifndef MTK_AUTOCONSTRUCT_HPP_
|
||||
#define MTK_AUTOCONSTRUCT_HPP_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <boost/preprocessor/seq.hpp>
|
||||
#include <boost/preprocessor/cat.hpp>
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "src/SubManifold.hpp"
|
||||
#include "startIdx.hpp"
|
||||
|
||||
#ifndef PARSED_BY_DOXYGEN
|
||||
//////// internals //////
|
||||
|
||||
#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple
|
||||
|
||||
#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))
|
||||
|
||||
#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)
|
||||
|
||||
#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type()
|
||||
#define MTK_CONSTRUCTOR_COPY( type, id) id(id)
|
||||
#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale);
|
||||
#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale);
|
||||
#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id);
|
||||
#define MTK_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<type, dof, dim> 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<MTK::SO3<double> > Pose;
|
||||
* typedef MTK::vect<double, 3> Vec3;
|
||||
* MTK_BUILD_MANIFOLD(imu_state,
|
||||
* ((Pose, pose))
|
||||
* ((Vec3, vel))
|
||||
* ((Vec3, acc_bias))
|
||||
* )
|
||||
* @endcode
|
||||
* Whitespace is optional, but the double parentheses are necessary.
|
||||
* Construction is done entirely in preprocessor.
|
||||
* After construction @a name is also a manifold. Its members can be
|
||||
* accessed by names given in @a entries.
|
||||
*
|
||||
* @note Variable types are not allowed to have commas, thus types like
|
||||
* @c vect<double, 3> need to be typedef'ed ahead.
|
||||
*/
|
||||
#define MTK_BUILD_MANIFOLD(name, entries) \
|
||||
struct name { \
|
||||
typedef name self; \
|
||||
std::vector<std::pair<int, int> > S2_state;\
|
||||
std::vector<std::pair<int, int> > SO3_state;\
|
||||
std::vector<std::pair<std::pair<int, int>, int> > vect_state;\
|
||||
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<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
|
||||
MTK_TRANSFORM(MTK_BOXPLUS, entries)\
|
||||
} \
|
||||
void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) { \
|
||||
MTK_TRANSFORM(MTK_OPLUS, entries)\
|
||||
} \
|
||||
void boxminus(MTK::vectview<scalar,DOF> __res, const name& __oth) const { \
|
||||
MTK_TRANSFORM(MTK_BOXMINUS, entries)\
|
||||
} \
|
||||
friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \
|
||||
return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \
|
||||
} \
|
||||
void build_S2_state(){\
|
||||
MTK_TRANSFORM(MTK_S2_state, entries)\
|
||||
}\
|
||||
void build_vect_state(){\
|
||||
MTK_TRANSFORM(MTK_vect_state, entries)\
|
||||
}\
|
||||
void build_SO3_state(){\
|
||||
MTK_TRANSFORM(MTK_SO3_state, entries)\
|
||||
}\
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {\
|
||||
MTK_TRANSFORM(MTK_S2_hat, entries)\
|
||||
}\
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) {\
|
||||
MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\
|
||||
}\
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, Eigen::Matrix<scalar, 2, 1> dx, int idx) {\
|
||||
MTK_TRANSFORM(MTK_S2_Mx, entries)\
|
||||
}\
|
||||
friend std::istream& operator>>(std::istream& __is, name& __var){ \
|
||||
return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \
|
||||
} \
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /*MTK_AUTOCONSTRUCT_HPP_*/
|
|
@ -0,0 +1,123 @@
|
|||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/SubManifold.hpp
|
||||
* @brief Defines the SubManifold class
|
||||
*/
|
||||
|
||||
|
||||
#ifndef SUBMANIFOLD_HPP_
|
||||
#define SUBMANIFOLD_HPP_
|
||||
|
||||
|
||||
#include "vectview.hpp"
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* @ingroup SubManifolds
|
||||
* Helper class for compound manifolds.
|
||||
* This class wraps a manifold T and provides an enum IDX refering to the
|
||||
* index of the SubManifold within the compound manifold.
|
||||
*
|
||||
* Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds".
|
||||
*
|
||||
* @tparam T The manifold type of the sub-type
|
||||
* @tparam idx The index of the sub-type within the compound manifold
|
||||
*/
|
||||
template<class T, int idx, int dim>
|
||||
struct SubManifold : public T
|
||||
{
|
||||
enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };
|
||||
//! manifold type
|
||||
typedef T type;
|
||||
|
||||
//! Construct from derived type
|
||||
template<class X>
|
||||
explicit
|
||||
SubManifold(const X& t) : T(t) {};
|
||||
|
||||
//! Construct from internal type
|
||||
//explicit
|
||||
SubManifold(const T& t) : T(t) {};
|
||||
|
||||
//! inherit assignment operator
|
||||
using T::operator=;
|
||||
|
||||
};
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /* SUBMANIFOLD_HPP_ */
|
|
@ -0,0 +1,294 @@
|
|||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/mtkmath.hpp
|
||||
* @brief several math utility functions.
|
||||
*/
|
||||
|
||||
#ifndef MTKMATH_H_
|
||||
#define MTKMATH_H_
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <boost/math/tools/precision.hpp>
|
||||
|
||||
#include "../types/vect.hpp"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.1415926535897932384626433832795
|
||||
#endif
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<class Manifold>
|
||||
struct traits {
|
||||
typedef typename Manifold::scalar scalar;
|
||||
enum {DOF = Manifold::DOF};
|
||||
typedef vect<DOF, scalar> vectorized_type;
|
||||
typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<float> : traits<Scalar<float> > {};
|
||||
template<>
|
||||
struct traits<double> : traits<Scalar<double> > {};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
/**
|
||||
* \defgroup MTKMath Mathematical helper functions
|
||||
*/
|
||||
//@{
|
||||
|
||||
//! constant @f$ \pi @f$
|
||||
const double pi = M_PI;
|
||||
|
||||
template<class scalar> inline scalar tolerance();
|
||||
|
||||
template<> inline float tolerance<float >() { return 1e-5f; }
|
||||
template<> inline double tolerance<double>() { return 1e-11; }
|
||||
|
||||
|
||||
/**
|
||||
* normalize @a x to @f$[-bound, bound] @f$.
|
||||
*
|
||||
* result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
|
||||
*/
|
||||
template<class scalar>
|
||||
inline scalar normalize(scalar x, scalar bound){ //not used
|
||||
if(std::fabs(x) <= bound) return x;
|
||||
int r = (int)(x *(scalar(1.0)/ bound));
|
||||
return x - ((r + (r>>31) + 1) & ~1)*bound;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate cosine and sinc of sqrt(x2).
|
||||
* @param x2 the squared angle must be non-negative
|
||||
* @return a pair containing cos and sinc of sqrt(x2)
|
||||
*/
|
||||
template<class scalar>
|
||||
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
|
||||
using std::sqrt;
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
|
||||
static scalar const taylor_2_bound = sqrt(taylor_0_bound);
|
||||
static scalar const taylor_n_bound = sqrt(taylor_2_bound);
|
||||
|
||||
assert(x2>=0 && "argument must be non-negative");
|
||||
|
||||
// FIXME check if bigger bounds are possible
|
||||
if(x2>=taylor_n_bound) {
|
||||
// slow fall-back solution
|
||||
scalar x = sqrt(x2);
|
||||
return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
|
||||
}
|
||||
|
||||
// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
|
||||
// TODO Find optimal coefficients using Remez algorithm
|
||||
static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
|
||||
scalar cosi = 1., sinc=1;
|
||||
scalar term = -1/2. * x2;
|
||||
for(int i=0; i<3; ++i) {
|
||||
cosi += term;
|
||||
term *= inv[2*i];
|
||||
sinc += term;
|
||||
term *= -inv[2*i+1] * x2;
|
||||
}
|
||||
|
||||
return std::make_pair(cosi, sinc);
|
||||
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
res << 0, -v[2], v[1],
|
||||
v[2], 0, -v[0],
|
||||
-v[1], v[0], 0;
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
if(v.norm() > MTK::tolerance<typename Base::scalar>())
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
if(v.norm() > MTK::tolerance<typename Base::scalar>())
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() - 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename scalar>
|
||||
Eigen::Matrix<scalar, 2, 3> S2_w_expw_( Eigen::Matrix<scalar, 2, 1> v, scalar length)
|
||||
{
|
||||
Eigen::Matrix<scalar, 2, 3> res;
|
||||
scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]);
|
||||
if(norm < MTK::tolerance<scalar>()){
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
res(0, 1) = 1;
|
||||
res(1, 2) = 1;
|
||||
res /= length;
|
||||
}
|
||||
else{
|
||||
res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0,
|
||||
-v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm);
|
||||
res /= length;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
|
||||
double norm = std::sqrt(squaredNorm);
|
||||
if(norm < MTK::tolerance<typename Base::scalar>()){
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
else{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
template<class scalar, int n>
|
||||
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
|
||||
scalar norm2 = vec.squaredNorm();
|
||||
std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
|
||||
scalar mult = cos_sinc.second * scale;
|
||||
result = mult * vec;
|
||||
return cos_sinc.first;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Inverse function to @c exp.
|
||||
*
|
||||
* @param result @c vectview to the result
|
||||
* @param w scalar part of input
|
||||
* @param vec vector part of input
|
||||
* @param scale scale result by this value
|
||||
* @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
|
||||
*/
|
||||
template<class scalar, int n>
|
||||
void log(vectview<scalar, n> result,
|
||||
const scalar &w, const vectview<const scalar, n> vec,
|
||||
const scalar &scale, bool plus_minus_periodicity)
|
||||
{
|
||||
// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
|
||||
scalar nv = vec.norm();
|
||||
if(nv < tolerance<scalar>()) {
|
||||
if(!plus_minus_periodicity && w < 0) {
|
||||
// find the maximal entry:
|
||||
int i;
|
||||
nv = vec.cwiseAbs().maxCoeff(&i);
|
||||
result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
|
||||
return;
|
||||
}
|
||||
nv = tolerance<scalar>();
|
||||
}
|
||||
scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );
|
||||
|
||||
result = s * vec;
|
||||
}
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /* MTKMATH_H_ */
|
|
@ -0,0 +1,168 @@
|
|||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/vectview.hpp
|
||||
* @brief Wrapper class around a pointer used as interface for plain vectors.
|
||||
*/
|
||||
|
||||
#ifndef VECTVIEW_HPP_
|
||||
#define VECTVIEW_HPP_
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* A view to a vector.
|
||||
* Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.
|
||||
* The dimension of the vector is given as template parameter and type-checked when used in expressions.
|
||||
* Data has to be modifiable.
|
||||
*
|
||||
* @tparam scalar Scalar type of the vector.
|
||||
* @tparam dim Dimension of the vector.
|
||||
*
|
||||
* @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct
|
||||
*/
|
||||
namespace internal {
|
||||
template<class Base, class T1, class T2>
|
||||
struct CovBlock {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> ConstType;
|
||||
};
|
||||
|
||||
template<class Base, class T1, class T2>
|
||||
struct CovBlock_ {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> ConstType;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2>
|
||||
struct CrossCovBlock {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> ConstType;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2>
|
||||
struct CrossCovBlock_ {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> ConstType;
|
||||
};
|
||||
|
||||
template<class scalar, int dim>
|
||||
struct VectviewBase {
|
||||
typedef Eigen::Matrix<scalar, dim, 1> matrix_type;
|
||||
typedef typename matrix_type::MapType Type;
|
||||
typedef typename matrix_type::ConstMapType ConstType;
|
||||
};
|
||||
|
||||
template<class T>
|
||||
struct UnalignedType {
|
||||
typedef T type;
|
||||
};
|
||||
}
|
||||
|
||||
template<class scalar, int dim>
|
||||
class vectview : public internal::VectviewBase<scalar, dim>::Type {
|
||||
typedef internal::VectviewBase<scalar, dim> VectviewBase;
|
||||
public:
|
||||
//! plain matrix type
|
||||
typedef typename VectviewBase::matrix_type matrix_type;
|
||||
//! base type
|
||||
typedef typename VectviewBase::Type base;
|
||||
//! construct from pointer
|
||||
explicit
|
||||
vectview(scalar* data, int dim_=dim) : base(data, dim_) {}
|
||||
//! construct from plain matrix
|
||||
vectview(matrix_type& m) : base(m.data(), m.size()) {}
|
||||
//! construct from another @c vectview
|
||||
vectview(const vectview &v) : base(v) {}
|
||||
//! construct from Eigen::Block:
|
||||
template<class Base>
|
||||
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0), block.size()) {}
|
||||
template<class Base, bool PacketAccess>
|
||||
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0), block.size()) {}
|
||||
|
||||
//! inherit assignment operator
|
||||
using base::operator=;
|
||||
//! data pointer
|
||||
scalar* data() {return const_cast<scalar*>(base::data());}
|
||||
};
|
||||
|
||||
/**
|
||||
* @c const version of @c vectview.
|
||||
* Compared to @c Eigen::Map this implementation is const correct, i.e.,
|
||||
* data will not be modifiable using this view.
|
||||
*
|
||||
* @tparam scalar Scalar type of the vector.
|
||||
* @tparam dim Dimension of the vector.
|
||||
*
|
||||
* @sa vectview
|
||||
*/
|
||||
template<class scalar, int dim>
|
||||
class vectview<const scalar, dim> : public internal::VectviewBase<scalar, dim>::ConstType {
|
||||
typedef internal::VectviewBase<scalar, dim> VectviewBase;
|
||||
public:
|
||||
//! plain matrix type
|
||||
typedef typename VectviewBase::matrix_type matrix_type;
|
||||
//! base type
|
||||
typedef typename VectviewBase::ConstType base;
|
||||
//! construct from const pointer
|
||||
explicit
|
||||
vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}
|
||||
//! construct from column vector
|
||||
template<int options>
|
||||
vectview(const Eigen::Matrix<scalar, dim, 1, options>& m) : base(m.data()) {}
|
||||
//! construct from row vector
|
||||
template<int options, int phony>
|
||||
vectview(const Eigen::Matrix<scalar, 1, dim, options, phony>& m) : base(m.data()) {}
|
||||
//! construct from another @c vectview
|
||||
vectview(vectview<scalar, dim> x) : base(x.data()) {}
|
||||
//! construct from base
|
||||
vectview(const base &x) : base(x) {}
|
||||
/**
|
||||
* Construct from Block
|
||||
* @todo adapt this, when Block gets const-correct
|
||||
*/
|
||||
template<class Base>
|
||||
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0)) {}
|
||||
template<class Base, bool PacketAccess>
|
||||
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0)) {}
|
||||
|
||||
};
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /* VECTVIEW_HPP_ */
|
|
@ -0,0 +1,328 @@
|
|||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/startIdx.hpp
|
||||
* @brief Tools to access sub-elements of compound manifolds.
|
||||
*/
|
||||
#ifndef GET_START_INDEX_H_
|
||||
#define GET_START_INDEX_H_
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "src/SubManifold.hpp"
|
||||
#include "src/vectview.hpp"
|
||||
|
||||
namespace MTK {
|
||||
|
||||
|
||||
/**
|
||||
* \defgroup SubManifolds Accessing Submanifolds
|
||||
* For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers
|
||||
* can be used to get sub-vectors or matrix-blocks of a corresponding big matrix.
|
||||
* E.g. for a type @a pose consisting of @a orient and @a trans the member pointers
|
||||
* @c &pose::orient and @c &pose::trans give all required information and are still
|
||||
* valid if the base type gets extended or the actual types of @a orient and @a trans
|
||||
* change (e.g. from 2D to 3D).
|
||||
*
|
||||
* @todo Maybe require manifolds to typedef MatrixType and VectorType, etc.
|
||||
*/
|
||||
//@{
|
||||
|
||||
/**
|
||||
* Determine the index of a sub-variable within a compound variable.
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getStartIdx( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return idx;
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getStartIdx_( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return dim;
|
||||
}
|
||||
|
||||
/**
|
||||
* Determine the degrees of freedom of a sub-variable within a compound variable.
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getDof( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return T::DOF;
|
||||
}
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getDim( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return T::DIM;
|
||||
}
|
||||
|
||||
/**
|
||||
* set the diagonal elements of a covariance matrix corresponding to a sub-variable
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
|
||||
{
|
||||
cov.diagonal().template segment<T::DOF>(idx).setConstant(val);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
void setDiagonal_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
|
||||
{
|
||||
cov.diagonal().template segment<T::DIM>(dim).setConstant(val);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the subblock of corresponding to two members, i.e.
|
||||
* \code
|
||||
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
|
||||
* MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression;
|
||||
* MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans();
|
||||
* \endcode
|
||||
* lets you modify mixed covariance entries in a bigger covariance matrix.
|
||||
*/
|
||||
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
|
||||
typename MTK::internal::CovBlock<Base, T1, T2>::Type
|
||||
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
|
||||
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
|
||||
{
|
||||
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
|
||||
}
|
||||
|
||||
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
|
||||
typename MTK::internal::CovBlock_<Base, T1, T2>::Type
|
||||
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
|
||||
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
|
||||
{
|
||||
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
|
||||
}
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
|
||||
typename MTK::internal::CrossCovBlock<Base1, Base2, T1, T2>::Type
|
||||
subblock(Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
|
||||
{
|
||||
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
|
||||
}
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
|
||||
typename MTK::internal::CrossCovBlock_<Base1, Base2, T1, T2>::Type
|
||||
subblock_(Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
|
||||
{
|
||||
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
|
||||
}
|
||||
/**
|
||||
* Get the subblock of corresponding to a member, i.e.
|
||||
* \code
|
||||
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
|
||||
* MTK::subblock(m, &Pose::orient) = some_expression;
|
||||
* \endcode
|
||||
* lets you modify covariance entries in a bigger covariance matrix.
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
typename MTK::internal::CovBlock_<Base, T, T>::Type
|
||||
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cov.template block<T::DIM, T::DIM>(dim, dim);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
typename MTK::internal::CovBlock<Base, T, T>::Type
|
||||
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cov.template block<T::DOF, T::DOF>(idx, idx);
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
class get_cov {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> type;
|
||||
typedef const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> const_type;
|
||||
};
|
||||
|
||||
template<typename Base>
|
||||
class get_cov_ {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> type;
|
||||
typedef const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> const_type;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2>
|
||||
class get_cross_cov {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> type;
|
||||
typedef const type const_type;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2>
|
||||
class get_cross_cov_ {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> type;
|
||||
typedef const type const_type;
|
||||
};
|
||||
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<typename Base::scalar, T::DIM>
|
||||
subvector_impl_(vectview<typename Base::scalar, Base::DIM> vec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return vec.template segment<T::DIM>(dim);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<typename Base::scalar, T::DOF>
|
||||
subvector_impl(vectview<typename Base::scalar, Base::DOF> vec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return vec.template segment<T::DOF>(idx);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the subvector corresponding to a sub-manifold from a bigger vector.
|
||||
*/
|
||||
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DIM>
|
||||
subvector_(vectview<Scalar, BaseDIM> vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl_(vec, ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DOF>
|
||||
subvector(vectview<Scalar, BaseDOF> vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(vec, ptr);
|
||||
}
|
||||
|
||||
/**
|
||||
* @todo This should be covered already by subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
|
||||
*/
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DOF>
|
||||
subvector(Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(vectview<Scalar, BaseDOF>(vec), ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DIM>
|
||||
subvector_(Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl_(vectview<Scalar, BaseDIM>(vec), ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
|
||||
vectview<const Scalar, T::DIM>
|
||||
subvector_(const Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl_(vectview<const Scalar, BaseDIM>(vec), ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<const Scalar, T::DOF>
|
||||
subvector(const Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(vectview<const Scalar, BaseDOF>(vec), ptr);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* const version of subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<const typename Base::scalar, T::DOF>
|
||||
subvector_impl(const vectview<const typename Base::scalar, Base::DOF> cvec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cvec.template segment<T::DOF>(idx);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<const typename Base::scalar, T::DIM>
|
||||
subvector_impl_(const vectview<const typename Base::scalar, Base::DIM> cvec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cvec.template segment<T::DIM>(dim);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<const Scalar, T::DOF>
|
||||
subvector(const vectview<const Scalar, BaseDOF> cvec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(cvec, ptr);
|
||||
}
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif // GET_START_INDEX_H_
|
|
@ -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 <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/types/S2.hpp
|
||||
* @brief Unit vectors on the sphere, or directions in 3D.
|
||||
*/
|
||||
#ifndef S2_H_
|
||||
#define S2_H_
|
||||
|
||||
|
||||
#include "vect.hpp"
|
||||
|
||||
#include "SOn.hpp"
|
||||
#include "../src/mtkmath.hpp"
|
||||
|
||||
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* Manifold representation of @f$ S^2 @f$.
|
||||
* Used for unit vectors on the sphere or directions in 3D.
|
||||
*
|
||||
* @todo add conversions from/to polar angles?
|
||||
*/
|
||||
template<class _scalar = double, int den = 1, int num = 1, int S2_typ = 3>
|
||||
struct S2 {
|
||||
|
||||
typedef _scalar scalar;
|
||||
typedef vect<3, scalar> vect_type;
|
||||
typedef SO3<scalar> SO3_type;
|
||||
typedef typename vect_type::base vec3;
|
||||
scalar length = scalar(den)/scalar(num);
|
||||
enum {DOF=2, TYP = 1, DIM = 3};
|
||||
|
||||
//private:
|
||||
/**
|
||||
* Unit vector on the sphere, or vector pointing in a direction
|
||||
*/
|
||||
vect_type vec;
|
||||
|
||||
public:
|
||||
S2() {
|
||||
if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1));
|
||||
if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0);
|
||||
if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0);
|
||||
}
|
||||
S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) {
|
||||
vec.normalize();
|
||||
vec = vec * length;
|
||||
}
|
||||
|
||||
S2(const vect_type &_vec) : vec(_vec) {
|
||||
vec.normalize();
|
||||
vec = vec * length;
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, 3> delta, scalar scale = 1)
|
||||
{
|
||||
SO3_type res;
|
||||
res.w() = MTK::exp<scalar, 3>(res.vec(), delta, scalar(scale/2));
|
||||
vec = res.toRotationMatrix() * vec;
|
||||
}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, 2> delta, scalar scale=1) {
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
vect_type Bu = Bx*delta;SO3_type res;
|
||||
res.w() = MTK::exp<scalar, 3>(res.vec(), Bu, scalar(scale/2));
|
||||
vec = res.toRotationMatrix() * vec;
|
||||
}
|
||||
|
||||
void boxminus(MTK::vectview<scalar, 2> res, const S2<scalar, den, num, S2_typ>& other) const {
|
||||
scalar v_sin = (MTK::hat(vec)*other.vec).norm();
|
||||
scalar v_cos = vec.transpose() * other.vec;
|
||||
scalar theta = std::atan2(v_sin, v_cos);
|
||||
if(v_sin < MTK::tolerance<scalar>())
|
||||
{
|
||||
if(std::fabs(theta) > MTK::tolerance<scalar>() )
|
||||
{
|
||||
res[0] = 3.1415926;
|
||||
res[1] = 0;
|
||||
}
|
||||
else{
|
||||
res[0] = 0;
|
||||
res[1] = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
S2<scalar, den, num, S2_typ> other_copy = other;
|
||||
Eigen::Matrix<scalar, 3, 2>Bx;
|
||||
other_copy.S2_Bx(Bx);
|
||||
res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec;
|
||||
}
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 3> skew_vec;
|
||||
skew_vec << scalar(0), -vec[2], vec[1],
|
||||
vec[2], scalar(0), -vec[0],
|
||||
-vec[1], vec[0], scalar(0);
|
||||
res = skew_vec;
|
||||
}
|
||||
|
||||
|
||||
void S2_Bx(Eigen::Matrix<scalar, 3, 2> &res)
|
||||
{
|
||||
if(S2_typ == 3)
|
||||
{
|
||||
if(vec[2] + length > tolerance<scalar>())
|
||||
{
|
||||
|
||||
res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]),
|
||||
-vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]),
|
||||
-vec[0], -vec[1];
|
||||
res /= length;
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
res(1, 1) = -1;
|
||||
res(2, 0) = 1;
|
||||
}
|
||||
}
|
||||
else if(S2_typ == 2)
|
||||
{
|
||||
if(vec[1] + length > tolerance<scalar>())
|
||||
{
|
||||
|
||||
res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]),
|
||||
-vec[0], -vec[2],
|
||||
-vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]);
|
||||
res /= length;
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
res(1, 1) = -1;
|
||||
res(2, 0) = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(vec[0] + length > tolerance<scalar>())
|
||||
{
|
||||
|
||||
res << -vec[1], -vec[2],
|
||||
length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]),
|
||||
-vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]);
|
||||
res /= length;
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
res(1, 1) = -1;
|
||||
res(2, 0) = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void S2_Nx(Eigen::Matrix<scalar, 2, 3> &res, S2<scalar, den, num, S2_typ>& subtrahend)
|
||||
{
|
||||
if((vec+subtrahend.vec).norm() > tolerance<scalar>())
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
if((vec-subtrahend.vec).norm() > tolerance<scalar>())
|
||||
{
|
||||
scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm();
|
||||
scalar v_cos = vec.transpose() * subtrahend.vec;
|
||||
|
||||
res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length));
|
||||
}
|
||||
else
|
||||
{
|
||||
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "No N(x, y) for x=-y" << std::endl;
|
||||
std::exit(100);
|
||||
}
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
if(delta.norm() < tolerance<scalar>())
|
||||
{
|
||||
res = -MTK::hat(vec)*Bx;
|
||||
}
|
||||
else{
|
||||
vect_type Bu = Bx*delta;
|
||||
SO3_type exp_delta;
|
||||
exp_delta.w() = MTK::exp<scalar, 3>(exp_delta.vec(), Bu, scalar(1/2));
|
||||
res = -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<scalar, den, num, S2_typ> operator*(const SO3<scalar>& rot, const S2<scalar, den, num, S2_typ>& dir)
|
||||
{
|
||||
S2<scalar, den, num, S2_typ> ret;
|
||||
ret.vec = rot * dir.vec;
|
||||
return ret;
|
||||
}
|
||||
|
||||
scalar operator[](int idx) const {return vec[idx]; }
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const S2<scalar, den, num, S2_typ>& vec){
|
||||
return os << vec.vec.transpose() << " ";
|
||||
}
|
||||
friend std::istream& operator>>(std::istream &is, S2<scalar, den, num, S2_typ>& vec){
|
||||
for(int i=0; i<3; ++i)
|
||||
is >> vec.vec[i];
|
||||
vec.vec.normalize();
|
||||
vec.vec = vec.vec * vec.length;
|
||||
return is;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /*S2_H_*/
|
|
@ -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 <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/types/SOn.hpp
|
||||
* @brief Standard Orthogonal Groups i.e.\ rotatation groups.
|
||||
*/
|
||||
#ifndef SON_H_
|
||||
#define SON_H_
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include "vect.hpp"
|
||||
#include "../src/mtkmath.hpp"
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
|
||||
/**
|
||||
* Two-dimensional orientations represented as scalar.
|
||||
* There is no guarantee that the representing scalar is within any interval,
|
||||
* but the result of boxminus will always have magnitude @f$\le\pi @f$.
|
||||
*/
|
||||
template<class _scalar = double, int Options = Eigen::AutoAlign>
|
||||
struct SO2 : public Eigen::Rotation2D<_scalar> {
|
||||
enum {DOF = 1, DIM = 2, TYP = 3};
|
||||
|
||||
typedef _scalar scalar;
|
||||
typedef Eigen::Rotation2D<scalar> base;
|
||||
typedef vect<DIM, scalar, Options> vect_type;
|
||||
|
||||
//! Construct from angle
|
||||
SO2(const scalar& angle = 0) : base(angle) { }
|
||||
|
||||
//! Construct from Eigen::Rotation2D
|
||||
SO2(const base& src) : base(src) {}
|
||||
|
||||
/**
|
||||
* Construct from 2D vector.
|
||||
* Resulting orientation will rotate the first unit vector to point to vec.
|
||||
*/
|
||||
SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};
|
||||
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
SO2 operator%(const base &r) const {
|
||||
return base::inverse() * r;
|
||||
}
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
template<class Derived>
|
||||
vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
|
||||
return base::inverse() * vec;
|
||||
}
|
||||
|
||||
//! Calculate @c *this * @c r.inverse()
|
||||
SO2 operator/(const SO2 &r) const {
|
||||
return *this * r.inverse();
|
||||
}
|
||||
|
||||
//! Gets the angle as scalar.
|
||||
operator scalar() const {
|
||||
return base::angle();
|
||||
}
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
//! @name Manifold requirements
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
base::angle() += scale * vec[0];
|
||||
}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
base::angle() += scale * vec[0];
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& other) const {
|
||||
res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));
|
||||
}
|
||||
|
||||
friend std::istream& operator>>(std::istream &is, SO2<scalar>& ang){
|
||||
return is >> ang.angle();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Three-dimensional orientations represented as Quaternion.
|
||||
* It is assumed that the internal Quaternion always stays normalized,
|
||||
* should this not be the case, call inherited member function @c normalize().
|
||||
*/
|
||||
template<class _scalar = double, int Options = Eigen::AutoAlign>
|
||||
struct SO3 : public Eigen::Quaternion<_scalar, Options> {
|
||||
enum {DOF = 3, DIM = 3, TYP = 2};
|
||||
typedef _scalar scalar;
|
||||
typedef Eigen::Quaternion<scalar, Options> base;
|
||||
typedef Eigen::Quaternion<scalar> Quaternion;
|
||||
typedef vect<DIM, scalar, Options> vect_type;
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
template<class OtherDerived> EIGEN_STRONG_INLINE
|
||||
Quaternion operator%(const Eigen::QuaternionBase<OtherDerived> &r) const {
|
||||
return base::conjugate() * r;
|
||||
}
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
template<class Derived>
|
||||
vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
|
||||
return base::conjugate() * vec;
|
||||
}
|
||||
|
||||
//! Calculate @c this * @c r.conjugate()
|
||||
template<class OtherDerived> EIGEN_STRONG_INLINE
|
||||
Quaternion operator/(const Eigen::QuaternionBase<OtherDerived> &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<class Derived>
|
||||
SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
|
||||
|
||||
/**
|
||||
* Construct from arbitrary rotation type.
|
||||
* @note Invalid rotation matrices may lead to spurious behavior.
|
||||
*/
|
||||
template<class Derived>
|
||||
SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}
|
||||
|
||||
//! @name Manifold requirements
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
SO3 delta = exp(vec, scale);
|
||||
*this = *this * delta;
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const SO3<scalar>& other) const {
|
||||
res = SO3::log(other.conjugate() * *this);
|
||||
}
|
||||
//}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
SO3 delta = exp(vec, scale);
|
||||
*this = *this * delta;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const SO3<scalar, Options>& q){
|
||||
return os << q.coeffs().transpose() << " ";
|
||||
}
|
||||
|
||||
friend std::istream& operator>>(std::istream &is, SO3<scalar, Options>& 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<const scalar, 3> dvec, scalar scale = 1){
|
||||
static SO3 exp(const Eigen::Matrix<scalar, 3, 1>& dvec, scalar scale = 1){
|
||||
SO3 res;
|
||||
res.w() = MTK::exp<scalar, 3>(res.vec(), dvec, scalar(scale/2));
|
||||
return res;
|
||||
}
|
||||
/**
|
||||
* Calculate the inverse of @c exp.
|
||||
* Only guarantees that <code>exp(log(x)) == x </code>
|
||||
*/
|
||||
static typename base::Vector3 log(const SO3 &orient){
|
||||
typename base::Vector3 res;
|
||||
MTK::log<scalar, 3>(res, orient.w(), orient.vec(), scalar(2), true);
|
||||
return res;
|
||||
}
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
template<class Scalar, int Options>
|
||||
struct UnalignedType<SO2<Scalar, Options > >{
|
||||
typedef SO2<Scalar, Options | Eigen::DontAlign> type;
|
||||
};
|
||||
|
||||
template<class Scalar, int Options>
|
||||
struct UnalignedType<SO3<Scalar, Options > >{
|
||||
typedef SO3<Scalar, Options | Eigen::DontAlign> type;
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /*SON_H_*/
|
||||
|
|
@ -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 <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/types/vect.hpp
|
||||
* @brief Basic vectors interpreted as manifolds.
|
||||
*
|
||||
* This file also implements a simple wrapper for matrices, for arbitrary scalars
|
||||
* and for positive scalars.
|
||||
*/
|
||||
#ifndef VECT_H_
|
||||
#define VECT_H_
|
||||
|
||||
#include <iosfwd>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
#include "../src/vectview.hpp"
|
||||
|
||||
namespace MTK {
|
||||
|
||||
static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]");
|
||||
|
||||
|
||||
/**
|
||||
* A simple vector class.
|
||||
* Implementation is basically a wrapper around Eigen::Matrix with manifold
|
||||
* requirements added.
|
||||
*/
|
||||
template<int D = 3, class _scalar = double, int _Options=Eigen::AutoAlign>
|
||||
struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> {
|
||||
typedef Eigen::Matrix<_scalar, D, 1, _Options> base;
|
||||
enum {DOF = D, DIM = D, TYP = 0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
//using base::operator=;
|
||||
|
||||
/** Standard constructor. Sets all values to zero. */
|
||||
vect(const base &src = base::Zero()) : base(src) {}
|
||||
|
||||
/** Constructor copying the value of the expression \a other */
|
||||
template<typename OtherDerived>
|
||||
EIGEN_STRONG_INLINE vect(const Eigen::DenseBase<OtherDerived>& other) : base(other) {}
|
||||
|
||||
/** Construct from memory. */
|
||||
vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { }
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
|
||||
*this += scale * vec;
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, D> res, const vect<D, scalar>& other) const {
|
||||
res = *this - other;
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
|
||||
*this += scale * vec;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const vect<D, scalar, _Options>& v){
|
||||
// Eigen sometimes messes with the streams flags, so output manually:
|
||||
for(int i=0; i<DOF; ++i)
|
||||
os << v(i) << " ";
|
||||
return os;
|
||||
}
|
||||
friend std::istream& operator>>(std::istream &is, vect<D, scalar, _Options>& v){
|
||||
char term=0;
|
||||
is >> std::ws; // skip whitespace
|
||||
switch(is.peek()) {
|
||||
case '(': term=')'; is.ignore(1); break;
|
||||
case '[': term=']'; is.ignore(1); break;
|
||||
case '{': term='}'; is.ignore(1); break;
|
||||
default: break;
|
||||
}
|
||||
if(D==Eigen::Dynamic) {
|
||||
assert(term !=0 && "Dynamic vectors must be embraced");
|
||||
std::vector<scalar> temp;
|
||||
while(is.good() && is.peek() != term) {
|
||||
scalar x;
|
||||
is >> x;
|
||||
temp.push_back(x);
|
||||
if(is.peek()==',') is.ignore(1);
|
||||
}
|
||||
v = vect::Map(temp.data(), temp.size());
|
||||
} else
|
||||
for(int i=0; i<v.size(); ++i){
|
||||
is >> v[i];
|
||||
if(is.peek()==',') { // ignore commas between values
|
||||
is.ignore(1);
|
||||
}
|
||||
}
|
||||
if(term!=0) {
|
||||
char x;
|
||||
is >> x;
|
||||
if(x!=term) {
|
||||
is.setstate(is.badbit);
|
||||
// assert(x==term && "start and end bracket do not match!");
|
||||
}
|
||||
}
|
||||
return is;
|
||||
}
|
||||
|
||||
template<int dim>
|
||||
vectview<scalar, dim> tail(){
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template tail<dim>();
|
||||
}
|
||||
template<int dim>
|
||||
vectview<const scalar, dim> tail() const{
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template tail<dim>();
|
||||
}
|
||||
template<int dim>
|
||||
vectview<scalar, dim> head(){
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template head<dim>();
|
||||
}
|
||||
template<int dim>
|
||||
vectview<const scalar, dim> head() const{
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template head<dim>();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* A simple matrix class.
|
||||
* Implementation is basically a wrapper around Eigen::Matrix with manifold
|
||||
* requirements added, i.e., matrix is viewed as a plain vector for that.
|
||||
*/
|
||||
template<int M, int N, class _scalar = double, int _Options = Eigen::Matrix<_scalar, M, N>::Options>
|
||||
struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> {
|
||||
typedef Eigen::Matrix<_scalar, M, N, _Options> base;
|
||||
enum {DOF = M * N, TYP = 4, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
using base::operator=;
|
||||
|
||||
/** Standard constructor. Sets all values to zero. */
|
||||
matrix() {
|
||||
base::setZero();
|
||||
}
|
||||
|
||||
/** Constructor copying the value of the expression \a other */
|
||||
template<typename OtherDerived>
|
||||
EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase<OtherDerived>& other) : base(other) {}
|
||||
|
||||
/** Construct from memory. */
|
||||
matrix(const scalar* src) : base(src) { }
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
*this += scale * base::Map(vec.data());
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const matrix& other) const {
|
||||
base::Map(res.data()) = *this - other;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
*this += scale * base::Map(vec.data());
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const matrix<M, N, scalar, _Options>& mat){
|
||||
for(int i=0; i<DOF; ++i){
|
||||
os << mat.data()[i] << " ";
|
||||
}
|
||||
return os;
|
||||
}
|
||||
friend std::istream& operator>>(std::istream &is, matrix<M, N, scalar, _Options>& mat){
|
||||
for(int i=0; i<DOF; ++i){
|
||||
is >> mat.data()[i];
|
||||
}
|
||||
return is;
|
||||
}
|
||||
};// @todo What if M / N = Eigen::Dynamic?
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* A simple scalar type.
|
||||
*/
|
||||
template<class _scalar = double>
|
||||
struct Scalar {
|
||||
enum {DOF = 1, TYP = 5, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
scalar value;
|
||||
|
||||
Scalar(const scalar& value = scalar(0)) : value(value) {}
|
||||
operator const scalar&() const { return value; }
|
||||
operator scalar&() { return value; }
|
||||
Scalar& operator=(const scalar& val) { value = val; return *this; }
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
value += scale * vec[0];
|
||||
}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
value += scale * vec[0];
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const Scalar& other) const {
|
||||
res[0] = *this - other;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Positive scalars.
|
||||
* Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$.
|
||||
*/
|
||||
template<class _scalar = double>
|
||||
struct PositiveScalar {
|
||||
enum {DOF = 1, TYP = 6, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
scalar value;
|
||||
|
||||
PositiveScalar(const scalar& value = scalar(1)) : value(value) {
|
||||
assert(value > scalar(0));
|
||||
}
|
||||
operator const scalar&() const { return value; }
|
||||
PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; }
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
value *= std::exp(scale * vec[0]);
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const PositiveScalar& other) const {
|
||||
res[0] = std::log(*this / other);
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
value *= std::exp(scale * vec[0]);
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
|
||||
friend std::istream& operator>>(std::istream &is, PositiveScalar<scalar>& s){
|
||||
is >> s.value;
|
||||
assert(s.value > 0);
|
||||
return is;
|
||||
}
|
||||
};
|
||||
|
||||
template<class _scalar = double>
|
||||
struct Complex : public std::complex<_scalar>{
|
||||
enum {DOF = 2, TYP = 7, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
typedef std::complex<scalar> Base;
|
||||
|
||||
Complex(const Base& value) : Base(value) {}
|
||||
Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {}
|
||||
Complex(const MTK::vectview<const scalar, 2> &in) : Base(in[0], in[1]) {}
|
||||
template<class Derived>
|
||||
Complex(const Eigen::DenseBase<Derived> &in) : Base(in[0], in[1]) {}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
Base::real() += scale * vec[0];
|
||||
Base::imag() += scale * vec[1];
|
||||
};
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const Complex& other) const {
|
||||
Complex diff = *this - other;
|
||||
res << diff.real(), diff.imag();
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
Base::real() += scale * vec[0];
|
||||
Base::imag() += scale * vec[1];
|
||||
};
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
scalar squaredNorm() const {
|
||||
return std::pow(Base::real(),2) + std::pow(Base::imag(),2);
|
||||
}
|
||||
|
||||
const scalar& operator()(int i) const {
|
||||
assert(0<=i && i<2 && "Index out of range");
|
||||
return i==0 ? Base::real() : Base::imag();
|
||||
}
|
||||
scalar& operator()(int i){
|
||||
assert(0<=i && i<2 && "Index out of range");
|
||||
return i==0 ? Base::real() : Base::imag();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<int dim, class Scalar, int Options>
|
||||
struct UnalignedType<vect<dim, Scalar, Options > >{
|
||||
typedef vect<dim, Scalar, Options | Eigen::DontAlign> type;
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /*VECT_H_*/
|
|
@ -0,0 +1,113 @@
|
|||
/*
|
||||
* Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Rene Wagner <rene.wagner@dfki.de>
|
||||
* Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the DFKI GmbH
|
||||
* nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific
|
||||
* prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef WRAPPED_CV_MAT_HPP_
|
||||
#define WRAPPED_CV_MAT_HPP_
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <opencv/cv.h>
|
||||
|
||||
namespace MTK {
|
||||
|
||||
template<class f_type>
|
||||
struct cv_f_type;
|
||||
|
||||
template<>
|
||||
struct cv_f_type<double>
|
||||
{
|
||||
enum {value = CV_64F};
|
||||
};
|
||||
|
||||
template<>
|
||||
struct cv_f_type<float>
|
||||
{
|
||||
enum {value = CV_32F};
|
||||
};
|
||||
|
||||
/**
|
||||
* cv_mat wraps a CvMat around an Eigen Matrix
|
||||
*/
|
||||
template<int rows, int cols, class f_type = double>
|
||||
class cv_mat : public matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor>
|
||||
{
|
||||
typedef matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor> base_type;
|
||||
enum {type_ = cv_f_type<f_type>::value};
|
||||
CvMat cv_mat_;
|
||||
|
||||
public:
|
||||
cv_mat()
|
||||
{
|
||||
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
|
||||
}
|
||||
|
||||
cv_mat(const cv_mat& oth) : base_type(oth)
|
||||
{
|
||||
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
|
||||
}
|
||||
|
||||
template<class Derived>
|
||||
cv_mat(const Eigen::MatrixBase<Derived> &value) : base_type(value)
|
||||
{
|
||||
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
|
||||
}
|
||||
|
||||
template<class Derived>
|
||||
cv_mat& operator=(const Eigen::MatrixBase<Derived> &value)
|
||||
{
|
||||
base_type::operator=(value);
|
||||
return *this;
|
||||
}
|
||||
|
||||
cv_mat& operator=(const cv_mat& value)
|
||||
{
|
||||
base_type::operator=(value);
|
||||
return *this;
|
||||
}
|
||||
|
||||
// FIXME: Maybe overloading operator& is not a good idea ...
|
||||
CvMat* operator&()
|
||||
{
|
||||
return &cv_mat_;
|
||||
}
|
||||
const CvMat* operator&() const
|
||||
{
|
||||
return &cv_mat_;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /* WRAPPED_CV_MAT_HPP_ */
|
|
@ -5,66 +5,76 @@
|
|||
#include <Eigen/Eigen>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <fast_lio/States.h>
|
||||
#include <fast_lio/Pose6D.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
|
||||
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<max)?v:max):min)
|
||||
#define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols()
|
||||
#define STD_VEC_FROM_EIGEN(mat) std::vector<decltype(mat)::Scalar> (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<decltype(mat)::Scalar> (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<PointType> PointCloudXYZI;
|
||||
typedef vector<PointType, Eigen::aligned_allocator<PointType>> 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<double, (a), (b)>
|
||||
#define VD(a) Matrix<double, (a), 1>
|
||||
#define MF(a,b) Matrix<float, (a), (b)>
|
||||
#define VF(a) Matrix<float, (a), 1>
|
||||
|
||||
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<sensor_msgs::Imu::ConstPtr> imu;
|
||||
deque<sensor_msgs::Imu::ConstPtr> 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<double,DIM_OF_STATES,DIM_OF_STATES>::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<double, DIM_OF_STATES, 1> &state_add)
|
||||
StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &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<double, DIM_OF_STATES, 1> &state_add)
|
||||
StatesGroup& operator+=(const Matrix<double, DIM_STATE, 1> &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<double, DIM_OF_STATES, 1> operator-(const StatesGroup& b)
|
||||
Matrix<double, DIM_STATE, 1> operator-(const StatesGroup& b)
|
||||
{
|
||||
Eigen::Matrix<double, DIM_OF_STATES, 1> a;
|
||||
Eigen::Matrix3d rotd(b.rot_end.transpose() * this->rot_end);
|
||||
Matrix<double, DIM_STATE, 1> 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<double, DIM_OF_STATES, DIM_OF_STATES> 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<double, DIM_STATE, DIM_STATE> cov; // states covariance
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
|
@ -149,8 +165,8 @@ T deg2rad(T degrees)
|
|||
}
|
||||
|
||||
template<typename T>
|
||||
auto set_pose6d(const double t, const Eigen::Matrix<T, 3, 1> &a, const Eigen::Matrix<T, 3, 1> &g, \
|
||||
const Eigen::Matrix<T, 3, 1> &v, const Eigen::Matrix<T, 3, 1> &p, const Eigen::Matrix<T, 3, 3> &R)
|
||||
auto set_pose6d(const double t, const Matrix<T, 3, 1> &a, const Matrix<T, 3, 1> &g, \
|
||||
const Matrix<T, 3, 1> &v, const Matrix<T, 3, 1> &p, const Matrix<T, 3, 3> &R)
|
||||
{
|
||||
Pose6D rot_kp;
|
||||
rot_kp.offset_time = t;
|
||||
|
@ -162,10 +178,81 @@ auto set_pose6d(const double t, const Eigen::Matrix<T, 3, 1> &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<Eigen::Matrix3d>(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<typename T>
|
||||
bool esti_normvector(Matrix<T, 3, 1> &normvec, const PointVector &point, const T &threshold, const int &point_num)
|
||||
{
|
||||
MatrixXf A(point_num, 3);
|
||||
MatrixXf b(point_num, 1);
|
||||
b.setOnes();
|
||||
b *= -1.0f;
|
||||
|
||||
for (int j = 0; j < point_num; j++)
|
||||
{
|
||||
A(j,0) = point[j].x;
|
||||
A(j,1) = point[j].y;
|
||||
A(j,2) = point[j].z;
|
||||
}
|
||||
normvec = A.colPivHouseholderQr().solve(b);
|
||||
|
||||
for (int j = 0; j < point_num; j++)
|
||||
{
|
||||
if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
#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<typename T>
|
||||
bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
|
||||
{
|
||||
Matrix<T, NUM_MATCH_POINTS, 3> A;
|
||||
Matrix<T, NUM_MATCH_POINTS, 1> b;
|
||||
A.setZero();
|
||||
b.setOnes();
|
||||
b *= -1.0f;
|
||||
|
||||
for (int j = 0; j < NUM_MATCH_POINTS; j++)
|
||||
{
|
||||
A(j,0) = point[j].x;
|
||||
A(j,1) = point[j].y;
|
||||
A(j,2) = point[j].z;
|
||||
}
|
||||
|
||||
Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);
|
||||
|
||||
T n = normvec.norm();
|
||||
pca_result(0) = normvec(0) / n;
|
||||
pca_result(1) = normvec(1) / n;
|
||||
pca_result(2) = normvec(2) / n;
|
||||
pca_result(3) = 1.0 / n;
|
||||
|
||||
for (int j = 0; j < NUM_MATCH_POINTS; j++)
|
||||
{
|
||||
if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 3d115a41377243420a74fc15dd7cf7ef337730df
|
|
@ -3,11 +3,17 @@
|
|||
|
||||
#include <math.h>
|
||||
#include <Eigen/Core>
|
||||
#include <opencv2/core.hpp>
|
||||
// #include <common_lib.h>
|
||||
|
||||
#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 3> skew_sym_mat(const Eigen::Matrix<T, 3, 1> &v)
|
||||
{
|
||||
Eigen::Matrix<T, 3, 3> skew_sym_mat;
|
||||
skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0;
|
||||
return skew_sym_mat;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
|
||||
{
|
||||
|
|
|
@ -0,0 +1,126 @@
|
|||
#ifndef USE_IKFOM_H
|
||||
#define USE_IKFOM_H
|
||||
|
||||
#include <IKFoM_toolkit/esekfom/esekfom.hpp>
|
||||
|
||||
typedef MTK::vect<3, double> vect3;
|
||||
typedef MTK::SO3<double> SO3;
|
||||
typedef MTK::S2<double, 98090, 10000, 1> S2;
|
||||
typedef MTK::vect<1, double> vect1;
|
||||
typedef MTK::vect<2, double> vect2;
|
||||
|
||||
MTK_BUILD_MANIFOLD(state_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<process_noise_ikfom>::type process_noise_cov()
|
||||
{
|
||||
MTK::get_cov<process_noise_ikfom>::type cov = MTK::get_cov<process_noise_ikfom>::type::Zero();
|
||||
MTK::setDiagonal<process_noise_ikfom, vect3, 0>(cov, &process_noise_ikfom::ng, 0.0001);// 0.03
|
||||
MTK::setDiagonal<process_noise_ikfom, vect3, 3>(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05
|
||||
MTK::setDiagonal<process_noise_ikfom, vect3, 6>(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
|
||||
MTK::setDiagonal<process_noise_ikfom, vect3, 9>(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<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
|
||||
{
|
||||
Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
|
||||
vect3 omega;
|
||||
in.gyro.boxminus(omega, s.bg);
|
||||
vect3 a_inertial = s.rot * (in.acc-s.ba);
|
||||
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<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in)
|
||||
{
|
||||
Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::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<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
|
||||
Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
|
||||
s.S2_Mx(grav_matrix, vec, 21);
|
||||
cov.template block<3, 2>(12, 21) = grav_matrix;
|
||||
cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity();
|
||||
return cov;
|
||||
}
|
||||
|
||||
|
||||
Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in)
|
||||
{
|
||||
Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::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<double, 3, 1> _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
|
|
@ -0,0 +1,28 @@
|
|||
<launch>
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="log" />
|
||||
<param name="lidar_type" type="int" value="1"/>
|
||||
<param name="feature_extract_enable" type="bool" value="0"/>
|
||||
<param name="blind" type="double" value="1"/>
|
||||
<param name="point_filter_num" type="int" value="3"/>
|
||||
|
||||
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" required="true" launch-prefix="gdb -ex run --args">
|
||||
<param name="imu_topic" type="string" value="/livox/imu" />
|
||||
<param name="map_file_path" type="string" value=" " />
|
||||
<param name="max_iteration" type="int" value="4" />
|
||||
<param name="dense_map_enable" type="bool" value="1" />
|
||||
<param name="fov_degree" type="double" value="75" />
|
||||
<param name="filter_size_corner" type="double" value="0.2" />
|
||||
<param name="filter_size_surf" type="double" value="0.2" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="runtime_pos_log" type="bool" value="1" />
|
||||
<param name="cube_side_length" type="double" value="2000" />
|
||||
</node>
|
||||
|
||||
<!-- <group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group> -->
|
||||
|
||||
</launch>
|
|
@ -1,41 +1,22 @@
|
|||
<launch>
|
||||
<!-- Launch file for Livox AVIA LiDAR -->
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<param name="lidar_type" type="int" value="1"/>
|
||||
<param name="blind" type="double" value="0.5"/>
|
||||
<param name="inf_bound" type="double" value="10"/>
|
||||
<param name="N_SCANS" type="int" value="6"/>
|
||||
<param name="group_size" type="int" value="8"/>
|
||||
<param name="disA" type="double" value="0.01"/>
|
||||
<param name="disB" type="double" value="0.1"/>
|
||||
<param name="p2l_ratio" type="double" value="225"/>
|
||||
<param name="limit_maxmid" type="double" value="6.25"/>
|
||||
<param name="limit_midmin" type="double" value="6.25"/>
|
||||
<param name="limit_maxmin" type="double" value="3.24"/>
|
||||
<param name="jump_up_limit" type="double" value="170.0"/>
|
||||
<param name="jump_down_limit" type="double" value="8.0"/>
|
||||
<param name="cos160" type="double" value="160.0"/>
|
||||
<param name="edgea" type="double" value="2"/>
|
||||
<param name="edgeb" type="double" value="0.1"/>
|
||||
<param name="smallp_intersect" type="double" value="172.5"/>
|
||||
<param name="smallp_ratio" type="double" value="1.2"/>
|
||||
<rosparam command="load" file="$(find fast_lio)/config/avia.yaml" />
|
||||
|
||||
<param name="feature_extract_enable" type="bool" value="0"/>
|
||||
<param name="point_filter_num" type="int" value="3"/>
|
||||
<node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
|
||||
<param name="max_iteration" type="int" value="3" />
|
||||
<param name="dense_map_enable" type="bool" value="1" />
|
||||
<param name="filter_size_surf" type="double" value="0.5" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="cube_side_length" type="double" value="1000" />
|
||||
<param name="runtime_pos_log" type="bool" value="0" />
|
||||
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
|
||||
|
||||
<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen" required="true">
|
||||
<param name="map_file_path" type="string" value=" " />
|
||||
<param name="max_iteration" type="int" value="10" />
|
||||
<param name="dense_map_enable" type="bool" value="true" />
|
||||
<param name="fov_degree" type="double" value="75" />
|
||||
<param name="filter_size_corner" type="double" value="0.3" />
|
||||
<param name="filter_size_surf" type="double" value="0.2" />
|
||||
<param name="filter_size_map" type="double" value="0.2" />
|
||||
<param name="cube_side_length" type="double" value="10" />
|
||||
</node>
|
||||
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
|
|
@ -1,41 +0,0 @@
|
|||
<launch>
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<param name="lidar_type" type="int" value="1"/>
|
||||
<param name="blind" type="double" value="0.5"/>
|
||||
<param name="inf_bound" type="double" value="10"/>
|
||||
<param name="N_SCANS" type="int" value="6"/>
|
||||
<param name="group_size" type="int" value="8"/>
|
||||
<param name="disA" type="double" value="0.01"/>
|
||||
<param name="disB" type="double" value="0.1"/>
|
||||
<param name="p2l_ratio" type="double" value="225"/>
|
||||
<param name="limit_maxmid" type="double" value="6.25"/>
|
||||
<param name="limit_midmin" type="double" value="6.25"/>
|
||||
<param name="limit_maxmin" type="double" value="3.24"/>
|
||||
<param name="jump_up_limit" type="double" value="170.0"/>
|
||||
<param name="jump_down_limit" type="double" value="8.0"/>
|
||||
<param name="cos160" type="double" value="160.0"/>
|
||||
<param name="edgea" type="double" value="2"/>
|
||||
<param name="edgeb" type="double" value="0.1"/>
|
||||
<param name="smallp_intersect" type="double" value="172.5"/>
|
||||
<param name="smallp_ratio" type="double" value="1.2"/>
|
||||
<param name="point_filter_num" type="int" value="1"/>
|
||||
<node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>
|
||||
|
||||
<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
|
||||
<param name="map_file_path" type="string" value=" " />
|
||||
<param name="max_iteration" type="int" value="85" />
|
||||
<param name="dense_map_enable" type="bool" value="true" />
|
||||
<param name="fov_degree" type="double" value="70" />
|
||||
<param name="filter_size_corner" type="double" value="0.5" />
|
||||
<param name="filter_size_surf" type="double" value="0.5" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="cube_side_length" type="double" value="100" />
|
||||
</node>
|
||||
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,22 @@
|
|||
<launch>
|
||||
<!-- Launch file for Livox Horizon LiDAR -->
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<rosparam command="load" file="$(find fast_lio)/config/horizon.yaml" />
|
||||
|
||||
<param name="feature_extract_enable" type="bool" value="0"/>
|
||||
<param name="point_filter_num" type="int" value="3"/>
|
||||
<param name="max_iteration" type="int" value="3" />
|
||||
<param name="dense_map_enable" type="bool" value="0" />
|
||||
<param name="filter_size_surf" type="double" value="0.5" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="cube_side_length" type="double" value="1000" />
|
||||
<param name="runtime_pos_log" type="bool" value="0" />
|
||||
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
|
||||
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,22 @@
|
|||
<launch>
|
||||
<!-- Launch file for ouster OS2-64 LiDAR -->
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<rosparam command="load" file="$(find fast_lio)/config/ouster64.yaml" />
|
||||
|
||||
<param name="feature_extract_enable" type="bool" value="1"/>
|
||||
<param name="point_filter_num" type="int" value="3"/>
|
||||
<param name="max_iteration" type="int" value="4" />
|
||||
<param name="dense_map_enable" type="bool" value="1" />
|
||||
<param name="filter_size_surf" type="double" value="0.5" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="cube_side_length" type="double" value="2000" />
|
||||
<param name="runtime_pos_log_enable" type="bool" value="0" />
|
||||
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
|
||||
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,22 @@
|
|||
<launch>
|
||||
<!-- Launch file for velodyne16 VLP-16 LiDAR -->
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<rosparam command="load" file="$(find fast_lio)/config/velody16.yaml" />
|
||||
|
||||
<param name="feature_extract_enable" type="bool" value="0"/>
|
||||
<param name="point_filter_num" type="int" value="4"/>
|
||||
<param name="max_iteration" type="int" value="3" />
|
||||
<param name="dense_map_enable" type="bool" value="1" />
|
||||
<param name="filter_size_surf" type="double" value="0.5" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="cube_side_length" type="double" value="1000" />
|
||||
<param name="runtime_pos_log" type="bool" value="0" />
|
||||
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
|
||||
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -1,26 +0,0 @@
|
|||
<launch>
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<!-- <node pkg="fast_lio" type="imu_process" name="imu_process" output="screen"/>
|
||||
|
||||
|
||||
<node pkg="fast_lio" type="loam_scanRegistration_horizon" name="scanRegistration_horizon" output="screen">
|
||||
</node>
|
||||
|
||||
|
||||
<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
|
||||
<param name="map_file_path" type="string" value=" " />
|
||||
<param name="filter_parameter_corner" type="double" value="0.3" />
|
||||
<param name="filter_parameter_surf" type="double" value="0.2" />
|
||||
</node>
|
||||
|
||||
<node pkg="fast_lio" type="livox_repub" name="livox_repub" output="screen" >
|
||||
<remap from="/livox/lidar" to="/livox/lidar" />
|
||||
</node> -->
|
||||
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -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
|
|
@ -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: <Fixed Frame>
|
||||
Value: false
|
||||
- Class: rviz/Axes
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Length: 0.699999988079071
|
||||
Name: Axes
|
||||
Radius: 0.05999999865889549
|
||||
Reference Frame: <Fixed 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: <Fixed 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: <Fixed 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
|
||||
|
|
|
@ -21,12 +21,12 @@
|
|||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <fast_lio/States.h>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
#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<sensor_msgs::Imu::ConstPtr> &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<double, 12, 12> Q;
|
||||
void Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);
|
||||
|
||||
double scale_gravity;
|
||||
|
||||
Eigen::Vector3d angvel_last;
|
||||
Eigen::Vector3d acc_s_last;
|
||||
|
||||
Eigen::Matrix<double,DIM_OF_PROC_N,1> 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<state_ikfom, 12, input_ikfom> &kf_state, int &N);
|
||||
void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &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<sensor_msgs::ImuConstPtr> v_imu_;
|
||||
vector<Pose6D> IMUpose;
|
||||
vector<M3D> 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<sensor_msgs::ImuConstPtr> v_imu_;
|
||||
std::vector<Eigen::Matrix3d> v_rot_pcl_;
|
||||
std::vector<Pose6D> 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<double,DIM_OF_PROC_N,1>::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<double,DIM_OF_PROC_N,1>::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<state_ikfom, 12, input_ikfom> &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: "<<mean_acc.norm()<<endl;
|
||||
}
|
||||
|
||||
for (const auto &imu : meas.imu)
|
||||
|
@ -157,22 +181,37 @@ void ImuProcess::IMU_Initial(const MeasureGroup &meas, StatesGroup &state_inout,
|
|||
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
|
||||
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
|
||||
|
||||
scale_gravity += (cur_acc.norm() - scale_gravity) / N;
|
||||
mean_acc += (cur_acc - mean_acc) / N;
|
||||
mean_gyr += (cur_gyr - mean_gyr) / N;
|
||||
|
||||
cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
|
||||
cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);
|
||||
|
||||
// cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
|
||||
|
||||
N ++;
|
||||
}
|
||||
state_ikfom init_state = kf_state.get_x();
|
||||
init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);
|
||||
|
||||
//state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
|
||||
init_state.bg = mean_gyr;
|
||||
init_state.offset_T_L_I = Lidar_T_wrt_IMU;
|
||||
init_state.offset_R_L_I = Lidar_R_wrt_IMU;
|
||||
kf_state.change_x(init_state);
|
||||
|
||||
state_inout.gravity = - mean_acc /scale_gravity * G_m_s2;
|
||||
state_inout.rot_end = Eye3d; // Exp(mean_acc.cross(Eigen::Vector3d(0, 0, -1 / scale_gravity)));
|
||||
state_inout.bias_g = mean_gyr;
|
||||
esekfom::esekf<state_ikfom, 12, input_ikfom>::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<state_ikfom, 12, input_ikfom> &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 "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
|
||||
<<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<std::endl;
|
||||
// cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
|
||||
// <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;
|
||||
|
||||
/*** Initialize IMU pose ***/
|
||||
state_ikfom imu_state = kf_state.get_x();
|
||||
IMUpose.clear();
|
||||
// IMUpose.push_back(set_pose6d(0.0, Zero3d, Zero3d, state.vel_end, state.pos_end, state.rot_end));
|
||||
IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end));
|
||||
IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
|
||||
|
||||
/*** forward propagation at each imu point ***/
|
||||
Eigen::Vector3d acc_imu, angvel_avr, acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end);
|
||||
Eigen::Matrix3d R_imu(state_inout.rot_end);
|
||||
Eigen::MatrixXd F_x(Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES>::Identity());
|
||||
Eigen::MatrixXd cov_w(Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES>::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<<head->header.stamp.toSec()<<" "<<angvel_avr.transpose()<<" "<<acc_avr.transpose()<<std::endl;
|
||||
#endif
|
||||
dt = tail->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<<SKEW_SYM_MATRX(angvel_avr);
|
||||
|
||||
F_x.block<3,3>(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 "<<acc_imu.transpose()<<"vel "<<acc_imu.transpose()<<"vel "<<pos_imu.transpose()<<std::endl;
|
||||
IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu));
|
||||
IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
|
||||
}
|
||||
|
||||
/*** calculated the pos and attitude prediction at the frame-end ***/
|
||||
dt = pcl_end_time - imu_end_time;
|
||||
state_inout.vel_end = vel_imu + acc_imu * dt;
|
||||
state_inout.rot_end = R_imu * Exp(angvel_avr, dt);
|
||||
state_inout.pos_end = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt;
|
||||
|
||||
auto pos_liD_e = state_inout.pos_end + state_inout.rot_end * Lidar_offset_to_IMU;
|
||||
// auto R_liD_e = state_inout.rot_end * Lidar_R_to_IMU;
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
std::cout<<"[ IMU Process ]: vel "<<state_inout.vel_end.transpose()<<" pos "<<state_inout.pos_end.transpose()<<" ba"<<state_inout.bias_a.transpose()<<" bg "<<state_inout.bias_g.transpose()<<std::endl;
|
||||
std::cout<<"propagated cov: "<<state_inout.cov.diagonal().transpose()<<std::endl;
|
||||
#endif
|
||||
double note = pcl_end_time > 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<<MAT_FROM_ARRAY(head->rot);
|
||||
acc_imu<<VEC_FROM_ARRAY(head->acc);
|
||||
// std::cout<<"head imu acc: "<<acc_imu.transpose()<<std::endl;
|
||||
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
|
||||
vel_imu<<VEC_FROM_ARRAY(head->vel);
|
||||
pos_imu<<VEC_FROM_ARRAY(head->pos);
|
||||
angvel_avr<<VEC_FROM_ARRAY(head->gyr);
|
||||
acc_imu<<VEC_FROM_ARRAY(tail->acc);
|
||||
angvel_avr<<VEC_FROM_ARRAY(tail->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<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
|
||||
{
|
||||
double t1,t2,t3;
|
||||
t1 = omp_get_wtime();
|
||||
|
||||
if(meas.imu.empty()) {std::cout<<"no imu data"<<std::endl;return;};
|
||||
if(meas.imu.empty()) {return;};
|
||||
ROS_ASSERT(meas.lidar != nullptr);
|
||||
|
||||
if (imu_need_init_)
|
||||
{
|
||||
/// The very first lidar frame
|
||||
IMU_Initial(meas, stat, init_iter_num);
|
||||
IMU_init(meas, kf_state, init_iter_num);
|
||||
|
||||
imu_need_init_ = true;
|
||||
|
||||
last_imu_ = meas.imu.back();
|
||||
|
||||
state_ikfom imu_state = kf_state.get_x();
|
||||
if (init_iter_num > MAX_INI_COUNT)
|
||||
{
|
||||
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
|
||||
imu_need_init_ = false;
|
||||
// std::cout<<"mean acc: "<<mean_acc<<" acc measures in word frame:"<<state.rot_end.transpose()*mean_acc<<std::endl;
|
||||
ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
|
||||
stat.gravity[0], stat.gravity[1], stat.gravity[2], stat.bias_g[0], stat.bias_g[1], stat.bias_g[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
|
||||
// ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
|
||||
// imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_acc_scale[0], cov_acc_scale[1], cov_acc_scale[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
|
||||
// cov_acc = cov_acc.cwiseProduct(cov_acc_scale);
|
||||
// cov_gyr = cov_gyr.cwiseProduct(cov_gyr_scale);
|
||||
|
||||
cov_acc = cov_acc_scale;
|
||||
cov_gyr = cov_gyr_scale;
|
||||
// cout<<"mean acc: "<<mean_acc<<" acc measures in word frame:"<<state.rot_end.transpose()*mean_acc<<endl;
|
||||
ROS_INFO("IMU Initials: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
|
||||
imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
|
||||
fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/// Undistort points: the first point is assummed as the base frame
|
||||
/// Compensate lidar points with IMU rotation (with only rotation now)
|
||||
UndistortPcl(meas, stat, *cur_pcl_un_);
|
||||
UndistortPcl(meas, kf_state, *cur_pcl_un_);
|
||||
|
||||
t2 = omp_get_wtime();
|
||||
|
||||
// {
|
||||
// static ros::Publisher pub_UndistortPcl =
|
||||
// nh.advertise<sensor_msgs::PointCloud2>("/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: "<<t3 - t1<<std::endl;
|
||||
}
|
||||
// cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
|
||||
}
|
||||
|
|
File diff suppressed because it is too large
Load Diff
1734
src/laserMapping.cpp
1734
src/laserMapping.cpp
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,920 @@
|
|||
#include "preprocess.h"
|
||||
|
||||
#define RETURN0 0x00
|
||||
#define RETURN0AND1 0x10
|
||||
|
||||
Preprocess::Preprocess()
|
||||
:feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
|
||||
{
|
||||
inf_bound = 10;
|
||||
N_SCANS = 6;
|
||||
group_size = 8;
|
||||
disA = 0.01;
|
||||
disA = 0.1; // B?
|
||||
p2l_ratio = 225;
|
||||
limit_maxmid =6.25;
|
||||
limit_midmin =6.25;
|
||||
limit_maxmin = 3.24;
|
||||
jump_up_limit = 170.0;
|
||||
jump_down_limit = 8.0;
|
||||
cos160 = 160.0;
|
||||
edgea = 2;
|
||||
edgeb = 0.1;
|
||||
smallp_intersect = 172.5;
|
||||
smallp_ratio = 1.2;
|
||||
given_offset_time = false;
|
||||
|
||||
jump_up_limit = cos(jump_up_limit/180*M_PI);
|
||||
jump_down_limit = cos(jump_down_limit/180*M_PI);
|
||||
cos160 = cos(cos160/180*M_PI);
|
||||
smallp_intersect = cos(smallp_intersect/180*M_PI);
|
||||
}
|
||||
|
||||
Preprocess::~Preprocess() {}
|
||||
|
||||
void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
|
||||
{
|
||||
feature_enabled = feat_en;
|
||||
lidar_type = lid_type;
|
||||
blind = bld;
|
||||
point_filter_num = pfilt_num;
|
||||
}
|
||||
|
||||
void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
|
||||
{
|
||||
avia_handler(msg);
|
||||
*pcl_out = pl_surf;
|
||||
}
|
||||
|
||||
void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
|
||||
{
|
||||
switch (lidar_type)
|
||||
{
|
||||
case OUST64:
|
||||
oust64_handler(msg);
|
||||
break;
|
||||
|
||||
case VELO16:
|
||||
velodyne_handler(msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("Error LiDAR Type");
|
||||
break;
|
||||
}
|
||||
*pcl_out = pl_surf;
|
||||
}
|
||||
|
||||
void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
|
||||
{
|
||||
pl_surf.clear();
|
||||
pl_corn.clear();
|
||||
pl_full.clear();
|
||||
double t1 = omp_get_wtime();
|
||||
int plsize = msg->point_num;
|
||||
// cout<<"plsie: "<<plsize<<endl;
|
||||
|
||||
pl_corn.reserve(plsize);
|
||||
pl_surf.reserve(plsize);
|
||||
pl_full.resize(plsize);
|
||||
|
||||
for(int i=0; i<N_SCANS; i++)
|
||||
{
|
||||
pl_buff[i].clear();
|
||||
pl_buff[i].reserve(plsize);
|
||||
}
|
||||
uint valid_num = 0;
|
||||
|
||||
if (feature_enabled)
|
||||
{
|
||||
for(uint i=1; i<plsize; i++)
|
||||
{
|
||||
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10))
|
||||
{
|
||||
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<N_SCANS; j++)
|
||||
{
|
||||
if(pl_buff[j].size() <= 5) continue;
|
||||
pcl::PointCloud<PointType> &pl = pl_buff[j];
|
||||
plsize = pl.size();
|
||||
vector<orgtype> &types = typess[j];
|
||||
types.clear();
|
||||
types.resize(plsize);
|
||||
plsize--;
|
||||
for(uint i=0; i<plsize; i++)
|
||||
{
|
||||
types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y;
|
||||
vx = pl[i].x - pl[i + 1].x;
|
||||
vy = pl[i].y - pl[i + 1].y;
|
||||
vz = pl[i].z - pl[i + 1].z;
|
||||
types[i].dista = vx * vx + vy * vy + vz * vz;
|
||||
}
|
||||
types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
|
||||
give_feature(pl, types);
|
||||
// pl_surf += pl;
|
||||
}
|
||||
time += omp_get_wtime() - t0;
|
||||
printf("Feature extraction time: %lf \n", time / count);
|
||||
}
|
||||
else
|
||||
{
|
||||
for(uint i=1; i<plsize; i++)
|
||||
{
|
||||
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10))
|
||||
{
|
||||
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<ouster_ros::Point> 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<orgtype> &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<velodyne_ros::Point> 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<orgtype> &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<<"!!!!!!"<<i<<" "<<plsize<<endl;
|
||||
|
||||
added_pt.normal_x = 0;
|
||||
added_pt.normal_y = 0;
|
||||
added_pt.normal_z = 0;
|
||||
added_pt.x = pl_orig.points[i].x;
|
||||
added_pt.y = pl_orig.points[i].y;
|
||||
added_pt.z = pl_orig.points[i].z;
|
||||
added_pt.intensity = pl_orig.points[i].intensity;
|
||||
added_pt.curvature = pl_orig.points[i].t / 1000.0;
|
||||
|
||||
if (!given_offset_time)
|
||||
{
|
||||
int layer = pl_orig.points[i].ring;
|
||||
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
|
||||
|
||||
if (is_first[layer])
|
||||
{
|
||||
// printf("layer: %d; is first: %d", layer, is_first[layer]);
|
||||
yaw_fp[layer]=yaw_angle;
|
||||
is_first[layer]=false;
|
||||
added_pt.curvature = 0.0;
|
||||
yaw_last[layer]=yaw_angle;
|
||||
time_last[layer]=added_pt.curvature;
|
||||
continue;
|
||||
}
|
||||
|
||||
// compute offset time
|
||||
if (yaw_angle <= yaw_fp[layer])
|
||||
{
|
||||
added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
|
||||
}
|
||||
else
|
||||
{
|
||||
added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l;
|
||||
}
|
||||
|
||||
if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l;
|
||||
|
||||
// added_pt.curvature = pl_orig.points[i].t;
|
||||
|
||||
yaw_last[layer] = yaw_angle;
|
||||
time_last[layer]=added_pt.curvature;
|
||||
}
|
||||
|
||||
// if(i==(plsize-1)) printf("index: %d layer: %d, yaw: %lf, offset-time: %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature, prints);
|
||||
if (i % point_filter_num == 0)
|
||||
{
|
||||
if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > blind)
|
||||
{
|
||||
pl_surf.points.push_back(added_pt);
|
||||
// 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<PointType> &pl, vector<orgtype> &types)
|
||||
{
|
||||
int plsize = pl.size();
|
||||
int plsize2;
|
||||
if(plsize == 0)
|
||||
{
|
||||
printf("something wrong\n");
|
||||
return;
|
||||
}
|
||||
uint head = 0;
|
||||
|
||||
while(types[head].range < blind)
|
||||
{
|
||||
head++;
|
||||
}
|
||||
|
||||
// Surf
|
||||
plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
|
||||
|
||||
Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
|
||||
Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
|
||||
|
||||
uint i_nex = 0, i2;
|
||||
uint last_i = 0; uint last_i_nex = 0;
|
||||
int last_state = 0;
|
||||
int plane_type;
|
||||
|
||||
for(uint i=head; i<plsize2; i++)
|
||||
{
|
||||
if(types[i].range < blind)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
i2 = i;
|
||||
|
||||
plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
|
||||
|
||||
if(plane_type == 1)
|
||||
{
|
||||
for(uint j=i; j<=i_nex; j++)
|
||||
{
|
||||
if(j!=i && j!=i_nex)
|
||||
{
|
||||
types[j].ftype = Real_Plane;
|
||||
}
|
||||
else
|
||||
{
|
||||
types[j].ftype = Poss_Plane;
|
||||
}
|
||||
}
|
||||
|
||||
// if(last_state==1 && fabs(last_direct.sum())>0.5)
|
||||
if(last_state==1 && last_direct.norm()>0.1)
|
||||
{
|
||||
double mod = last_direct.transpose() * curr_direct;
|
||||
if(mod>-0.707 && mod<0.707)
|
||||
{
|
||||
types[i].ftype = Edge_Plane;
|
||||
}
|
||||
else
|
||||
{
|
||||
types[i].ftype = Real_Plane;
|
||||
}
|
||||
}
|
||||
|
||||
i = i_nex - 1;
|
||||
last_state = 1;
|
||||
}
|
||||
else // if(plane_type == 2)
|
||||
{
|
||||
i = i_nex;
|
||||
last_state = 0;
|
||||
}
|
||||
// else if(plane_type == 0)
|
||||
// {
|
||||
// if(last_state == 1)
|
||||
// {
|
||||
// uint i_nex_tem;
|
||||
// uint j;
|
||||
// for(j=last_i+1; j<=last_i_nex; j++)
|
||||
// {
|
||||
// uint i_nex_tem2 = i_nex_tem;
|
||||
// Eigen::Vector3d curr_direct2;
|
||||
|
||||
// uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
|
||||
|
||||
// if(ttem != 1)
|
||||
// {
|
||||
// i_nex_tem = i_nex_tem2;
|
||||
// break;
|
||||
// }
|
||||
// curr_direct = curr_direct2;
|
||||
// }
|
||||
|
||||
// if(j == last_i+1)
|
||||
// {
|
||||
// last_state = 0;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// for(uint k=last_i_nex; k<=i_nex_tem; k++)
|
||||
// {
|
||||
// if(k != i_nex_tem)
|
||||
// {
|
||||
// types[k].ftype = Real_Plane;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// types[k].ftype = Poss_Plane;
|
||||
// }
|
||||
// }
|
||||
// i = i_nex_tem-1;
|
||||
// i_nex = i_nex_tem;
|
||||
// i2 = j-1;
|
||||
// last_state = 1;
|
||||
// }
|
||||
|
||||
// }
|
||||
// }
|
||||
|
||||
last_i = i2;
|
||||
last_i_nex = i_nex;
|
||||
last_direct = curr_direct;
|
||||
}
|
||||
|
||||
plsize2 = plsize > 3 ? plsize - 3 : 0;
|
||||
for(uint i=head+3; i<plsize2; i++)
|
||||
{
|
||||
if(types[i].range<blind || types[i].ftype>=Real_Plane)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if(types[i-1].dista<1e-16 || types[i].dista<1e-16)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
|
||||
Eigen::Vector3d vecs[2];
|
||||
|
||||
for(int j=0; j<2; j++)
|
||||
{
|
||||
int m = -1;
|
||||
if(j == 1)
|
||||
{
|
||||
m = 1;
|
||||
}
|
||||
|
||||
if(types[i+m].range < blind)
|
||||
{
|
||||
if(types[i].range > inf_bound)
|
||||
{
|
||||
types[i].edj[j] = Nr_inf;
|
||||
}
|
||||
else
|
||||
{
|
||||
types[i].edj[j] = Nr_blind;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z);
|
||||
vecs[j] = vecs[j] - vec_a;
|
||||
|
||||
types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
|
||||
if(types[i].angle[j] < jump_up_limit)
|
||||
{
|
||||
types[i].edj[j] = Nr_180;
|
||||
}
|
||||
else if(types[i].angle[j] > jump_down_limit)
|
||||
{
|
||||
types[i].edj[j] = Nr_zero;
|
||||
}
|
||||
}
|
||||
|
||||
types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
|
||||
if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista)
|
||||
{
|
||||
if(types[i].intersect > cos160)
|
||||
{
|
||||
if(edge_jump_judge(pl, types, i, Prev))
|
||||
{
|
||||
types[i].ftype = Edge_Jump;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista)
|
||||
{
|
||||
if(types[i].intersect > cos160)
|
||||
{
|
||||
if(edge_jump_judge(pl, types, i, Next))
|
||||
{
|
||||
types[i].ftype = Edge_Jump;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf)
|
||||
{
|
||||
if(edge_jump_judge(pl, types, i, Prev))
|
||||
{
|
||||
types[i].ftype = Edge_Jump;
|
||||
}
|
||||
}
|
||||
else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor)
|
||||
{
|
||||
if(edge_jump_judge(pl, types, i, Next))
|
||||
{
|
||||
types[i].ftype = Edge_Jump;
|
||||
}
|
||||
|
||||
}
|
||||
else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor)
|
||||
{
|
||||
if(types[i].ftype == Nor)
|
||||
{
|
||||
types[i].ftype = Wire;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
plsize2 = plsize-1;
|
||||
double ratio;
|
||||
for(uint i=head+1; i<plsize2; i++)
|
||||
{
|
||||
if(types[i].range<blind || types[i-1].range<blind || types[i+1].range<blind)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if(types[i-1].dista<1e-8 || types[i].dista<1e-8)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if(types[i].ftype == Nor)
|
||||
{
|
||||
if(types[i-1].dista > types[i].dista)
|
||||
{
|
||||
ratio = types[i-1].dista / types[i].dista;
|
||||
}
|
||||
else
|
||||
{
|
||||
ratio = types[i].dista / types[i-1].dista;
|
||||
}
|
||||
|
||||
if(types[i].intersect<smallp_intersect && ratio < smallp_ratio)
|
||||
{
|
||||
if(types[i-1].ftype == Nor)
|
||||
{
|
||||
types[i-1].ftype = Real_Plane;
|
||||
}
|
||||
if(types[i+1].ftype == Nor)
|
||||
{
|
||||
types[i+1].ftype = Real_Plane;
|
||||
}
|
||||
types[i].ftype = Real_Plane;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int last_surface = -1;
|
||||
for(uint j=head; j<plsize; j++)
|
||||
{
|
||||
if(types[j].ftype==Poss_Plane || types[j].ftype==Real_Plane)
|
||||
{
|
||||
if(last_surface == -1)
|
||||
{
|
||||
last_surface = j;
|
||||
}
|
||||
|
||||
if(j == uint(last_surface+point_filter_num-1))
|
||||
{
|
||||
PointType ap;
|
||||
ap.x = pl[j].x;
|
||||
ap.y = pl[j].y;
|
||||
ap.z = pl[j].z;
|
||||
ap.curvature = pl[j].curvature;
|
||||
pl_surf.push_back(ap);
|
||||
|
||||
last_surface = -1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(types[j].ftype==Edge_Jump || types[j].ftype==Edge_Plane)
|
||||
{
|
||||
pl_corn.push_back(pl[j]);
|
||||
}
|
||||
if(last_surface != -1)
|
||||
{
|
||||
PointType ap;
|
||||
for(uint k=last_surface; k<j; k++)
|
||||
{
|
||||
ap.x += pl[k].x;
|
||||
ap.y += pl[k].y;
|
||||
ap.z += pl[k].z;
|
||||
ap.curvature += pl[k].curvature;
|
||||
}
|
||||
ap.x /= (j-last_surface);
|
||||
ap.y /= (j-last_surface);
|
||||
ap.z /= (j-last_surface);
|
||||
ap.curvature /= (j-last_surface);
|
||||
pl_surf.push_back(ap);
|
||||
}
|
||||
last_surface = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct)
|
||||
{
|
||||
pl.height = 1; pl.width = pl.size();
|
||||
sensor_msgs::PointCloud2 output;
|
||||
pcl::toROSMsg(pl, output);
|
||||
output.header.frame_id = "livox";
|
||||
output.header.stamp = ct;
|
||||
}
|
||||
|
||||
int Preprocess::plane_judge(const PointCloudXYZI &pl, vector<orgtype> &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<double> disarr;
|
||||
disarr.reserve(20);
|
||||
|
||||
for(i_nex=i_cur; i_nex<i_cur+group_size; i_nex++)
|
||||
{
|
||||
if(types[i_nex].range < blind)
|
||||
{
|
||||
curr_direct.setZero();
|
||||
return 2;
|
||||
}
|
||||
disarr.push_back(types[i_nex].dista);
|
||||
}
|
||||
|
||||
for(;;)
|
||||
{
|
||||
if((i_cur >= pl.size()) || (i_nex >= pl.size())) break;
|
||||
|
||||
if(types[i_nex].range < blind)
|
||||
{
|
||||
curr_direct.setZero();
|
||||
return 2;
|
||||
}
|
||||
vx = pl[i_nex].x - pl[i_cur].x;
|
||||
vy = pl[i_nex].y - pl[i_cur].y;
|
||||
vz = pl[i_nex].z - pl[i_cur].z;
|
||||
two_dis = vx*vx + vy*vy + vz*vz;
|
||||
if(two_dis >= group_dis)
|
||||
{
|
||||
break;
|
||||
}
|
||||
disarr.push_back(types[i_nex].dista);
|
||||
i_nex++;
|
||||
}
|
||||
|
||||
double leng_wid = 0;
|
||||
double v1[3], v2[3];
|
||||
for(uint j=i_cur+1; j<i_nex; j++)
|
||||
{
|
||||
if((j >= pl.size()) || (i_cur >= pl.size())) break;
|
||||
v1[0] = pl[j].x - pl[i_cur].x;
|
||||
v1[1] = pl[j].y - pl[i_cur].y;
|
||||
v1[2] = pl[j].z - pl[i_cur].z;
|
||||
|
||||
v2[0] = v1[1]*vz - vy*v1[2];
|
||||
v2[1] = v1[2]*vx - v1[0]*vz;
|
||||
v2[2] = v1[0]*vy - vx*v1[1];
|
||||
|
||||
double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2];
|
||||
if(lw > leng_wid)
|
||||
{
|
||||
leng_wid = lw;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if((two_dis*two_dis/leng_wid) < p2l_ratio)
|
||||
{
|
||||
curr_direct.setZero();
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint disarrsize = disarr.size();
|
||||
for(uint j=0; j<disarrsize-1; j++)
|
||||
{
|
||||
for(uint k=j+1; k<disarrsize; k++)
|
||||
{
|
||||
if(disarr[j] < disarr[k])
|
||||
{
|
||||
leng_wid = disarr[j];
|
||||
disarr[j] = disarr[k];
|
||||
disarr[k] = leng_wid;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(disarr[disarr.size()-2] < 1e-16)
|
||||
{
|
||||
curr_direct.setZero();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(lidar_type==AVIA)
|
||||
{
|
||||
double dismax_mid = disarr[0]/disarr[disarrsize/2];
|
||||
double dismid_min = disarr[disarrsize/2]/disarr[disarrsize-2];
|
||||
|
||||
if(dismax_mid>=limit_maxmid || dismid_min>=limit_midmin)
|
||||
{
|
||||
curr_direct.setZero();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
double dismax_min = disarr[0] / disarr[disarrsize-2];
|
||||
if(dismax_min >= limit_maxmin)
|
||||
{
|
||||
curr_direct.setZero();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
curr_direct << vx, vy, vz;
|
||||
curr_direct.normalize();
|
||||
return 1;
|
||||
}
|
||||
|
||||
bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir)
|
||||
{
|
||||
if(nor_dir == 0)
|
||||
{
|
||||
if(types[i-1].range<blind || types[i-2].range<blind)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if(nor_dir == 1)
|
||||
{
|
||||
if(types[i+1].range<blind || types[i+2].range<blind)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
double d1 = types[i+nor_dir-1].dista;
|
||||
double d2 = types[i+3*nor_dir-2].dista;
|
||||
double d;
|
||||
|
||||
if(d1<d2)
|
||||
{
|
||||
d = d1;
|
||||
d1 = d2;
|
||||
d2 = d;
|
||||
}
|
||||
|
||||
d1 = sqrt(d1);
|
||||
d2 = sqrt(d2);
|
||||
|
||||
|
||||
if(d1>edgea*d2 || (d1-d2)>edgeb)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
|
@ -0,0 +1,122 @@
|
|||
#include <ros/ros.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <livox_ros_driver/CustomMsg.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
#define IS_VALID(a) ((abs(a)>1e8) ? true : false)
|
||||
|
||||
typedef pcl::PointXYZINormal PointType;
|
||||
typedef pcl::PointCloud<PointType> PointCloudXYZI;
|
||||
|
||||
enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{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<orgtype> 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<orgtype> &types);
|
||||
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
|
||||
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
|
||||
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
|
||||
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
|
||||
|
||||
int group_size;
|
||||
double disA, disB, inf_bound;
|
||||
double limit_maxmid, limit_midmin, limit_maxmin;
|
||||
double p2l_ratio;
|
||||
double jump_up_limit, jump_down_limit;
|
||||
double cos160;
|
||||
double edgea, edgeb;
|
||||
double smallp_intersect, smallp_ratio;
|
||||
double vx, vy, vz;
|
||||
};
|
Loading…
Reference in New Issue