Groundtruth建图

gt_mapping.py

# !/usr/bin/env python2
# coding=utf-8

from __future__ import absolute_import, print_function
import os

import numpy as np
import numpy.linalg as LA
import open3d as o3d
import message_filters

import rospy
import ros_numpy
from nav_msgs.msg import Odometry
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2
from autolab_core import RigidTransform

semantic_mapping = {  # bgr
    0: [0, 0, 0],  # "unlabeled", and others ignored
    1: [245, 150, 100],  # "car"
    2: [245, 230, 100],  # "bicycler"
    3: [150, 60, 30],  # "motorcycle"
    4: [180, 30, 80],  # "truck"
    5: [255, 0, 0],  # "other-vehicle"
    6: [30, 30, 255],  # "person"
    7: [200, 40, 255],  # "bicyclist"
    8: [90, 30, 150],  # "motorcyclist"
    9: [255, 0, 255],  # "road"
    10: [255, 150, 255],  # "parking"
    11: [75, 0, 75],  # "sidewalk"
    12: [75, 0, 175],  # "other-ground"
    13: [0, 200, 255],  # "building"
    14: [50, 120, 255],  # "fence"
    15: [0, 175, 0],  # "vegetation"
    16: [0, 60, 135],  # "trunk"
    17: [80, 240, 150],  # "terrain"
    18: [150, 240, 255],  # "pole"
    19: [0, 0, 255]  # "traffic-sign"
}

root = "/media/hyx/701e2c63-271c-466f-a047-f683746987da/Segmantic_kitti/dataset/sequences"
sequence = "07"

global_map = o3d.geometry.PointCloud()


# def load_calib(calib_file_path):
#     """
#     load calibration file(KITTI object format)
#     :param calib_file_path:
#     :return:
#     """
#     calib_file = open(calib_file_path, 'r').readlines()
#     calib_file = [line
#                       .replace('Tr_velo_to_cam', 'Tr_velo_cam')
#                       .replace('Tr:', 'Tr_velo_cam:')
#                       .replace('R0_rect', 'R_rect')
#                       .replace('\n', '')
#                       .replace(':', '')
#                       .split(' ')
#                   for line in calib_file]
#     calib_file = {line[0]: [float(item) for item in line[1:] if item != ''] for line in calib_file if len(line) > 1}
#     return calib_file


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


def callback(pc_msg, odom_msg):
    pc_array = ros_numpy.numpify(pc_msg)

    if len(pc_array.shape) == 2:
        pc = np.zeros((pc_array.shape[0] * pc_array.shape[1], 4))
    else:
        pc = np.zeros((pc_array.shape[0], 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)

    rotation_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])

    T_qua2rota = RigidTransform(rotation_quaternion, translation)

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

    print(RT_matrix)
    # # 1. translate points in camera frame
    # pc = rigid_translate(pc, extrinsic)

    # convert to world frame
    pc = rigid_translate(pc, RT_matrix)

    points = o3d.geometry.PointCloud()
    points.points = o3d.utility.Vector3dVector(pc[:, :3])

    points = points.voxel_down_sample(voxel_size=0.8)

    global global_map
    global_map += points


if __name__ == '__main__':
    rospy.init_node('RS128_mapping')

    # # load calibration
    # calib = load_calib(os.path.join(root, sequence, "calib.txt"))

    # # lidar to camera extrinsic
    # extrinsic = np.row_stack([np.array(calib['Tr_velo_cam']).reshape(3, 4), [0, 0, 0, 1]])
    # print(extrinsic)

    # 订阅
    pc_subscriber = message_filters.Subscriber('/rslidar_points', PointCloud2)
    odom_subscriber = message_filters.Subscriber('/odometry_gt', Odometry)

    # 时间同步器
    ts = message_filters.ApproximateTimeSynchronizer(
        [pc_subscriber, odom_subscriber],
        queue_size=20, slop=0.05
    )
    ts.registerCallback(callback)

    rospy.loginfo('INIT...')
    rospy.spin()

    print("Done\n")
    global_map = global_map.voxel_down_sample(voxel_size=1.0)
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    vis.add_geometry(global_map)

    vis.run()
    vis.destroy_window()