first version
buaaxw@gmail.com
parent
3bc66ff15a
commit
9172e2d953
|
|
@ -0,0 +1,4 @@
|
|||
build
|
||||
Log/*.png
|
||||
Log/*.txt
|
||||
Log/time.pdf
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(fast_lio)
|
||||
|
||||
SET(CMAKE_BUILD_TYPE "Debug")
|
||||
|
||||
ADD_COMPILE_OPTIONS(-std=c++14 )
|
||||
ADD_COMPILE_OPTIONS(-std=c++14 )
|
||||
set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
|
||||
|
||||
add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
|
||||
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
|
||||
|
||||
option(DEPLOY "Deploy on hardware" OFF)
|
||||
if(DEPLOY)
|
||||
add_definitions(-DDEPLOY)
|
||||
endif(DEPLOY)
|
||||
|
||||
find_package(OpenMP QUIET)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
|
||||
|
||||
find_package(PythonLibs REQUIRED)
|
||||
find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
sensor_msgs
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
pcl_ros
|
||||
tf
|
||||
livox_ros_driver
|
||||
message_generation
|
||||
eigen_conversions
|
||||
)
|
||||
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL 1.8 REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
|
||||
message(Eigen: ${EIGEN3_INCLUDE_DIR})
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${PYTHON_INCLUDE_DIRS}
|
||||
include)
|
||||
|
||||
add_message_files(
|
||||
FILES
|
||||
Pose6D.msg
|
||||
States.msg
|
||||
)
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
geometry_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
|
||||
DEPENDS EIGEN3 PCL OpenCV
|
||||
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)
|
||||
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}")
|
||||
|
||||
|
|
@ -0,0 +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.
|
||||
104
README.md
104
README.md
|
|
@ -1,2 +1,102 @@
|
|||
# FAST_LIO
|
||||
A computationally efficient and robust LiDAR-inertial odometry (LIO) package
|
||||
## 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;
|
||||
5. Surpports for different FOV.
|
||||
|
||||
To know more about the details, please refer to our related paper:)
|
||||
|
||||
**Our related paper**: our related papers are now available on arxiv:
|
||||
|
||||
[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.05957)
|
||||
|
||||
**Our related video**: Commming soon.
|
||||
|
||||
<div align="center">
|
||||
<img src="doc/results/HKU_HW.png" width = 49% >
|
||||
<img src="doc/results/HKU_MB_001.png" width = 49% >
|
||||
</div>
|
||||
|
||||
## 1. Prerequisites
|
||||
### 1.1 **Ubuntu** and **ROS**
|
||||
Ubuntu >= 18.04.
|
||||
|
||||
ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
|
||||
|
||||
### 1.2. **PCL && Eigen && openCV**
|
||||
PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).
|
||||
|
||||
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
|
||||
cd ..
|
||||
catkin_make
|
||||
source devel/setup.bash
|
||||
```
|
||||
|
||||
*Remarks:*
|
||||
- If you want to save the pcd file please add map_file_path in launch file.
|
||||
## 3. Directly run
|
||||
### 3.1 For indoor environments and high LiDAR sample rate (20-100hz)
|
||||
Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
|
||||
```
|
||||
....
|
||||
roslaunch fast_lio mapping_avia.launch
|
||||
roslaunch livox_ros_driver livox_lidar_msg.launch
|
||||
|
||||
```
|
||||
|
||||
### 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)
|
||||
|
||||
<div align="center"><img src="doc/results/HKU_LG_Indoor.png" width=100% /></div>
|
||||
|
||||
Download [avia_indoor_quick_shake_example1](https://drive.google.com/file/d/1SWmrwlUD5FlyA-bTr1rakIYx1GxS4xNl/view?usp=sharing) or [avia_indoor_quick_shake_example2](https://drive.google.com/file/d/1wD485CIbzZlNs4z8e20Dv2Q1q-7Gv_AT/view?usp=sharing) and then
|
||||
```
|
||||
roslaunch fast_lio mapping_avia.launch
|
||||
rosbag play YOUR_DOWNLOADED.bag
|
||||
```
|
||||
|
||||
### 4.2 Outdoor rosbag (Livox Avia LiDAR)
|
||||
|
||||
<div align="center"><img src="doc/results/HKU_MB_002.png" width=100% /></div>
|
||||
|
||||
<!-- <div align="center"><img src="doc/results/mid40_outdoor.png" width=90% /></div> -->
|
||||
|
||||
Download [avia_hku_main building_mapping](https://drive.google.com/file/d/1GSb9eLQuwqmgI3VWSB5ApEUhOCFG_Sv5/view?usp=sharing) and then
|
||||
```
|
||||
roslaunch fast_lio mapping_avia_outdoor.launch
|
||||
rosbag play YOUR_DOWNLOADED.bag
|
||||
```
|
||||
|
||||
### 4.3 High-rate rosbag (Livox Avia LiDAR sampled at 100Hz)
|
||||
|
||||
Download [high_rate_avia](https://drive.google.com/file/d/1UM6O3PRN3b730ZeuvKKT3yuOLNQuz8Yf/view?usp=sharing) and then
|
||||
```
|
||||
roslaunch fast_lio mapping_avia.launch
|
||||
rosbag play YOUR_DOWNLOADED.bag
|
||||
```
|
||||
|
||||
## 5.Acknowledgments
|
||||
Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping) and [Loam_Livox](https://github.com/hku-mars/loam_livox).
|
||||
|
|
|
|||
Binary file not shown.
|
After Width: | Height: | Size: 323 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 244 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 2.2 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.6 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.9 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 4.9 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 3.1 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 962 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 580 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.2 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 774 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 698 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 549 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.2 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 434 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 2.0 MiB |
|
|
@ -0,0 +1,103 @@
|
|||
#ifndef EXP_MAT_H
|
||||
#define EXP_MAT_H
|
||||
|
||||
#include <math.h>
|
||||
#include <Eigen/Core>
|
||||
#include <opencv/cv.h>
|
||||
// #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> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
|
||||
{
|
||||
T ang_norm = ang.norm();
|
||||
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
|
||||
if (ang_norm > 0.0000001)
|
||||
{
|
||||
Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
|
||||
Eigen::Matrix<T, 3, 3> K;
|
||||
K << SKEW_SYM_MATRX(r_axis);
|
||||
/// Roderigous Tranformation
|
||||
return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
|
||||
}
|
||||
else
|
||||
{
|
||||
return Eye3;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T, typename Ts>
|
||||
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
|
||||
{
|
||||
T ang_vel_norm = ang_vel.norm();
|
||||
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
|
||||
|
||||
if (ang_vel_norm > 0.0000001)
|
||||
{
|
||||
Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
|
||||
Eigen::Matrix<T, 3, 3> K;
|
||||
|
||||
K << SKEW_SYM_MATRX(r_axis);
|
||||
|
||||
T r_ang = ang_vel_norm * dt;
|
||||
|
||||
/// Roderigous Tranformation
|
||||
return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
|
||||
}
|
||||
else
|
||||
{
|
||||
return Eye3;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
|
||||
{
|
||||
T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
|
||||
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
|
||||
if (norm > 0.00001)
|
||||
{
|
||||
T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
|
||||
Eigen::Matrix<T, 3, 3> K;
|
||||
K << SKEW_SYM_MATRX(r_ang);
|
||||
|
||||
/// Roderigous Tranformation
|
||||
return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
|
||||
}
|
||||
else
|
||||
{
|
||||
return Eye3;
|
||||
}
|
||||
}
|
||||
|
||||
/* Logrithm of a Rotation Matrix */
|
||||
template<typename T>
|
||||
Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R)
|
||||
{
|
||||
T &&theta = std::acos(0.5 * (R.trace() - 1));
|
||||
Eigen::Matrix<T,3,1> K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
|
||||
return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
|
||||
}
|
||||
|
||||
// template<typename T>
|
||||
// cv::Mat Exp(const T &v1, const T &v2, const T &v3)
|
||||
// {
|
||||
|
||||
// T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
|
||||
// cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F);
|
||||
// if (norm > 0.0000001)
|
||||
// {
|
||||
// T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
|
||||
// cv::Mat K = (cv::Mat_<T>(3,3) << SKEW_SYM_MATRX(r_ang));
|
||||
|
||||
// /// Roderigous Tranformation
|
||||
// return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// return Eye3;
|
||||
// }
|
||||
// }
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,140 @@
|
|||
#ifndef COMMON_LIB_H
|
||||
#define COMMON_LIB_H
|
||||
|
||||
#include <Exp_mat.h>
|
||||
#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>
|
||||
|
||||
|
||||
// #define DEBUG_PRINT
|
||||
|
||||
#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 CUBE_LEN (6.0)
|
||||
#define LIDAR_SP_LEN (2)
|
||||
#define INIT_COV (0.0001)
|
||||
|
||||
#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))
|
||||
|
||||
typedef fast_lio::Pose6D Pose6D;
|
||||
typedef pcl::PointXYZINormal PointType;
|
||||
typedef pcl::PointCloud<PointType> PointCloudXYZI;
|
||||
|
||||
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
|
||||
|
||||
struct MeasureGroup // Lidar data and imu dates for the curent process
|
||||
{
|
||||
MeasureGroup()
|
||||
{
|
||||
this->lidar.reset(new PointCloudXYZI());
|
||||
};
|
||||
double lidar_beg_time;
|
||||
PointCloudXYZI::Ptr lidar;
|
||||
std::deque<sensor_msgs::Imu::ConstPtr> imu;
|
||||
};
|
||||
|
||||
struct StatesGroup
|
||||
{
|
||||
StatesGroup() {
|
||||
this->rot_end = Eigen::Matrix3d::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;
|
||||
};
|
||||
|
||||
StatesGroup& operator+=(const Eigen::Matrix<double, DIM_OF_STATES, 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);
|
||||
this->vel_end += state_add.block<3,1>(6,0);
|
||||
this->bias_g += state_add.block<3,1>(9,0);
|
||||
this->bias_a += state_add.block<3,1>(12,0);
|
||||
this->gravity += state_add.block<3,1>(15,0);
|
||||
return *this;
|
||||
};
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
T rad2deg(T radians)
|
||||
{
|
||||
return radians * 180.0 / PI_M;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T deg2rad(T degrees)
|
||||
{
|
||||
return degrees * PI_M / 180.0;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
Pose6D rot_kp;
|
||||
rot_kp.offset_time = t;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
rot_kp.acc[i] = a(i);
|
||||
rot_kp.gyr[i] = g(i);
|
||||
rot_kp.vel[i] = v(i);
|
||||
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);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
|
||||
{
|
||||
T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
|
||||
bool singular = sy < 1e-6;
|
||||
T x, y, z;
|
||||
if(!singular)
|
||||
{
|
||||
x = atan2(rot(2, 1), rot(2, 2));
|
||||
y = atan2(-rot(2, 0), sy);
|
||||
z = atan2(rot(1, 0), rot(0, 0));
|
||||
}
|
||||
else
|
||||
{
|
||||
x = atan2(-rot(1, 2), rot(1, 1));
|
||||
y = atan2(-rot(2, 0), sy);
|
||||
z = 0;
|
||||
}
|
||||
Eigen::Matrix<T, 3, 1> ang(x, y, z);
|
||||
return ang;
|
||||
}
|
||||
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,41 @@
|
|||
<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="5"/>
|
||||
<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="2" />
|
||||
<param name="dense_map_enable" type="bool" value="true" />
|
||||
<param name="fov_degree" type="double" value="80" />
|
||||
<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="6" />
|
||||
</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,41 @@
|
|||
<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="3"/>
|
||||
<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="10" />
|
||||
<param name="dense_map_enable" type="bool" value="true" />
|
||||
<param name="fov_degree" type="double" value="80" />
|
||||
<param name="filter_size_corner" type="double" value="0.5" />
|
||||
<param name="filter_size_surf" type="double" value="0.4" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="cube_side_length" type="double" value="50" />
|
||||
</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,26 @@
|
|||
<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>
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
# the preintegrated Lidar states at the time of IMU measurements in a frame
|
||||
float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point
|
||||
float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin
|
||||
float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin
|
||||
float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin
|
||||
float64[3] pos # the preintegrated position (global frame) at the Lidar origin
|
||||
float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
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
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>fast_lio</name>
|
||||
<version>0.0.0</version>
|
||||
|
||||
<description>
|
||||
This is a modified version of LOAM which is original algorithm
|
||||
is described in the following paper:
|
||||
J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
|
||||
Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
|
||||
</description>
|
||||
|
||||
<maintainer email="dev@livoxtech.com">claydergc</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<author email="zhangji@cmu.edu">Ji Zhang</author>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>pcl_ros</build_depend>
|
||||
<build_depend>livox_ros_driver</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>pcl_ros</run_depend>
|
||||
<run_depend>livox_ros_driver</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>rosbag</test_depend>
|
||||
|
||||
<export>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,697 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Axes1
|
||||
- /odometry1/odomPath1
|
||||
- /mapping1
|
||||
- /mapping1/surround1
|
||||
- /mapping1/currPoints1
|
||||
- /mapping1/PointCloud21
|
||||
- /Debug1/PointCloud21
|
||||
- /Debug1/PointCloud22
|
||||
- /Debug1/PointCloud23
|
||||
- /Odometry1/Odometry1
|
||||
- /Odometry1/Odometry1/Shape1
|
||||
- /Odometry1/Odometry2
|
||||
- /Odometry1/Odometry2/Shape1
|
||||
- /Odometry1/Odometry3
|
||||
- /Odometry1/Odometry3/Shape1
|
||||
- /Features1
|
||||
- /Features1/PointCloud21
|
||||
- /Features1/PointCloud22
|
||||
- /Features1/PointCloud23
|
||||
- /Axes2
|
||||
- /Odometry2
|
||||
- /Odometry2/Shape1
|
||||
- /Marker1
|
||||
- /PointStamped1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 1066
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: currPoints
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: false
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 160
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: false
|
||||
- Class: rviz/Axes
|
||||
Enabled: true
|
||||
Length: 0.5
|
||||
Name: Axes
|
||||
Radius: 0.10000000149011612
|
||||
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
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Buffer Length: 10000
|
||||
Class: rviz/Path
|
||||
Color: 255; 85; 0
|
||||
Enabled: false
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.029999999329447746
|
||||
Name: mappingPath
|
||||
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: false
|
||||
- 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: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 15
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: surround
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 1
|
||||
Selectable: false
|
||||
Size (Pixels): 4
|
||||
Size (m): 0.05000000074505806
|
||||
Style: Points
|
||||
Topic: /cloud_registered
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.20000000298023224
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: -1.7674484252929688
|
||||
Min Value: -8.243647575378418
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 1e+9
|
||||
Enabled: true
|
||||
Invert Rainbow: true
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 90.00662994384766
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: currPoints
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 100000
|
||||
Selectable: true
|
||||
Size (Pixels): 1
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /cloud_registered
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 0; 0
|
||||
Max Intensity: 152
|
||||
Min Color: 255; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.05000000074505806
|
||||
Style: Flat Squares
|
||||
Topic: /Laser_map
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: false
|
||||
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:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Keep: 1
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.0010000000474974513
|
||||
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
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Axes
|
||||
Topic: /re_local_pose
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: false
|
||||
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): 5
|
||||
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: 155
|
||||
Min Color: 78; 154; 6
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.05999999865889549
|
||||
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: 160
|
||||
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: true
|
||||
Name: Features
|
||||
- Class: rviz/Axes
|
||||
Enabled: true
|
||||
Length: 1
|
||||
Name: Axes
|
||||
Radius: 0.10000000149011612
|
||||
Reference Frame: aft_mapped
|
||||
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: true
|
||||
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: true
|
||||
- Class: rviz/Marker
|
||||
Enabled: false
|
||||
Marker Topic: visualization_marker
|
||||
Name: Marker
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Class: rviz/PointStamped
|
||||
Color: 204; 41; 204
|
||||
Enabled: true
|
||||
History Length: 1
|
||||
Name: PointStamped
|
||||
Radius: 0.20000000298023224
|
||||
Topic: /clicked_point
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 0; 0; 0
|
||||
Default Light: true
|
||||
Fixed Frame: camera_init
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 34.710601806640625
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 6.608828544616699
|
||||
Y: 3.081573247909546
|
||||
Z: -3.704350709915161
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: -0.17020323872566223
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.0474979877471924
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1383
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000004b5fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000618000001a800000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004df00000052fc0100000002fb0000000800540069006d00650100000000000004df000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000383000004b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1247
|
||||
X: 67
|
||||
Y: 27
|
||||
|
|
@ -0,0 +1,368 @@
|
|||
#include <cmath>
|
||||
#include <math.h>
|
||||
#include <deque>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <fstream>
|
||||
#include <csignal>
|
||||
#include <ros/ros.h>
|
||||
#include <Exp_mat.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <common_lib.h>
|
||||
#include <pcl/common/io.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <condition_variable>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
#include <pcl/kdtree/kdtree_flann.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <fast_lio/States.h>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
|
||||
/// *************Preconfiguration
|
||||
|
||||
#define MAX_INI_COUNT (50)
|
||||
|
||||
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
|
||||
|
||||
/// *************IMU Process and undistortion
|
||||
class ImuProcess
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
ImuProcess();
|
||||
~ImuProcess();
|
||||
|
||||
void 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);
|
||||
|
||||
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;
|
||||
|
||||
private:
|
||||
/*** Whether is the first frame, init for first frame ***/
|
||||
bool b_first_frame_ = true;
|
||||
bool imu_need_init_ = true;
|
||||
|
||||
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 ***/
|
||||
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;
|
||||
};
|
||||
|
||||
ImuProcess::ImuProcess()
|
||||
: b_first_frame_(true), imu_need_init_(true), last_imu_(nullptr), 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);
|
||||
}
|
||||
|
||||
ImuProcess::~ImuProcess() {fout.close();}
|
||||
|
||||
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;
|
||||
v_imu_.clear();
|
||||
IMUpose.clear();
|
||||
|
||||
cur_pcl_un_.reset(new PointCloudXYZI());
|
||||
fout.close();
|
||||
|
||||
}
|
||||
|
||||
void ImuProcess::IMU_Initial(const MeasureGroup &meas, StatesGroup &state_inout, 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;
|
||||
|
||||
if (b_first_frame_)
|
||||
{
|
||||
Reset();
|
||||
N = 1;
|
||||
b_first_frame_ = false;
|
||||
}
|
||||
|
||||
for (const auto &imu : meas.imu)
|
||||
{
|
||||
const auto &imu_acc = imu->linear_acceleration;
|
||||
const auto &gyr_acc = imu->angular_velocity;
|
||||
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
|
||||
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
|
||||
|
||||
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);
|
||||
|
||||
N ++;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void ImuProcess::UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out)
|
||||
{
|
||||
/*** add the imu of the last frame-tail to the of current frame-head ***/
|
||||
auto v_imu = meas.imu;
|
||||
v_imu.push_front(last_imu_);
|
||||
const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
|
||||
const double &imu_end_time = v_imu.back()->header.stamp.toSec();
|
||||
const double &pcl_beg_time = meas.lidar_beg_time;
|
||||
|
||||
/*** sort point clouds by offset time ***/
|
||||
pcl_out = *(meas.lidar);
|
||||
std::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;
|
||||
|
||||
/*** Initialize IMU pose ***/
|
||||
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));
|
||||
|
||||
/*** 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());
|
||||
double dt = 0;
|
||||
for (auto it_imu = v_imu.begin(); it_imu != (v_imu.end() - 1); it_imu++)
|
||||
{
|
||||
auto &&head = *(it_imu);
|
||||
auto &&tail = *(it_imu + 1);
|
||||
|
||||
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);
|
||||
acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
|
||||
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;
|
||||
|
||||
#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();
|
||||
|
||||
/* 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_f;
|
||||
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;
|
||||
|
||||
/* save the poses at each IMU measurements */
|
||||
angvel_last = angvel_avr;
|
||||
acc_s_last = acc_imu;
|
||||
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));
|
||||
}
|
||||
|
||||
/*** 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
|
||||
|
||||
/*** undistort each lidar point (backward propagation) ***/
|
||||
auto it_pcl = pcl_out.points.end() - 1;
|
||||
for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
|
||||
{
|
||||
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;
|
||||
vel_imu<<VEC_FROM_ARRAY(head->vel);
|
||||
pos_imu<<VEC_FROM_ARRAY(head->pos);
|
||||
angvel_avr<<VEC_FROM_ARRAY(head->gyr);
|
||||
|
||||
int i = 0;
|
||||
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
|
||||
{
|
||||
dt = it_pcl->curvature / double(1000) - head->offset_time;
|
||||
|
||||
/* Transform to the 'end' frame, using only the rotation
|
||||
* 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
|
||||
it_pcl->x = P_compensate(0);
|
||||
it_pcl->y = P_compensate(1);
|
||||
it_pcl->z = P_compensate(2);
|
||||
|
||||
if (it_pcl == pcl_out.points.begin()) break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ImuProcess::Process(const MeasureGroup &meas, StatesGroup &stat, 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;};
|
||||
ROS_ASSERT(meas.lidar != nullptr);
|
||||
|
||||
if (imu_need_init_)
|
||||
{
|
||||
/// The very first lidar frame
|
||||
IMU_Initial(meas, stat, init_iter_num);
|
||||
|
||||
imu_need_init_ = true;
|
||||
|
||||
last_imu_ = meas.imu.back();
|
||||
|
||||
if (init_iter_num > MAX_INI_COUNT)
|
||||
{
|
||||
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]);
|
||||
}
|
||||
|
||||
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_);
|
||||
|
||||
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;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,86 @@
|
|||
# import matplotlib
|
||||
# matplotlib.use('Agg')
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
fig, axs = plt.subplots(5)
|
||||
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')
|
||||
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')
|
||||
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 i in range(5):
|
||||
# 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()
|
||||
|
||||
# #### Draw time calculation
|
||||
# fig = plt.figure()
|
||||
# font1 = {'family' : 'Times New Roman',
|
||||
# 'weight' : 'normal',
|
||||
# 'size' : 12,
|
||||
# }
|
||||
# c="red"
|
||||
# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
|
||||
# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
|
||||
# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
|
||||
# # n = a_out[:,1].size
|
||||
# # time_mean = a_out[:,1].mean()
|
||||
# # time_se = a_out[:,1].std() / np.sqrt(n)
|
||||
# # time_err = a_out[:,1] - time_mean
|
||||
# # feat_mean = a_out[:,2].mean()
|
||||
# # feat_err = a_out[:,2] - feat_mean
|
||||
# # feat_se = a_out[:,2].std() / np.sqrt(n)
|
||||
# ax1 = fig.add_subplot(111)
|
||||
# ax1.set_ylabel('Effective Feature Numbers',font1)
|
||||
# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
|
||||
# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
|
||||
# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
|
||||
# ax1.set_ylim([0, 3000])
|
||||
|
||||
# ax2 = ax1.twinx()
|
||||
# ax2.spines['right'].set_color('red')
|
||||
# ax2.set_ylabel('Compute Time (ms)',font1)
|
||||
# ax2.yaxis.label.set_color('red')
|
||||
# ax2.tick_params(axis='y', colors='red')
|
||||
# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
|
||||
# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
|
||||
# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
|
||||
# ax2.set_xlim([0.5, 3.5])
|
||||
# ax2.set_ylim([0, 100])
|
||||
|
||||
# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
|
||||
# # print(time_se)
|
||||
# # print(a_out3[:,2])
|
||||
plt.grid()
|
||||
plt.savefig("Log/time.pdf", dpi=1200)
|
||||
plt.show()
|
||||
Loading…
Reference in New Issue