ROS_open3d—点云叠帧可视化

点云可视化

 pip3 install open3d-python

定位和pose

 连续多帧显示点云，需要 点云文件 和 定位信息（IMU惯导信息）
   时间对齐
   空间对齐
    时间对齐：就是说我们哪一个时间下录制的pcd要和对应时间下旋转矩阵相乘（用的只是他们的差值小于0.01，就认为是匹配的）
    空间对齐：1.看看静态物体（比如杆子）有没有对齐   
              2.看看地面有没有变厚
  细节：
     pcd
	 pose： 自车转世界坐标系 的4×4的矩阵

参考的转换方式

验证转换的情况的方式
  01.根据提供的自车坐标系的pcd和pose 转到世界坐标系pcd
  02.根据世界坐标系的pcd可视化
  
验证转换的情况的方式
  01.根据提供的自车坐标系的pcd和pose 转到世界坐标系pcd
  02.世界坐标系的pcd根据某一帧的pose进行转换
  03.可视化转换后的pcd

可视化代码

#!/usr/bin/python3
# -*- coding: utf-8 -*- 

import os
import open3d as o3d
import numpy as np
 
def vis_pcds(pcd_files):
    pcds = []
    pcds_p = []
    files = os.listdir(pcd_files)
    for f in files:
        pcd_path = os.path.join(pcd_files,f)
        pcds_p.append(pcd_path)
        pcd = o3d.io.read_point_cloud(pcd_path)
        # 降采样
        pcd_dow = pcd.voxel_down_sample(voxel_size=0.2)
        pcds.append(pcd_dow)
 
    for i,pcd_element  in enumerate(pcds_p):
        if i == 0:
            pcd_base = o3d.io.read_point_cloud(pcd_element)
            o3d.io.write_point_cloud("pcd_20_20.pcd",pcd_base)
        else:
            pcd_base = o3d.io.read_point_cloud("pcd_20_20.pcd")
            pcd_mid = pcd_base + o3d.io.read_point_cloud(pcd_element)
            o3d.io.write_point_cloud("pcd_20_20.pcd",pcd_mid)
    pcd_last = o3d.io.read_point_cloud("pcd_20_20.pcd")
	o3d.visualization.draw_geometries([pcd_last], window_name="拼接20个点云",
                                              width=1024, height=768,
                                              left=50, top=50,
                                              mesh_show_back_face=False)

 
if __name__ == '__main__':
    exp_pcd_file=r""
    vis_pcds(exp_pcd_file)

代码说明

 # 以下代码实现了与以下相同的效果:
 # o3d.visualization.draw_geometries([pcd])    geometry_list (List[open3d.geometry.Geometry]) – 待显示的pcd点云格式列表
 
 def custom_draw_geometry(pcd):
     vis = o3d.visualization.Visualizer()
     vis.create_window()
     vis.add_geometry(pcd)
     vis.run()
     vis.destroy_window()
	 
# 创建一个PointCloud对象 pcd = o3d.geometry.PointCloud()	 

点云

###msg数据格式是sensor_msgs.PointCloud2
  这是一个一维或二维的数组，数据经过处理，无法直接读取点坐标XYZ信息，需要一步读取操作
  PointCloud2的data是序列化后的数据
  gen = point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True)
  cloud = point_cloud2.read_points_numpy(msg, field_names=['x', 'y', 'z', "intensity"], skip_nans=True)
    ###https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs_py/sensor_msgs_py/point_cloud2.py
import open3d as o3d
open3d Warning  pointcloud contains intensity attribute which is not supported by PCD IO Only

pcd = o3d.t.io.read_point_cloud(pc_file)
points = pcd.point
xyz = points['positions'].numpy()
intensity = points['intensity'].numpy()

	
pcd = o3d.t.geometry.PointCloud()
pcd.point['positions'] = o3d.core.Tensor(xyz,dtype=o3d.core.Dtype.Float32)
pcd.point['intensity'] = o3d.core.Tensor(intensity.numpy(),dtype=o3d.core.Dtype.Float32)
o3d.t.io.write_point_cloud( save_file, pcd, write_ascii=False)	


python3 -c '''
import open3d
print(open3d.__version__)
'''

代码示例

# -*- coding: utf-8 -*-


import os
import argparse
import cv2
import open3d as o3d
import numpy as np
import rosbag
import sensor_msgs.point_cloud2 as point_cloud2
from rosbags.image import message_to_cvimage
from tqdm import tqdm


def extract_bag_pcd(bag_path):
    bag = rosbag.Bag(bag_path)
    pcd_path = os.path.join(os.path.splittext(bag_path)[0],"pcd")
    if not os.path.exists(pcd_path):
        os.makedirs(pcd_path)
    topic = ['/points']  
    for topic, msg, t in tqdm(bag.read_messages(topics=topic)):
        msg_stamp = str(msg.header.stamp)
        pcd_file = os.path.join(pcd_path, '{}.{}.pcd'.format(msg_stamp[0:10], msg_stamp[10:-3]))
        cloud  = np.asarray(list(point_cloud2.read_points(msg, field_names=['x', 'y', 'z', "intensity"]))) 
        #cloud  = np.asarray(list(point_cloud2.read_points(msg, field_names=['x', 'y', 'z', "intensity"])))           
        pcd_obj = o3d.t.geometry.PointCloud()
        pcd_obj.point['positions'] = o3d.core.Tensor(cloud[:, :3],dtype=o3d.core.Dtype.Float32)
        pcd_obj.point['intensity'] = o3d.core.Tensor(np.asarray(cloud[:, 3]),dtype=o3d.core.Dtype.Float32)
        o3d.t.io.write_point_cloud( pcd_file, pcd_obj, write_ascii=False)
    bag.close()

def extract_bag_image(bag_path):
    bag = rosbag.Bag(bag_path)
    image_path = os.path.join(os.path.splittext(bag_path)[0],"image")
    if not os.path.exists(image_path):
        os.makedirs(image_path)
    topics = ['/tt/image_raw','/tt/image_raw']
    for topic, msg, t in tqdm(bag.read_messages(topics=topics)):
        msg_stamp = str(msg.header.stamp)
        image_dir = os.path.join(image_path, topic.split('/')[1])
        if not os.path.exists(image_dir):
            os.makedirs(image_dir)
        image_file = os.path.join(image_dir, '{}.{}.jpg'.format(msg_stamp[0:10], msg_stamp[10:-3]))
        cv_image = message_to_cvimage(msg)
        cv2.imwrite(image_file, cv_image)
    bag.close()

if __name__ == "__main__":
    parser = argparse.ArgumentParser( description='请将bag目录整理')
    parser.add_argument('--bag_path', type=str, default='/data/1.bag', help='bag path')                                    
    args = parser.parse_args()
    exp_bag = args.bag_path
    extract_pcd(exp_bag)
    extract_image(exp_bag)

参考

 Python: 用open3D库，连续多帧显示点云  https://blog.csdn.net/melally/article/details/126116894
 http://www.open3d.org/docs/release/