12345qiupeng
/
Fast-Lio
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'borongyuan-jetson' into main

main
buaaxw@gmail.com 2021-01-19 14:31:54 +08:00
parent 47e9f27b14 ac8a46cfb1
commit d57fe8aeff
12 changed files with 893 additions and 479 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -1,5 +1,6 @@
cmake_minimum_required(VERSION 2.8.3)
project(fast_lio)
cmake_policy(SET CMP0074 NEW)
SET(CMAKE_BUILD_TYPE "Debug")
@ -79,6 +80,7 @@ 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}")

25
README.md
View File

@ -4,7 +4,14 @@
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.
It should be noted current version of FAST-LIO does not support Velodyne LiDAR, we may support them after March 2021.
**Developers**
[Wei Xu 徐威](https://github.com/XW-HKU): Laser mapping and pose optimization;
[Zheng Liu 刘政](https://github.com/Zale-Liu): Features extraction.
To know more about the details, please refer to our related paper:)
@ -12,7 +19,7 @@ To know more about the details, please refer to our related paper:)
[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196)
**Our related video**: Commming soon.
**Our related video**: https://youtu.be/iYCY6T79oNU
<div align="center">
<img src="doc/results/HKU_HW.png" width = 49% >
@ -46,8 +53,11 @@ Clone the repository and catkin_make:
catkin_make
source devel/setup.bash
```
*Remarks:*
- 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 and high frame-rate (such as 100hz)
### 3.1 For indoor environments (support maximum 50hz frame rate)
Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
```
....
@ -76,14 +86,7 @@ Download [avia_indoor_quick_shake_example1](https://drive.google.com/file/d/1SWm
roslaunch fast_lio mapping_avia.launch
rosbag play YOUR_DOWNLOADED.bag
```
### 4.2 High-rate rosbag (Livox Avia LiDAR with 100Hz frame-rate)
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
```
### 4.3 Outdoor rosbag (Livox Avia LiDAR)
### 4.2 Outdoor rosbag (Livox Avia LiDAR)
<div align="center"><img src="doc/results/HKU_MB_002.png" width=100% /></div>

0
config/fast_lio.yaml Normal file
View File

2
include/Exp_mat.h
View File

@ -3,7 +3,7 @@
#include <math.h>
#include <Eigen/Core>
#include <opencv/cv.h>
#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

79
include/common_lib.h
View File

@ -1,7 +1,7 @@
#ifndef COMMON_LIB_H
#define COMMON_LIB_H
#include <Exp_mat.h>
#include <so3_math.h>
#include <Eigen/Eigen>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
@ -14,6 +14,7 @@
// #define DEBUG_PRINT
// #define USE_ikdtree
#define PI_M (3.14159265358)
#define G_m_s2 (9.8099) // Gravaty const in GuangDong/China
@ -21,7 +22,7 @@
#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 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]
@ -29,6 +30,7 @@
#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;
@ -65,9 +67,45 @@ struct StatesGroup
this->cov = Eigen::Matrix<double,DIM_OF_STATES,DIM_OF_STATES>::Identity() * INIT_COV;
};
StatesGroup(const StatesGroup& b) {
this->rot_end = b.rot_end;
this->pos_end = b.pos_end;
this->vel_end = b.vel_end;
this->bias_g = b.bias_g;
this->bias_a = b.bias_a;
this->gravity = b.gravity;
this->cov = b.cov;
};
StatesGroup& operator=(const StatesGroup& b)
{
this->rot_end = b.rot_end;
this->pos_end = b.pos_end;
this->vel_end = b.vel_end;
this->bias_g = b.bias_g;
this->bias_a = b.bias_a;
this->gravity = b.gravity;
this->cov = b.cov;
return *this;
};
StatesGroup operator+(const Eigen::Matrix<double, DIM_OF_STATES, 1> &state_add)
{
StatesGroup a;
a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
a.pos_end = this->pos_end + state_add.block<3,1>(3,0);
a.vel_end = this->vel_end + state_add.block<3,1>(6,0);
a.bias_g = this->bias_g + state_add.block<3,1>(9,0);
a.bias_a = this->bias_a + state_add.block<3,1>(12,0);
a.gravity = this->gravity + state_add.block<3,1>(15,0);
a.cov = this->cov;
return a;
};
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->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);
@ -76,6 +114,19 @@ struct StatesGroup
return *this;
};
Eigen::Matrix<double, DIM_OF_STATES, 1> operator-(const StatesGroup& b)
{
Eigen::Matrix<double, DIM_OF_STATES, 1> a;
Eigen::Matrix3d 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;
a.block<3,1>(9,0) = this->bias_g - b.bias_g;
a.block<3,1>(12,0) = this->bias_a - b.bias_a;
a.block<3,1>(15,0) = this->gravity - b.gravity;
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)
@ -115,26 +166,6 @@ auto set_pose6d(const double t, const Eigen::Matrix<T, 3, 1> &a, const Eigen::Ma
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

105
include/so3_math.h Normal file
View File

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

6
launch/mapping_avia.launch
View File

@ -20,12 +20,12 @@
<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="4"/>
<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">
<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="2" />
<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" />

8
launch/mapping_avia_outdoor.launch
View File

@ -20,18 +20,18 @@
<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"/>
<param name="point_filter_num" type="int" value="4"/>
<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="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.4" />
<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="50" />
<param name="cube_side_length" type="double" value="80" />
</node>
<group if="$(arg rviz)">

184
rviz_cfg/loam_livox.rviz
View File

@ -1,6 +1,6 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
@ -11,6 +11,7 @@ Panels:
- /mapping1/surround1
- /mapping1/currPoints1
- /mapping1/PointCloud21
- /mapping1/PointCloud22
- /Debug1/PointCloud21
- /Debug1/PointCloud22
- /Debug1/PointCloud23
@ -20,17 +21,14 @@ Panels:
- /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
- /Path1
Splitter Ratio: 0.520588219165802
Tree Height: 839
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@ -49,7 +47,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: currPoints
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
@ -57,7 +55,7 @@ Toolbars:
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
- Alpha: 1
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
@ -77,9 +75,9 @@ Visualization Manager:
Value: false
- Class: rviz/Axes
Enabled: true
Length: 0.5
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Radius: 0.05999999865889549
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/Group
@ -161,29 +159,6 @@ Visualization Manager:
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:
@ -206,7 +181,7 @@ Visualization Manager:
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 4
Size (Pixels): 2
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_registered
@ -214,11 +189,11 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.20000000298023224
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.32742905616760254
Min Value: -2.179872751235962
Max Value: 7.485495090484619
Min Value: -0.6337304711341858
Value: true
Axis: Z
Channel Name: intensity
@ -245,6 +220,36 @@ 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
@ -253,26 +258,26 @@ Visualization Manager:
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 0; 0
Max Intensity: 152
Min Color: 255; 0; 0
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Queue Size: 1
Selectable: true
Size (Pixels): 3
Size (m): 0.05000000074505806
Size (m): 0.20000000298023224
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Use rainbow: true
Value: false
Enabled: true
Name: mapping
@ -500,7 +505,7 @@ Visualization Manager:
Position Transformer: XYZ
Queue Size: 1
Selectable: true
Size (Pixels): 5
Size (Pixels): 7
Size (m): 0.20000000298023224
Style: Points
Topic: /laser_cloud_flat
@ -523,7 +528,7 @@ Visualization Manager:
Enabled: true
Invert Rainbow: false
Max Color: 78; 154; 6
Max Intensity: 155
Max Intensity: 0
Min Color: 78; 154; 6
Min Intensity: 0
Name: PointCloud2
@ -531,7 +536,7 @@ Visualization Manager:
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.05999999865889549
Size (m): 0.07999999821186066
Style: Flat Squares
Topic: /livox_undistort
Unreliable: false
@ -553,7 +558,7 @@ Visualization Manager:
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 160
Max Intensity: 155
Min Color: 255; 255; 255
Min Intensity: 0
Name: PointCloud2
@ -568,13 +573,13 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: false
Value: false
Enabled: true
Enabled: false
Name: Features
- Class: rviz/Axes
Enabled: true
Length: 1
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Radius: 0.05999999865889549
Reference Frame: aft_mapped
Value: true
- Angle Tolerance: 0.10000000149011612
@ -594,7 +599,7 @@ Visualization Manager:
Scale: 1
Value: true
Value: true
Enabled: true
Enabled: false
Keep: 10000
Name: Odometry
Position Tolerance: 0.10000000149011612
@ -610,25 +615,42 @@ Visualization Manager:
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
- Alpha: 0
Buffer Length: 2
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
History Length: 1
Name: PointStamped
Radius: 0.20000000298023224
Topic: /clicked_point
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 0
Pose Style: Arrows
Radius: 0.029999999329447746
Shaft Diameter: 0.15000000596046448
Shaft Length: 0.15000000596046448
Topic: /path
Unreliable: false
Value: true
- Class: rviz/Image
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
Unreliable: false
Value: false
Enabled: true
Global Options:
Background Color: 0; 0; 0
@ -657,33 +679,35 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 34.710601806640625
Distance: 49.2824821472168
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 6.49392557144165
Y: -0.4694555699825287
Z: -1.3836920261383057
X: 9.622546195983887
Y: -18.686565399169922
Z: -4.2439751625061035
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.3497962951660156
Pitch: 0.8597958087921143
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 5.425689220428467
Yaw: 2.7072019577026367
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1383
Height: 1056
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd000000040000000000000156000004b5fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004b5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000618000001a800000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004df00000052fc0100000002fb0000000800540069006d00650100000000000004df000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000383000004b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000017900000384fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002700000384000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001aa000001e20000001600fffffffb0000000a0049006d00610067006501000002730000027f00000000000000000000000100000152000004b5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074900000052fc0100000002fb0000000800540069006d0065010000000000000749000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005ca0000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@ -692,6 +716,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: true
Width: 1247
X: 67
Y: 27
Width: 1865
X: 55
Y: 24

2
src/IMU_Processing.hpp
View File

@ -6,7 +6,7 @@
#include <fstream>
#include <csignal>
#include <ros/ros.h>
#include <Exp_mat.h>
#include <so3_math.h>
#include <Eigen/Eigen>
#include <common_lib.h>
#include <pcl/common/io.h>

63
src/feature_extract.cpp
View File

@ -5,8 +5,12 @@
// Feature will be updated in next version
typedef pcl::PointXYZINormal PointType;
using namespace std;
#define IS_VALID(a) ((abs(a)>1e8) ? true : false)
typedef pcl::PointXYZINormal PointType;
ros::Publisher pub_full, pub_surf, pub_corn;
enum LID_TYPE{MID, HORIZON, VELO16, OUST64};
@ -167,6 +171,7 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
pcl::PointCloud<PointType> pl_full, pl_corn, pl_surf;
uint plsize = msg->point_num;
pl_corn.reserve(plsize); pl_surf.reserve(plsize);
pl_full.resize(plsize);
@ -175,19 +180,28 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
pl_buff[i].reserve(plsize);
}
for(uint i=0; i<plsize; i++)
for(uint i=1; i<plsize; i++)
{
if(msg->points[i].line < N_SCANS)
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)
&& (!IS_VALID(msg->points[i].x)) && (!IS_VALID(msg->points[i].y)) && (!IS_VALID(msg->points[i].z)))
{
pl_full[i].x = msg->points[i].x;
pl_full[i].y = msg->points[i].y;
pl_full[i].z = msg->points[i].z;
pl_full[i].intensity = msg->points[i].reflectivity;
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
pl_buff[msg->points[i].line].push_back(pl_full[i]);
if((std::abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)
|| (std::abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
|| (std::abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
{
pl_buff[msg->points[i].line].push_back(pl_full[i]);
}
}
}
if(pl_buff[0].size() <= 7) {return;}
for(int j=0; j<N_SCANS; j++)
{
pcl::PointCloud<PointType> &pl = pl_buff[j];
@ -197,11 +211,13 @@ void horizon_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
plsize--;
for(uint i=0; i<plsize; 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;
// std::cout<<vx<<" "<<vx<<" "<<vz<<" "<<std::endl;
}
// plsize++;
types[plsize].range = sqrt(pl[plsize].x*pl[plsize].x + pl[plsize].y*pl[plsize].y);
@ -400,10 +416,8 @@ void velo16_handler1(const sensor_msgs::PointCloud2::ConstPtr &msg)
// last_stat = orders[scanID];
// }
idx++;
for(int j=0; j<N_SCANS; j++)
{
pcl::PointCloud<PointType> &pl = pl_buff[j];
@ -496,12 +510,12 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
}
// Surf
plsize2 = plsize - group_size;
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, i2;
uint i_nex = 0, i2;
uint last_i = 0; uint last_i_nex = 0;
int last_state = 0;
int plane_type;
@ -514,6 +528,7 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
}
// i_nex = i;
i2 = i;
// std::cout<<" i: "<<i<<" i_nex "<<i_nex<<"group_size: "<<group_size<<" plsize "<<plsize<<" plsize2 "<<plsize2<<std::endl;
plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
if(plane_type == 1)
@ -605,7 +620,7 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
}
plsize2 = plsize - 3;
plsize2 = plsize > 3 ? plsize - 3 : 0;
for(uint i=head+3; i<plsize2; i++)
{
if(types[i].range<blind || types[i].ftype>=Real_Plane)
@ -777,20 +792,24 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
if(j == uint(last_surface+point_filter_num-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;
PointType ap;
ap.x = pl[k].x;
ap.y = pl[k].y;
ap.z = pl[k].z;
ap.curvature = pl[k].curvature;
ap.intensity = pl[k].intensity;
pl_surf.push_back(ap);
}
ap.x /= point_filter_num;
ap.y /= point_filter_num;
ap.z /= point_filter_num;
ap.curvature /= point_filter_num;
pl_surf.push_back(ap);
// printf("%d-%d: %lf %lf %lf\n", last_surface, j, ap.x, ap.y, ap.z);
// PointType ap;
// ap.x = pl[last_surface].x;
// ap.y = pl[last_surface].y;
// ap.z = pl[last_surface].z;
// ap.curvature += pl[last_surface].curvature;
// pl_surf.push_back(ap);
last_surface = -1;
}
}
@ -819,7 +838,6 @@ void give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types, pcl::P
last_surface = -1;
}
}
}
void pub_func(pcl::PointCloud<PointType> &pl, ros::Publisher pub, const ros::Time &ct)
@ -854,6 +872,8 @@ int plane_judge(const pcl::PointCloud<PointType> &pl, vector<orgtype> &types, ui
for(;;)
{
if((i_cur >= pl.size()) || (i_nex >= pl.size())) break;
if(types[i_nex].range < blind)
{
curr_direct.setZero();
@ -875,6 +895,7 @@ int plane_judge(const pcl::PointCloud<PointType> &pl, vector<orgtype> &types, ui
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;

896
src/laserMapping.cpp
View File

File diff suppressed because it is too large Load Diff