las显示和转bag文件

显示las文件

import laspy
import numpy as np
import open3d as o3d

# 读取LAS文件
las = laspy.read("frz.las")

# 提取点云坐标
points = np.vstack((las.x, las.y, las.z)).transpose()

# 创建Open3D点云对象
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

# 添加颜色信息（如果有RGB数据）
if hasattr(las, 'red') and hasattr(las, 'green') and hasattr(las, 'blue'):
    colors = np.vstack((las.red / 65535, las.green / 65535, las.blue / 65535)).transpose()
    pcd.colors = o3d.utility.Vector3dVector(colors)

# 可视化点云
o3d.visualization.draw_geometries([pcd])

las转bag文件

import laspy
import numpy as np
import open3d as o3d
import rosbag
import rospy
from sensor_msgs.msg import PointField
from sensor_msgs.point_cloud2 import create_cloud

def read_las_file(las_file_path):
    print(f"Start to load LAS file {las_file_path}")
    try:
        las = laspy.read(las_file_path)
        points = np.vstack((las.x, las.y, las.z)).transpose()
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(points)
        if hasattr(las, 'red') and hasattr(las, 'green') and hasattr(las, 'blue'):
            colors = np.vstack((las.red / 65535, las.green / 65535, las.blue / 65535)).transpose()
            pcd.colors = o3d.utility.Vector3dVector(colors)
        print(f"Complete loading LAS file {las_file_path}")
        return pcd
    except Exception as e:
        print(f"Error loading LAS file: {e}")
        return None

def pcd_to_bag(pcd, bag_file_path):
    if pcd is None:
        print("No point cloud data to save.")
        return

    print(f"Start to save bag file {bag_file_path}")
    
    # Initialize ROS node (if necessary)
    try:
        rospy.init_node("pcd_to_bag_node", anonymous=True)
    except rospy.exceptions.ROSException as e:
        print(f"ROS node already initialized: {e}")

    # Create ROS message header
    header = rospy.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "map"

    # Define point cloud fields
    fields = [
        PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
        PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
        PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
        PointField(name='r', offset=12, datatype=PointField.UINT8, count=1),
        PointField(name='g', offset=13, datatype=PointField.UINT8, count=1),
        PointField(name='b', offset=14, datatype=PointField.UINT8, count=1)
    ]

    # Convert Vector3dVector to NumPy array
    points = np.asarray(pcd.points)

    # Handle color data
    if pcd.has_colors():
        colors = (np.asarray(pcd.colors) * 255).astype(np.uint8)  # Scale and convert to uint8
    else:
        colors = np.ones((len(points), 3), dtype=np.uint8) * 255  # Default to white

    # Create a structured array for the point cloud data
    dtype = np.dtype([
        ('x', np.float32),
        ('y', np.float32),
        ('z', np.float32),
        ('r', np.uint8),
        ('g', np.uint8),
        ('b', np.uint8)
    ])
    structured_points = np.empty(len(points), dtype=dtype)
    structured_points['x'] = points[:, 0].astype(np.float32)
    structured_points['y'] = points[:, 1].astype(np.float32)
    structured_points['z'] = points[:, 2].astype(np.float32)
    structured_points['r'] = colors[:, 0]
    structured_points['g'] = colors[:, 1]
    structured_points['b'] = colors[:, 2]

    # Create PointCloud2 message
    print("Creating PointCloud2 message...")
    cloud_msg = create_cloud(header, fields, structured_points)
    print("PointCloud2 message created.")

    # Write to bag file
    try:
        with rosbag.Bag(bag_file_path, "w") as bag:
            bag.write("/point_cloud", cloud_msg)
        print(f"Bag file saved to {bag_file_path}")
    except Exception as e:
        print(f"Error saving bag file: {e}")

if __name__ == '__main__':
    las_file_path = "frz.las"
    bag_file_path = "frz.bag"
    pcd = read_las_file(las_file_path)
    pcd_to_bag(pcd, bag_file_path)

• 运行环境：py3.8 + ros noetic