mirror of
https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
synced 2023-05-28 12:51:38 +08:00
Migrate to Open3D* for better performance.
This commit is contained in:
HViktorTsoi
17
README.md
17
README.md
@@ -2,6 +2,10 @@
|
||||
|
||||
A simple localization framework that can re-localize in built maps based on [FAST-LIO](https://github.com/hku-mars/FAST_LIO).
|
||||
|
||||
## News
|
||||
|
||||
- Migrate to **Open3D** for better performance.
|
||||
|
||||
## 1. Features
|
||||
- Realtime 3D global localization in a pre-built point cloud map.
|
||||
By fusing low-frequency global localization (about 0.5~0.2Hz), and high-frequency odometry from FAST-LIO, the entire system is computationally efficient.
|
||||
@@ -35,10 +39,17 @@ This part of dependency is consistent with FAST-LIO, please refer to the documen
|
||||
|
||||
- [ros_numpy](https://github.com/eric-wieser/ros_numpy)
|
||||
|
||||
- [python-pcl](https://github.com/strawlab/python-pcl)
|
||||
- [Open3d](https://github.com/strawlab/python-pcl)
|
||||
|
||||
Notice that, there may be issue when installing **Open3D** directly using pip in **Python2.7**, you may firstly install **pyrsistent**:
|
||||
```shell
|
||||
pip install pyrsistent==0.15
|
||||
```
|
||||
Then
|
||||
```
|
||||
pip install open3d
|
||||
```
|
||||
|
||||
Notice that, if using **Ubuntu 18.04** with native **PCL 1.8**, there may be issue after installing **python-pcl** through pip,
|
||||
please refer to https://github.com/barrygxwan/Python-PCL-Ubuntu18.04, download the **.whl** file and then install.
|
||||
|
||||
## 3. Build
|
||||
Clone the repository and catkin_make:
|
||||
|
||||
@@ -5,6 +5,7 @@ Panels:
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Grid1
|
||||
- /mapping1/surround1
|
||||
- /mapping1/currPoints1
|
||||
- /mapping1/currPoints1/Autocompute Value Bounds1
|
||||
@@ -47,10 +48,10 @@ Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Cell Size: 1000
|
||||
Cell Size: 0.20000000298023224
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: false
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
@@ -61,9 +62,9 @@ Visualization Manager:
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 40
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: false
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: body
|
||||
Value: true
|
||||
- Class: rviz/Axes
|
||||
Enabled: false
|
||||
Length: 0.699999988079071
|
||||
@@ -265,7 +266,7 @@ Visualization Manager:
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.30000001192092896
|
||||
- Alpha: 0.6000000238418579
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
@@ -397,25 +398,25 @@ Visualization Manager:
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 109.72235107421875
|
||||
Distance: 52.648162841796875
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
X: 17.5119571685791
|
||||
Y: -13.623329162597656
|
||||
Z: 4.375192642211914
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.0497967004776
|
||||
Pitch: 1.5697963237762451
|
||||
Target Frame: body
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 1.9003976583480835
|
||||
Yaw: 1.278587818145752
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
|
||||
@@ -6,7 +6,7 @@ import copy
|
||||
import thread
|
||||
import time
|
||||
|
||||
import pcl
|
||||
import open3d as o3d
|
||||
import rospy
|
||||
import ros_numpy
|
||||
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion
|
||||
@@ -40,30 +40,14 @@ def msg_to_array(pc_msg):
|
||||
|
||||
|
||||
def registration_at_scale(pc_scan, pc_map, initial, scale):
|
||||
try:
|
||||
sor = pc_scan.make_voxel_grid_filter()
|
||||
sor.set_leaf_size(SCAN_VOXEL_SIZE * scale, SCAN_VOXEL_SIZE * scale, SCAN_VOXEL_SIZE * scale)
|
||||
result_icp = o3d.registration.registration_icp(
|
||||
pc_scan.voxel_down_sample(SCAN_VOXEL_SIZE * scale), pc_map.voxel_down_sample(MAP_VOXEL_SIZE * scale),
|
||||
1.0 * scale, initial,
|
||||
o3d.registration.TransformationEstimationPointToPoint(),
|
||||
o3d.registration.ICPConvergenceCriteria(max_iteration=20)
|
||||
)
|
||||
|
||||
# 用初始解转换到对应坐标系
|
||||
pc = np.array(sor.filter())
|
||||
pc = np.column_stack([pc, np.ones(pc.shape[0]).reshape(-1, 1)])
|
||||
pc_in_map = (np.matmul(initial, pc.T)).T
|
||||
scan_tobe_mapped = pcl.PointCloud()
|
||||
scan_tobe_mapped.from_array(pc_in_map[:, :3].astype(np.float32))
|
||||
|
||||
# 对地图降采样
|
||||
sor = pc_map.make_voxel_grid_filter()
|
||||
sor.set_leaf_size(MAP_VOXEL_SIZE * scale, MAP_VOXEL_SIZE * scale, MAP_VOXEL_SIZE * scale)
|
||||
map_down = sor.filter()
|
||||
|
||||
icp = map_down.make_IterativeClosestPoint()
|
||||
converged, transformation, estimate, fitness = \
|
||||
icp.icp(scan_tobe_mapped, map_down, max_iter=10)
|
||||
# 这里要将初始解进行变换， 因为icp估计的是精确位置到初始解的delta
|
||||
return np.matmul(transformation, initial), fitness
|
||||
except Exception as e:
|
||||
rospy.logerr('{}'.format(e))
|
||||
return initial, 1e9
|
||||
return result_icp.transformation, result_icp.fitness
|
||||
|
||||
|
||||
def inverse_se3(trans):
|
||||
@@ -99,7 +83,7 @@ def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom):
|
||||
T_base_link_to_map = inverse_se3(T_map_to_base_link)
|
||||
|
||||
# 把地图转换到lidar系下
|
||||
global_map_in_map = np.array(global_map)
|
||||
global_map_in_map = np.array(global_map.points)
|
||||
global_map_in_map = np.column_stack([global_map_in_map, np.ones(len(global_map_in_map))])
|
||||
global_map_in_base_link = np.matmul(T_base_link_to_map, global_map_in_map.T).T
|
||||
|
||||
@@ -118,13 +102,13 @@ def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom):
|
||||
(global_map_in_base_link[:, 0] < FOV_FAR) &
|
||||
(np.abs(np.arctan2(global_map_in_base_link[:, 1], global_map_in_base_link[:, 0])) < FOV / 2.0)
|
||||
)
|
||||
global_map_in_FOV = pcl.PointCloud()
|
||||
global_map_in_FOV.from_array(np.squeeze(global_map_in_map[indices, :3]).astype(np.float32))
|
||||
global_map_in_FOV = o3d.geometry.PointCloud()
|
||||
global_map_in_FOV.points = o3d.utility.Vector3dVector(np.squeeze(global_map_in_map[indices, :3]))
|
||||
|
||||
# 发布fov内点云
|
||||
header = cur_odom.header
|
||||
header.frame_id = 'map'
|
||||
publish_point_cloud(pub_submap, header, global_map_in_FOV.to_array()[::10])
|
||||
publish_point_cloud(pub_submap, header, np.array(global_map_in_FOV.points)[::10])
|
||||
|
||||
return global_map_in_FOV
|
||||
|
||||
@@ -153,7 +137,7 @@ def global_localization(pose_estimation):
|
||||
rospy.loginfo('')
|
||||
|
||||
# 当全局定位成功时才更新map2odom
|
||||
if fitness < LOCALIZATION_TH:
|
||||
if fitness > LOCALIZATION_TH:
|
||||
# T_map_to_odom = np.matmul(transformation, pose_estimation)
|
||||
T_map_to_odom = transformation
|
||||
|
||||
@@ -175,11 +159,10 @@ def global_localization(pose_estimation):
|
||||
|
||||
def initialize_global_map(pc_msg):
|
||||
global global_map
|
||||
global_map = pcl.PointCloud()
|
||||
global_map.from_array(msg_to_array(pc_msg).astype(np.float32))
|
||||
sor = global_map.make_voxel_grid_filter()
|
||||
sor.set_leaf_size(MAP_VOXEL_SIZE, MAP_VOXEL_SIZE, MAP_VOXEL_SIZE)
|
||||
global_map = sor.filter()
|
||||
|
||||
global_map = o3d.geometry.PointCloud()
|
||||
global_map.points = o3d.utility.Vector3dVector(msg_to_array(pc_msg)[:, :3])
|
||||
global_map = global_map.voxel_down_sample(MAP_VOXEL_SIZE)
|
||||
rospy.loginfo('Global map received.')
|
||||
|
||||
|
||||
@@ -202,8 +185,8 @@ def cb_save_cur_scan(pc_msg):
|
||||
pc_msg.fields[3], pc_msg.fields[7]]
|
||||
pc = msg_to_array(pc_msg)
|
||||
|
||||
cur_scan = pcl.PointCloud()
|
||||
cur_scan.from_array(pc.astype(np.float32))
|
||||
cur_scan = o3d.geometry.PointCloud()
|
||||
cur_scan.points = o3d.utility.Vector3dVector(pc[:, :3])
|
||||
|
||||
|
||||
def thread_localization():
|
||||
@@ -223,14 +206,14 @@ if __name__ == '__main__':
|
||||
FREQ_LOCALIZATION = 0.5
|
||||
|
||||
# The threshold of global localization,
|
||||
# only those scan2map-matching with lower fitness than LOCALIZATION_TH will be taken
|
||||
LOCALIZATION_TH = 0.2
|
||||
# only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken
|
||||
LOCALIZATION_TH = 0.95
|
||||
|
||||
# FOV(rad), modify this according to your LiDAR type
|
||||
FOV = 1.6
|
||||
|
||||
# The farthest distance(meters) within FOV
|
||||
FOV_FAR = 300
|
||||
FOV_FAR = 150
|
||||
|
||||
rospy.init_node('fast_lio_localization')
|
||||
rospy.loginfo('Localization Node Inited...')
|
||||
|
||||
Reference in New Issue
Block a user