livox CustomMsg、ros pointcloud2、open3d、numpy互转(python)

写在前面

• 本文内容
  不同格式的点云：livox CustomMsg、ros pointcloud2、open3d、numpy之间的互相转换
  主要是将不同的格式都转成numpy，然后用numpy再转成其他格式

• 平台/环境
  ros, ubuntu1804

• 转载请注明出处：
  https://blog.csdn.net/qq_41102371/article/details/139834499

代码

依赖

import rospy
import open3d as o3d
import numpy as np
from livox_ros_driver.msg import CustomMsg
from sensor_msgs.msg import PointCloud2
import ros_numpy
import copy

livox2numpy

将xyz,intensity转化成(n, 4)的numpy，n是点云的点数

def livox2numpy(msg: CustomMsg) -> np.array:
    msg_points = msg.points
    x = np.array([p.x for p in msg_points])
    y = np.array([p.y for p in msg_points])
    z = np.array([p.z for p in msg_points])
    intensity = np.array([p.reflectivity for p in msg_points])
    xyz = np.stack((x, y, z, intensity), axis=-1)

    return xyz

rospcd22numpy

rospcd2转numpy

    def rospcd22numpy(self, rospcd2: PointCloud2) -> np.array:
        pc_array = ros_numpy.numpify(rospcd2)
        pcd_np = np.zeros([len(pc_array), 4])
        pcd_np[:, 0] = pc_array['x']
        pcd_np[:, 1] = pc_array['y']
        pcd_np[:, 2] = pc_array['z']
        pcd_np[:, 3] = pc_array['intensity']
        print("pcd_np.shape: ", pcd_np.shape)
        return pcd_np

注意发布到rospcd2的时候，如果是pcl转的sensor_msgs::PointCloud2，那么pcl的格式最好是pcl::PointXYZI，不要用pcl::PointXYZINormal，不然订阅后使用rospcd22numpy时ros_numpy会报错

pcl::PointCloud<pcl::PointXYZI>::Ptr pcd(new pcl::PointCloud<pcl::PointXYZI>());

....

sensor_msgs::PointCloud2 pcd_msg;
pcl::toROSMsg(*pcd, pcd_msg);

open3d2numpy

def open3d2numpy(pcd_o3d: o3d.geometry.PointCloud) -> np.array:
    print("convert pcd size: ", len(pcd_o3d.points))
    pcd_np = np.asarray(pcd_o3d.points)
    if pcd_o3d.has_colors():
        colos = np.asarray(pcd_o3d.colors)
        pcd_np = np.concatenate((pcd_np, colos), axis=1)
    return pcd_np

numpy2open3d

def numpy2open3d(pcd_np: np.array) -> o3d.geometry.PointCloud:
    pcd_o3d = o3d.geometry.PointCloud()
    print("numpy2open3d")
    pcd_xyz = copy.deepcopy(pcd_np[:, :3])
    pcd_o3d.points = o3d.utility.Vector3dVector(pcd_xyz)
    if pcd_np.shape[1] == 4:
        color = copy.deepcopy(pcd_np[:, 3]).reshape(
            pcd_np.shape[0], 1)/255.0
        color = np.repeat(color, 3, axis=1)
        print("color.shape: ", color.shape)
        pcd_o3d.colors = o3d.utility.Vector3dVector(color)

    if pcd_np.shape[1] == 6:
        color = copy.deepcopy(pcd_np[:, 3:])
        print("color.shape: ", color.shape)
        pcd_o3d.colors = o3d.utility.Vector3dVector(color)
        
    return pcd_o3d

numpy2rospcd2

def numpy2rospcd(pcd_np: np.array, frame_id: str = "livox_frame") -> PointCloud2:
    data = np.zeros(pcd_np.shape[0],
                    dtype=[
                        ('x', np.float32),
                        ('y', np.float32),
                        ('z', np.float32),
                        ('intensity', np.float32),
    ])
    data['x'] = pcd_np[:, 0]
    data['y'] = pcd_np[:, 1]
    data['z'] = pcd_np[:, 2]
    data['intensity'] = pcd_np[:, 3]
    rospcd2 = ros_numpy.msgify(PointCloud2, data)
    rospcd2.header.frame_id = frame_id
    rospcd2.header.stamp = rospy.Time().now()
    rospcd2.height = 1
    rospcd2.width = pcd_np.shape[0]
    print("convert points: ", rospcd2.width)
    return rospcd2

参考

https://www.open3d.org/docs/release/tutorial/geometry/working_with_numpy.html
https://wiki.ros.org/livox_ros_driver

完

主要做激光/影像三维重建，配准、分割等常用点云算法，熟悉open3d、pcl等开源点云库，技术交流、咨询可私信