LIO-SAM里程计建图

odomMapping.py

# !/usr/bin/env python2
# coding=utf-8
from __future__ import absolute_import, print_function

import os

import ros_numpy
import rospy
import message_filters

from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2, Image, PointField
import numpy as np
import numpy.linalg as LA
import transforms3d
import open3d as o3d
import cv2
from cv_bridge import CvBridge, CvBridgeError

frame_id = 0


def vis(pc):
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(pc[:, :3])
    pcd.colors = o3d.utility.Vector3dVector(pc[:, 4:] / 255)
    save_file = os.path.join("/home/hyx/RGBMap_30m.pcd")
    o3d.io.write_point_cloud(save_file, pcd)

    # line_pcd = o3d.geometry.LineSet()
    # position = poses[:, :3, 3]
    # plen = position.shape[0]
    # line_pcd.lines = o3d.utility.Vector2iVector(line_index)
    # line_pcd.points = o3d.utility.Vector3dVector(position)
    # line_pcd.colors = o3d.utility.Vector3dVector(np.asarray([np.zeros(plen), np.ones(plen) * 255, np.zeros(plen)]).T)

    # global frame_id

    # o3d.io.write_point_cloud(os.path.join(root, 'RGB-Points', '{:06d}.pcd'.format(frame_id)), pcd)
    # print("Saving--- " + root + "frame_id = ", frame_id)
    # frame_id += 1

    o3d_vis = o3d.visualization.Visualizer()
    o3d_vis.create_window()
    o3d_vis.add_geometry(pcd)
    # o3d_vis.add_geometry(line_pcd)

    opt = o3d_vis.get_render_option()
    opt.background_color = np.asarray([0, 0, 0])
    opt.point_size = 2
    o3d_vis.run()
    o3d_vis.destroy_window()


def get_RT_matrix(odom_msg):
    quaternion = np.asarray(
        [odom_msg.pose.pose.orientation.w, odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y,
         odom_msg.pose.pose.orientation.z])
    translation = np.asarray(
        [odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, odom_msg.pose.pose.position.z])

    rotation = transforms3d.quaternions.quat2mat(quaternion)

    # print(T_qua2rota)
    RT_matrix = np.eye(4)
    RT_matrix[:3, :3] = rotation
    RT_matrix[:3, 3] = translation.T

    # RT_matrix = np.matmul(R_axes, RT_matrix)

    return RT_matrix


def undistort_projection(points, intrinsic_matrix, extrinsic_matrix):
    points = np.column_stack([points, np.ones_like(points[:, 0])])

    # 外参矩阵
    points = np.matmul(extrinsic_matrix, points.T, )

    # 内参矩阵
    points = np.matmul(intrinsic_matrix, points[:3, :], ).T

    # 深度归一化
    points[:, :2] /= points[:, 2].reshape(-1, 1)

    return points


def back_projection(points, intrinsic_matrix, extrinsic_matrix):
    # 还原深度
    points[:, :2] *= points[:, 2].reshape(-1, 1)

    # 还原相平面相机坐标
    points[:, :3] = np.matmul(LA.inv(intrinsic_matrix), points[:, :3].T).T

    # 还原世界坐标
    # 旋转平移矩阵
    R, T = extrinsic_matrix[:3, :3], extrinsic_matrix[:3, 3]
    points[:, :3] = np.matmul(LA.inv(R), points[:, :3].T - T.reshape(-1, 1)).T

    return points


def img_to_pc(pc, img, extrinsic_matrix, intrinsic_matrix, distortion, root=""):
    # project pointcloud to image
    # print(pc.shape)
    projection_points = undistort_projection(pc[:, :3], intrinsic_matrix, extrinsic_matrix)
    # print(projection_points.shape)
    projection_points = np.column_stack([np.squeeze(projection_points), pc[:, 3:]])
    # print(projection_points.shape)

    # crop
    projection_points = projection_points[np.where(
        (projection_points[:, 0] > 0) &
        (projection_points[:, 0] < img.shape[1]) &
        (projection_points[:, 1] > 0) &
        (projection_points[:, 1] < img.shape[0])
    )]

    # depth map projection
    depth_map = np.zeros_like(img, dtype=np.float32)
    depth_map[np.int_(projection_points[:, 1]), np.int_(projection_points[:, 0]), 0] = projection_points[:, 2]
    depth_map[np.int_(projection_points[:, 1]), np.int_(projection_points[:, 0]), 1] = projection_points[:, 3]

    available_depth_indices = np.where(depth_map[..., 0] > 0)
    projection_points = np.row_stack([available_depth_indices[1], available_depth_indices[0],
                                      depth_map[available_depth_indices][..., 0],
                                      depth_map[available_depth_indices][..., 1]]).T

    # 图像点云深度匹配
    RGB = img[np.int_(projection_points[:, 1]), np.int_(projection_points[:, 0]), :]
    # print(RGB)

    # print(projection_points.shape)
    projection_points = np.column_stack([projection_points, RGB])
    # print(projection_points)

    # back projection
    pc = back_projection(projection_points, intrinsic_matrix, extrinsic_matrix)

    # vis(pc)

    # visualize_colored_pointcloud(pc)
    return pc


def rigid_translate(pc_input, extrinsic):
    # projection
    scan = np.row_stack([pc_input[:, :3].T, np.ones_like(pc_input[:, 0])])
    scan = np.matmul(extrinsic, scan)
    points = np.row_stack([scan[:3, :], pc_input[:, 3:].T]).T
    return points


def callback(odom_msg, pc_msg, img_msg):
    global frame_id
    print(frame_id)
    frame_id += 1

    odom = get_RT_matrix(odom_msg)

    birdge = CvBridge()
    cv_image = birdge.imgmsg_to_cv2(img_msg, 'rgb8')
    img = cv2.copyMakeBorder(cv_image, 400, 436, 0, 0, cv2.BORDER_CONSTANT, value=[0, 0, 0])
    img = cv2.undistort(img, intrinsic_matrix, distortion)

    pc_array = ros_numpy.numpify(pc_msg)
    if len(pc_array.shape) == 1:
        pc = np.zeros((pc_array.shape[0], 4))
        # 解析点格式
        pc[:, 0] = pc_array['x']
        pc[:, 1] = pc_array['y']
        pc[:, 2] = pc_array['z']
        pc[:, 3] = pc_array['intensity']
    elif len(pc_array.shape) == 2:
        pc = np.zeros((pc_array.shape[0] * pc_array.shape[1], 4))
        # 解析点格式
        pc[:, 0] = pc_array['x'].reshape(-1)
        pc[:, 1] = pc_array['y'].reshape(-1)
        pc[:, 2] = pc_array['z'].reshape(-1)
        pc[:, 3] = pc_array['intensity'].reshape(-1)
    else:
        raise Exception('Unsupported Pointcloud2 Format!!!')

    # process pc
    pc = pc[np.where(
        (pc[:, 0] > 0) &
        (abs(pc[:, 1]) < 30) &
        (pc[:, 2] > -5)
    )]

    pc = img_to_pc(pc, img, extrinsic_matrix, intrinsic_matrix, distortion, root)
    pc = rigid_translate(pc, odom)

    mapping.append(pc)


if __name__ == '__main__':
    rospy.init_node('odomMapping')
    rospy.loginfo("Mapping with rgb-points using odometry from LIO-SAM..")

    root = "/media/hyx/701e2c63-271c-466f-a047-f683746987da/2021.1.29"
    intrinsic_matrix = np.loadtxt(os.path.join(root, 'parameter', 'intrinsic'))
    distortion = np.loadtxt(os.path.join(root, 'parameter', 'distortion'))
    extrinsic_matrix = np.loadtxt(os.path.join(root, 'parameter', 'extrinsic'))

    mapping = []

    subOdom = message_filters.Subscriber('/lio_sam/mapping/odometry', Odometry)
    subPC = message_filters.Subscriber('/lio_sam/deskew/cloud_deskewed', PointCloud2)
    # subPC = message_filters.Subscriber('/rslidar_points', PointCloud2)
    subImg = message_filters.Subscriber('/camera/image_color', Image)

    ts = message_filters.ApproximateTimeSynchronizer([subOdom, subPC, subImg], 20, 0.2, allow_headerless=True)
    ts.registerCallback(callback)

    rospy.spin()

    global_pc = np.row_stack(mapping)
    vis(global_pc)