激光雷达不够用,怎么办?Ubuntu如何用一个激光雷达实现两个激光雷达的扫描点云效果?点云配准ICP,点云拼接、话题转换、ROS重录制bag包。

1.首先至少得有一个激光雷达,如果没有的话,可以考虑花呗买一个,毕竟研究这东西,有个实物还是比较稳妥,这里我选择的是Livox Mid-360,哈哈哈,公司的,大概长这样:

2. 比如我们想要用激光雷达去扫描一个货车,由于激光雷达扫到的面有限,无法获得具体的形状比如:

      单独从上述图片不能很好的知道货车的三维形状,怎么办,但是如果有两个激光雷达进行拼接就好了,按照这个思路,首先分别录制上述两个图片所示的bag包。

3.首先对其中一个bag进行重新录制,因为我们只有一个激光雷达,其ip地址是一样的,就算换不同的位置录制,其bag包话题不能自动更改,对于我这个雷达,其话题一直是/livox/lidar.呢么我们就要想办法给他转成其它话题,比如我转到/livox/lidar2,可以先启动核心,循环播放自己已有激光雷达的bag包,然后运行以下代码:

roscore

rosbag play xxx.bag -l

import rospy
from sensor_msgs.msg import PointCloud2

class PointCloudRelay:
    def __init__(self):
        # 初始化节点
        rospy.init_node('point_cloud_relay', anonymous=True)
        
        # 设置固定的订阅和发布话题名称
        self.input_topic = '/livox/lidar'
        self.output_topic = '/livox/lidar2'
        
        # 创建订阅者和发布者
        self.point_cloud_sub = rospy.Subscriber(self.input_topic, PointCloud2, self.callback)
        self.point_cloud_pub = rospy.Publisher(self.output_topic, PointCloud2, queue_size=10)
        
        rospy.loginfo(f"订阅话题: {self.input_topic}")
        rospy.loginfo(f"发布话题: {self.output_topic}")

    def callback(self, msg):
        # 收到点云消息时,直接发布出去
        rospy.loginfo("收到点云数据,重新发布中...")
        self.point_cloud_pub.publish(msg)

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    try:
        relay = PointCloudRelay()
        relay.run()
    except rospy.ROSInterruptException:
        pass

4.这样当前bag包的话题/livox/lidar就转成了/livox/lidar2,然后运行以下命令录制该话题为一个新的bag包

rosbag  record  -o   包名.bag   /话题topic

rosbag record -o   11.bag   /livox/lidar2

5.此时你已经拥有两个激光雷达的数据了,一个话题为 /livox/lidar的bag包和一个话题为/livox/lidar2的bag包,呢么该如何对此点云进行拼接呢,接下来先对bag包生成pcd文件,对pcd文件利用icp算法进行对准,其中初始矩阵可以通过两个测量位置进行推算旋转平移矩阵,可简单,百度一下就行,比如旋转平移矩阵是一种将物体在空间中进行旋转和移动的数学表示,通常用于三维几何变换。它将旋转矩阵(3x3)和位移向量(3x1)结合成一个4x4的矩阵,其中旋转矩阵描述物体的旋转,位移向量表示平移,我百度的。然后运行以下代码进行标定:

import open3d as o3d
import numpy as np

def load_point_cloud(file_path):
    """加载 .pcd 格式的点云文件"""
    point_cloud = o3d.io.read_point_cloud(file_path)
    if point_cloud.is_empty():
        raise ValueError(f"点云文件 {file_path} 为空或加载失败。")
    return point_cloud

def preprocess_point_cloud(pcd, voxel_size):
    """
    预处理点云以加速配准。
    
    参数:
    - pcd: 输入的点云
    - voxel_size: 体素化大小,用于法向量估计
    
    返回:
    - 体素化并估计法向量后的点云
    """
    pcd = pcd.voxel_down_sample(voxel_size)
    pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30))
    return pcd

def apply_icp(source, target, threshold=0.02, init_transformation=None, use_point_to_plane=False):
    """
    使用 ICP 进行点云配准。
    
    参数:
    - source: 源点云 (o3d.geometry.PointCloud)
    - target: 目标点云 (o3d.geometry.PointCloud)
    - threshold: 配准的距离阈值
    - init_transformation: 初始对齐矩阵 (4x4)
    - use_point_to_plane: 是否使用点到平面 ICP 方法 (默认 False)
    
    返回:
    - 精确配准后的变换矩阵 (4x4)
    """
    print("运行 ICP 配准...")
    
    # 选择 ICP 估计方法:点到点或点到平面
    estimation_method = o3d.pipelines.registration.TransformationEstimationPointToPoint()
    if use_point_to_plane:
        estimation_method = o3d.pipelines.registration.TransformationEstimationPointToPlane()
    
    # 多尺度 ICP 配准
    max_iterations = [60, 30, 10]  # 多尺度迭代次数
    thresholds = [threshold * 5, threshold * 2, threshold]  # 多尺度阈值
    transformation = init_transformation
    
    for scale, max_iter, th in zip([5, 2, 1], max_iterations, thresholds):
        print(f"Scale {scale}: 距离阈值 = {th}, 最大迭代次数 = {max_iter}")
        
        reg_p2p = o3d.pipelines.registration.registration_icp(
            source, target, th, transformation, estimation_method,
            o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=max_iter)
        )
        
        transformation = reg_p2p.transformation
        print(f"Scale {scale} 结果变换矩阵:\n", transformation)
    
    return transformation

def main():
    # 加载点云文件 (替换为实际路径)
    source_pcd_file = "/home/xsl/Downloads/pcl点云处理学习/ct货车车厢识别/pcd1/1.pcd"  # 源点云文件路径
    target_pcd_file = "/home/xsl/Downloads/pcl点云处理学习/ct货车车厢识别/pcd2/2.pcd"  # 目标点云文件路径

    source_cloud = load_point_cloud(source_pcd_file)
    target_cloud = load_point_cloud(target_pcd_file)

    # 预处理点云以估计法向量
    voxel_size = 0.2  # 根据点云密度调整体素大小
    source_cloud = preprocess_point_cloud(source_cloud, voxel_size)
    target_cloud = preprocess_point_cloud(target_cloud, voxel_size)

    # 可视化初始状态
    print("初始点云对齐可视化")
    o3d.visualization.draw_geometries([source_cloud.paint_uniform_color([1, 0, 0]), 
                                       target_cloud.paint_uniform_color([0, 1, 0])])

    # 初始变换矩阵
    initial_transformation = np.array([
        [0.319, 0.947,  0.016, 5.343],
        [-0.948,  0.319,  0.004,  9.281],
        [-0.001, -0.017,  1, -0.296],
        [0,  0,  0,  1]
    ])
    # ICP 配准
    transformation_matrix = apply_icp(source_cloud, target_cloud, threshold=0.5, 
                                      init_transformation=initial_transformation, use_point_to_plane=True)

    # 将源点云变换到目标点云坐标系
    source_cloud.transform(transformation_matrix)

    # 可视化配准结果
    print("配准结果可视化")
    o3d.visualization.draw_geometries([source_cloud.paint_uniform_color([1, 0, 0]), 
                                       target_cloud.paint_uniform_color([0, 1, 0])])

    # 保存结果
    np.savetxt("transformation_matrix.txt", transformation_matrix, fmt="%.6f")
    print("外参矩阵已保存到 transformation_matrix.txt") 

if __name__ == "__main__":
    main()

会生成点云配准前后的对比效果,比如:

 也会生成相应的旋转平移矩阵,因为是多尺度,所以越往下配准精度较高,结果如下图所示:

 6.得到这个旋转平移矩阵,有大用,因为可以以这个为依据将其中一个点云数据配准拼接到另一个点云,怎么做,很简单,修改以下代码中的旋转平移矩阵,为你自己的bag包用icp跑出来的旋转平移矩阵。代码如下:

import rospy
import numpy as np
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
import std_msgs.msg

class PointCloudMerger:
    def __init__(self):
        # 初始化节点
        rospy.init_node('point_cloud_merger', anonymous=True)
        
        # 设置固定的订阅和发布话题名称
        self.input_topic1 = '/livox/lidar'
        self.input_topic2 = '/livox/lidar2'
        self.output_topic = '/merged_point_cloud'
        
        # 变换矩阵
        self.transformation_matrix = np.array([
    	[0.296464, 0.954431, -0.001073, 5.280927],
    	[-0.955159, 0.296485, 0.015795, 9.223657],
    	[0.015717, -0.004231, 1.000011, -0.151635],
    	[0.000000, 0.000000, 0.000000, 1.000000]
	])
        
        # 初始化点云数据缓存
        self.point_cloud1 = None
        self.point_cloud2 = None
        
        # 创建订阅者和发布者
        self.point_cloud_sub1 = rospy.Subscriber(self.input_topic1, PointCloud2, self.callback_point_cloud1)
        self.point_cloud_sub2 = rospy.Subscriber(self.input_topic2, PointCloud2, self.callback_point_cloud2)
        self.point_cloud_pub = rospy.Publisher(self.output_topic, PointCloud2, queue_size=10)
        
        rospy.loginfo(f"订阅话题: {self.input_topic1} 和 {self.input_topic2}")
        rospy.loginfo(f"发布话题: {self.output_topic}")

    def callback_point_cloud1(self, msg):
        # 缓存第一个点云消息
        self.point_cloud1 = msg
        self.try_publish_merged_cloud()

    def callback_point_cloud2(self, msg):
        # 缓存第二个点云消息
        self.point_cloud2 = msg
        self.try_publish_merged_cloud()

    def transform_point_cloud(self, point_cloud_msg, transformation_matrix):
        """使用外参矩阵转换点云坐标"""
        # 转换为点云坐标的生成器
        points = pc2.read_points(point_cloud_msg, field_names=("x", "y", "z"), skip_nans=True)
        
        # 应用外参矩阵进行坐标变换
        transformed_points = [
            tuple(np.dot(transformation_matrix, np.array([x, y, z, 1.0]))[:3]) for x, y, z in points
        ]
        
        # 创建新的点云消息
        header = std_msgs.msg.Header()
        header.stamp = rospy.Time.now()
        header.frame_id = point_cloud_msg.header.frame_id
        return pc2.create_cloud_xyz32(header, transformed_points)

    def try_publish_merged_cloud(self):
        # 如果两个点云数据均已接收到,进行合并并发布
        if self.point_cloud1 and self.point_cloud2:
            rospy.loginfo("正在合并点云...")
            
            # 转换第二个点云到第一个点云的坐标系
            transformed_cloud2 = self.transform_point_cloud(self.point_cloud2, self.transformation_matrix)
            
            # 合并两个点云
            points1 = list(pc2.read_points(self.point_cloud1, field_names=("x", "y", "z"), skip_nans=True))
            points2 = list(pc2.read_points(transformed_cloud2, field_names=("x", "y", "z"), skip_nans=True))
            merged_points = points1 + points2
            
            # 创建并发布合并后的点云消息
            header = std_msgs.msg.Header()
            header.stamp = rospy.Time.now()
            header.frame_id = self.point_cloud1.header.frame_id
            merged_cloud_msg = pc2.create_cloud_xyz32(header, merged_points)
            self.point_cloud_pub.publish(merged_cloud_msg)
            
            rospy.loginfo("合并后的点云已发布。")

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    try:
        merger = PointCloudMerger()
        merger.run()
    except rospy.ROSInterruptException:
        pass

7.然后打开rviz,订阅点云拼接配准后的话题/merged_point_cloud,结果如下所示:

8.呢么怎么知道具体的大小呢,后续考虑用聚类方法获取其位置、倾斜角以及长宽高,下班下班。

最最最最最最重要的是一定保证:新录制bag包的播放,以及另一个未录制bag包的播放,不要播放错了,我研究半天,哈哈哈哈,最后一定改写相应的旋转平移矩阵。

 

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/915552.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

STM32H743ZIT6+LWIP+MPU+CUBEMX,通过stm32cubemx完成初始化,ping包亲测没问题,带解释!!

文章耗时两个月,原来写了一半,后来遇到其他项目,中间自己重新画了一块电路板。终于把初始化功能实现了,网上的教程能用的确实凤毛麟角! 一、MPU配置详解 个人对stm32H7的MPU属于新接触,为了弄懂&#xff0…

python制作一个简单的端口扫描器,用于检测目标主机上指定端口的开放状态

import argparse # 用于解析命令行参数 from socket import * # 导入 socket 库的所有内容,用于网络通信 from threading import * # 导入 threading 库的所有内容,用于多线程操作 # 创建一个信号量,初始值为 1,用于线程同步&…

网络基础Linux(整理)

计算机网络背景 网络发展 独立模式: 计算机之间相互独立; 网络互联: 多台计算机连接在一起, 完成数据共享; 局域网LAN: 计算机数量更多了, 通过交换机和路由器连接在一起; 广域网WAN: 将远隔千里的计算机都连在一起; 所谓 "局域网" 和 "广域网" 只是一个…

我的第一个PyQt5程序

PyQt5的开发环境配置完成之后,开始编写第一个PyQt5的程序。 方法一:使用将.ui转换成.py文件的方法 import sys from FirstPyQt import Ui_MainWindow from PyQt5.QtWidgets import *#QtCore,QtGui,QtWidgets # from QtTest import Ui_MainWindow#导入Q…

面试:TCP、UDP如何解决丢包问题

文章目录 一、TCP丢包原因、解决办法1.1 TCP为什么会丢包1.2 TCP传输协议如何解决丢包问题1.3 其他丢包情况(拓展)1.4 补充1.4.1 TCP端口号1.4.2 多个TCP请求的逻辑1.4.3 处理大量TCP连接请求的方法1.4.4 总结 二、UDP丢包2.1 UDP协议2.1.1 UDP简介2.1.2…

Vue全栈开发旅游网项目(11)-用户管理前端接口联调

联调基本步骤 1.阅读接口文档 2.配置接口地址 3.使用axios获取数据 4.将数据设置到模型层 1.发送验证码联调 1.1 配置接口地址 文件地址:src\utils\apis.js //系统相关的接口 const SystemApis {sliderListUrl:apiHost"/system/slider/list/",//发送…

【相关分析方法】MATLAB计算滑动时滞相关系数

【相关分析方法】MATLAB计算滑动时滞相关系数 1 滑动时滞相关系数2 MATLAB代码2.1 函数代码2.2 案例参考滑动时滞相关系数(Moving Time-Lagged Cross-Correlation, TLCC) 是一种常用于分析两个时间序列之间的滞后关系的工具。它可以帮助我们确定一个时间序列相对于另一个时间…

llama-cpp模型轻量化部署与量化

一、定义 定义配置环境遇到的问题,交互模式下模型一直输出,不会停止模型量化Qwen1.5-7B 案例demo 二、实现 定义 主要应用与cpu 上的部署框架。由c完成。配置环境 https://github.com/ggerganov/llama.cpp https://github.com/echonoshy/cgft-llm/blo…

MySQl基础----Linux下数据库的密码和数据库的存储引擎(内附 实操图和手绘图 简单易懂)

绪论​ 涓滴之水可磨损大石,不是由于他力量强大,而是由于昼夜不舍地滴坠。 只有勤奋不懈地努力,才能够获得那些技巧。 ——贝多芬。新开MySQL篇章,本章非常基础,但同时需要一定的Linux基础,所以假若你没学习…

Qwen2-VL:发票数据提取、视频聊天和使用 PDF 的多模态 RAG 的实践指南

概述 随着人工智能技术的迅猛发展,多模态模型在各类应用场景中展现出强大的潜力和广泛的适用性。Qwen2-VL 作为最新一代的多模态大模型,融合了视觉与语言处理能力,旨在提升复杂任务的执行效率和准确性。本指南聚焦于 Qwen2-VL 在三个关键领域…

科技资讯|Matter 1.4 标准正式发布,低功耗蓝牙助力其发展

连接标准联盟(CSA)宣布推出最新的 Matter 1.4 版本,引入了一系列新的设备类型和功能增强,有望提高包括 HomeKit 在内的智能家居生态系统之间的互操作性。 设备供应商和平台能够依靠增强的多管理员功能改善多生态系统下的用户体验&…

群控系统服务端开发模式-应用开发-前端登录页面开发

一、清理不必要的文件 1、删除auth-redirect.vue a、在根目录src文件夹下views文件夹下找到登录文件夹login,在login文件夹中删除auth-redirect.vue文件。 b、在根目录mock文件夹下role文件夹中的routes.js文件中,删除下面的代码 {path: /auth-redirect…

深入理解接口测试:实用指南与最佳实践5.0(三)

✨博客主页: https://blog.csdn.net/m0_63815035?typeblog 💗《博客内容》:.NET、Java.测试开发、Python、Android、Go、Node、Android前端小程序等相关领域知识 📢博客专栏: https://blog.csdn.net/m0_63815035/cat…

mongoDB的安装及使用

mongodb的安装参考: Centos系统中mongodb的安装详解_centos安装mongodb-CSDN博客 不要下载最新的版本,新的版本中mongo命令无法使用,也就是安装后不能通过mongo命令登录,我这里使用5.0.30版本; mongodb客户端demo: …

vue3面试题1|[2024-11-12]

问题1:vue2与vue3的区别 1.vue2 和 vue3 双向绑定 方法不同 vue2:Object.defineProperty() ***使用这种方法,对于后添加的属性是劫持不到的,所以就会出现数据更新了, 但是视图没有更新,所以vue2就需要使用$…

python-24-一篇文章彻底掌握Python HTTP库Requests

python-24-一篇文章彻底掌握Python HTTP库Requests 一.简介 在 Python 中,Requests 是一个非常流行且易于使用的 Python HTTP 库,专门用于发送 HTTP/HTTPS 请求,获取请求响应; 可能觉得HTTP请求不是应该前端去做么?…

打造移动友好网站:UI设计的自适应技巧

随着移动互联网的快速发展,手机已成为人们获取信息的主要渠道之一。对于UI设计师而言,打造一个能够自适应手机屏幕的网站变得尤为重要。这不仅能够提升用户体验,还能在搜索引擎优化(SEO)中占据优势。以下是实现UI设计网…

Python →爬虫实践

爬取研究中心的书目 现在&#xff0c;想要把如下网站中的书目信息爬取出来。 案例一 耶鲁 Publications | Yale Law School 分析网页&#xff0c;如下图所示&#xff0c;需要爬取的页面&#xff0c;标签信息是“<p>”&#xff0c;所以用 itemssoup.find_all("p&…

STM32问题集

这里写目录标题 一、烧录1、 Can not connect to target!【ST-LINK烧录】 一、烧录 1、 Can not connect to target!【ST-LINK烧录】 烧录突然 If the target is in low power mode, please enable “Debug in Low Power mode” option from Target->settings menu 然后就&…

正点原子IMX6ULL--嵌入式Linux开发板学习中常用命令和笔记记录

学习路线图 传驱动文件 sudo cp chrdevbase.ko chrdevbaseApp /home/txj/linux/nfs/rootfs/lib/modules/4.1.15/ -f bootcmd setenv bootcmd tftp 80800000 zImage;tftp 83000000 imx6ull-alientek-emmc.dtb;bootz 80800000 - 83000000 setenv bootcmd tftp 80800000 zImag…