mirrors/FAST_LIO_LOCALIZATION
1
0
Fork 0
mirror of https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git synced 2023-05-28 12:51:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Multi-scale global registration

Browse Source
This commit is contained in:
HViktorTsoi
2021-08-06 11:11:27 +08:00
parent 7283ff3793
commit 1364cc8416
1 changed files with 171 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

213
scripts/global_localization.py Normal file → Executable file
View File

@@ -1,25 +1,33 @@
#!/usr/bin/env python2
# coding=utf8
# !/usr/bin/env python2
from __future__ import print_function, division, absolute_import
import copy
import thread
import time
import open3d as o3d
import pcl
import rospy
import ros_numpy
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion
from nav_msgs.msg import Odometry
from sensor_msgs.msg import PointCloud2
import numpy as np
import tf
import tf.transformations
import multiprocessing
global_map = o3d.geometry.PointCloud()
global_map = None
initialized = False
T_map_to_odom = np.eye(4)
cur_scan = o3d.geometry.PointCloud()
cur_odom = None
cur_scan = None
def pose_to_mat(pose_msg):
return np.matmul(
tf.listener.xyz_to_mat44(pose_msg.pose.pose.position),
tf.listener.xyzw_to_mat44(pose_msg.pose.pose.orientation),
)
def msg_to_array(pc_msg):
@@ -31,61 +39,162 @@ def msg_to_array(pc_msg):
return pc
def publish_tf():
def transform_fusion():
br = tf.TransformBroadcaster()
while True:
rospy.sleep(0.01)
time.sleep(1 / FREQ_PUB_LOCALIZATION)
br.sendTransform(tf.transformations.translation_from_matrix(T_map_to_odom),
tf.transformations.quaternion_from_matrix(T_map_to_odom),
rospy.Time.now(),
'camera_init', 'map')
if cur_odom is not None:
# 发布全局定位的odometry
localization = Odometry()
T_odom_to_base_link = pose_to_mat(cur_odom)
T_map_to_base_link = np.matmul(T_map_to_odom, T_odom_to_base_link)
xyz = tf.transformations.translation_from_matrix(T_map_to_base_link)
quat = tf.transformations.quaternion_from_matrix(T_map_to_base_link)
localization.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
localization.twist = cur_odom.twist
localization.header.stamp = cur_odom.header.stamp
localization.header.frame_id = 'map'
localization.child_frame_id = 'body'
# rospy.loginfo_throttle(1, '{}'.format(np.matmul(T_map_to_odom, T_odom_to_base_link)))
pub_localization.publish(localization)
def global_localization(odometry_pose):
def registration_at_scale(pc_scan, pc_map, initial, scale):
sor = pc_scan.make_voxel_grid_filter()
sor.set_leaf_size(SCAN_VOXEL_SIZE * scale, SCAN_VOXEL_SIZE * scale, SCAN_VOXEL_SIZE * scale)
# 用初始解转换到对应坐标系
pc = np.array(sor.filter())
pc = np.column_stack([pc, np.ones(len(pc)).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
def inverse_se3(trans):
trans_inverse = np.eye(4)
# R
trans_inverse[:3, :3] = trans[:3, :3].T
# t
trans_inverse[:3, 3] = -np.matmul(trans[:3, :3].T, trans[:3, 3])
return trans_inverse
def publish_point_cloud(publisher, header, pc):
data = np.zeros(len(pc), dtype=[
('x', np.float32),
('y', np.float32),
('z', np.float32),
('intensity', np.float32),
])
data['x'] = pc[:, 0]
data['y'] = pc[:, 1]
data['z'] = pc[:, 2]
if pc.shape[1] == 4:
data['intensity'] = pc[:, 3]
msg = ros_numpy.msgify(PointCloud2, data)
msg.header = header
publisher.publish(msg)
def crop_global_map_in_FOV(pose_estimation):
global global_map, cur_odom
# 当前scan原点的位姿
T_odom_to_base_link = pose_to_mat(cur_odom)
T_map_to_base_link = np.matmul(pose_estimation, T_odom_to_base_link)
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.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
# 将视角内的地图点提取出来
# FOV_FAR>x>0 且角度小于FOV
indices = np.where(
(global_map_in_base_link[:, 0] > 0) &
(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))
# 发布fov内点云
header = cur_odom.header
header.frame_id = 'map'
publish_point_cloud(pub_submap, header, global_map_in_FOV.to_array()[::10])
return global_map_in_FOV
def global_localization(pose_estimation):
global global_map, cur_scan, T_map_to_odom
# 用icp配准
# print(global_map, cur_scan, T_map_to_odom)
rospy.loginfo('scan to map matching......')
rospy.loginfo('Global localization by scan-to-map matching......')
# TODO 这里注意线程安全
# 估计法线
scan_tobe_mapped = copy.copy(cur_scan)
scan_tobe_mapped.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
scan_tobe_mapped = scan_tobe_mapped.voxel_down_sample(SCAN_VOXEL_SIZE)
tic = time.time()
# 粗配准
icp_coarse = o3d.registration.registration_icp(
scan_tobe_mapped.voxel_down_sample(SCAN_VOXEL_SIZE * 5), global_map.voxel_down_sample(MAP_VOXEL_SIZE * 5),
MAP_VOXEL_SIZE * 5, odometry_pose,
o3d.registration.TransformationEstimationPointToPoint())
# 配准
icp_fine = o3d.registration.registration_icp(
scan_tobe_mapped, global_map,
MAP_VOXEL_SIZE, icp_coarse.transformation,
o3d.registration.TransformationEstimationPointToPoint())
print(icp_fine)
global_map_in_FOV = crop_global_map_in_FOV(pose_estimation)
# 粗配准
transformation, _ = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=5)
# 精配准
transformation, fitness = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=transformation,
scale=1)
toc = time.time()
rospy.loginfo('Time: {}'.format(toc - tic))
rospy.loginfo('')
# 当全局定位成功时才更新map2odom
if icp_fine.fitness > 0.9:
T_map_to_odom = icp_fine.transformation
if fitness < LOCALIZATION_TH:
# T_map_to_odom = np.matmul(transformation, pose_estimation)
T_map_to_odom = transformation
return True
else:
rospy.logwarn('Not match!!!!')
rospy.logwarn('{}'.format(transformation))
rospy.logwarn('fitness score:{}'.format(fitness))
return False
def initialize_global_map(pc_msg):
global global_map
global_map.points = o3d.utility.Vector3dVector(msg_to_array(pc_msg))
global_map.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
global_map.voxel_down_sample(MAP_VOXEL_SIZE)
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()
rospy.loginfo('Global map received.')
def cb_save_cur_odom(odom_msg):
global cur_odom
cur_odom = odom_msg
def cb_save_cur_scan(pc_msg):
global cur_scan
# 注意这里fastlio直接将scan转到odom系下了 不是lidar局部系
@@ -94,19 +203,21 @@ def cb_save_cur_scan(pc_msg):
pub_pc_in_map.publish(pc_msg)
# 转换为pcd
# 处理一下
# fastlio给的field有问题 处理一下
pc_msg.fields = [pc_msg.fields[0], pc_msg.fields[1], pc_msg.fields[2],
pc_msg.fields[4], pc_msg.fields[5], pc_msg.fields[6],
pc_msg.fields[3], pc_msg.fields[7]]
pc = msg_to_array(pc_msg)
cur_scan.points = o3d.utility.Vector3dVector(pc[:, :3])
cur_scan = pcl.PointCloud()
cur_scan.from_array(pc.astype(np.float32))
def thread_localization():
global T_map_to_odom
while True:
# 每隔一段时间进行全局定位
rospy.sleep(2)
rospy.sleep(1 / FREQ_LOCALIZATION)
# TODO 由于这里Fast lio发布的scan是已经转换到odom系下了 所以每次全局定位的初始解就是上一次的map2odom 不需要再拿odom了
global_localization(T_map_to_odom)
@@ -115,34 +226,52 @@ if __name__ == '__main__':
MAP_VOXEL_SIZE = 0.4
SCAN_VOXEL_SIZE = 0.1
# Global localization frequency (HZ)
FREQ_LOCALIZATION = 0.5
# tf and localization publishing frequency (HZ)
FREQ_PUB_LOCALIZATION = 50
# 全局定位的fitness预支
LOCALIZATION_TH = 0.2
# FOV内的最远距离
FOV_FAR = 300
# FOV范围(rad)
FOV = 1.6
rospy.init_node('fast_lio_localization')
rospy.loginfo('Localization Node Inited...')
# 发布定位消息
thread.start_new_thread(publish_tf, ())
thread.start_new_thread(transform_fusion, ())
# publisher
pub_pc_in_map = rospy.Publisher('/cur_scan_in_map', PointCloud2, queue_size=1)
# rospy.Subscriber('/livox/lidar/pc2', PointCloud2, cb_save_cur_scan, queue_size=1)
pub_submap = rospy.Publisher('/submap', PointCloud2, queue_size=1)
pub_localization = rospy.Publisher('/localization', Odometry, queue_size=1)
rospy.Subscriber('/cloud_registered', PointCloud2, cb_save_cur_scan, queue_size=1)
rospy.Subscriber('/Odometry', Odometry, cb_save_cur_odom, queue_size=1)
# 初始化全局地图
rospy.loginfo('Waiting for global map')
rospy.loginfo('Waiting for global map......')
initialize_global_map(rospy.wait_for_message('/map', PointCloud2))
# 初始化
while not initialized:
rospy.loginfo('Waiting for initial pose')
rospy.loginfo('Waiting for initial pose....')
# 等待初始位姿
pose_msg = rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
initial_pose = np.matmul(
tf.listener.xyz_to_mat44(pose_msg.pose.pose.position),
tf.listener.xyzw_to_mat44(pose_msg.pose.pose.orientation),
)
initialized = global_localization(initial_pose)
initial_pose = pose_to_mat(pose_msg)
if cur_scan:
initialized = global_localization(initial_pose)
else:
rospy.logwarn('First scan not received!!!!!')
rospy.loginfo('Initialized successfully!!!!!!')
rospy.loginfo('Initialize successfully!!!!!!')
# 开始定期全局定位
thread.start_new_thread(thread_localization, ())
# multiprocessing.Process(target=thread_localization, args=()).start()