python:根据旋转平移矩阵求取原始点云或者转换后点云

2024-04-25 07:12
文章标签 python 转换 旋转 矩阵 原始 点云 平移 后点 求取

本文主要是介绍python:根据旋转平移矩阵求取原始点云或者转换后点云，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

根据旋转平移矩阵求取原始点云或者转换后点云

  • 原始点云进行旋转平移
    • 示例 1
    • 示例 2
    • 示例 3
    • 示例 4
  • 根据转换后点云及转换矩阵求原始点云
    • 示例 1
    • 示例 2
    • 示例 3
    • 示例 4

原始点云进行旋转平移

转换前的点云可以表示为一个N行3列的矩阵，每一行代表一个点的坐标。我们定义一个旋转矩阵 R 和一个平移矩阵 T。假设转换矩阵为RT，要求转换后的点云，可以通过以下公式推导得到：

新的点云坐标 = R * 原始点云坐标 + T
或者：
转换后点云 = 转换矩阵 * 转换前点云

具体推导过程如下：

  1. 定义转换矩阵T
    RT = [R | t]
    其中R是一个3x3的旋转矩阵，t是一个3x1的平移向量。
    如给定三维空间旋转平移矩阵4*4：

    | r11 r12 r13 tx |
| r21 r22 r23 ty |
| r31 r32 r33 tz |
|  0   0   0  1 |

    其中，r11到r33为旋转矩阵的元素，tx、ty、tz为平移向量的元素。

    假设点云中的点为P = (x, y, z, 1)，将其转换到B坐标系中的点为P’ = (x’, y’, z’, 1)。
    转换公式为：

    x' = r11*x + r12*y + r13*z + tx
y' = r21*x + r22*y + r23*z + ty
z' = r31*x + r32*y + r33*z + tz

    其中，x、y、z为点P的坐标，x’、y’、z’为点P’的坐标。

  2. 转换前点云矩阵表示
    转换前点云矩阵表示为N行3列的矩阵P。

  3. 转换后点云计算
    转换后点云可以通过矩阵相乘得到：
    转换后点云 = P * T

示例 1

下面是Python代码实现：

import numpy as npdef transform_points(points, transform_matrix):"""将点云坐标系为A的点云通过转换矩阵转换为坐标系为B的点云:param points: 点云坐标系为A的点云，形状为(n, 4)，其中n为点的个数，每个点为(x, y, z, 1):param transform_matrix: 转换矩阵，形状为(4, 4):return: 坐标系为B的点云，形状为(n, 4)"""# 添加最后一列，将点云表示为齐次坐标形式points_homogeneous = np.concatenate((points, np.ones((points.shape[0], 1))), axis=1)# 转换点云坐标transformed_points_homogeneous = np.dot(points_homogeneous, transform_matrix.T)```# 或者也可以写成下面形式# transformed_points_homogeneous = np.dot(transform_matrix.T, points_homogeneous).T# 可以将RT矩阵拆开成R、t；可写成# transformed_points_homogeneous = np.dot(points_homogeneous, R) + t   # 推荐# 或者transformed_points_homogeneous = np.dot(R, points_homogeneous.T).T + t  # 会有误差# ```# 归一化处理，将齐次坐标转换为三维坐标transformed_points = transformed_points_homogeneous[:, :3] / transformed_points_homogeneous[:, 3:]return transformed_points

使用示例：

points_A = np.array([[1, 0, 0, 1],[0, 1, 0, 1],[0, 0, 1, 1],[1, 1, 1, 1],[2, 2, 2, 1]])transform_matrix = np.array([[1, 0, 0, 1],[0, 1, 0, 1],[0, 0, 1, 1],[0, 0, 0, 1]])points_B = transform_points(points_A, transform_matrix)
print(points_B)

输出结果：

[[2. 1. 1.][1. 2. 1.][1. 1. 2.][2. 2. 2.][3. 3. 3.]]

示例 2

首先，假设点云的坐标为 (x, y, z) 和齐次坐标 w，即点云的坐标可以表示为 (x, y, z, w)。

然后，我们定义一个旋转矩阵 R 和一个平移矩阵 T。点云的转换可以表示为：

新的点云坐标 = R * 原始点云坐标 + T

其中，旋转矩阵 R 是一个 3x3 的矩阵，平移矩阵 T 是一个 3x1 的矩阵。对于每个点云坐标 (x, y, z, w)，我们得到转换后的坐标 (x’, y’, z’, w’)：

x’ = R11 * x + R12 * y + R13 * z + T1
y’ = R21 * x + R22 * y + R23 * z + T2
z’ = R31 * x + R32 * y + R33 * z + T3
w’ = w

现在，我们需要将这个公式转换为代码实现。

首先，导入必要的库：

import numpy as np

然后，定义一个函数来进行点云的转换：

def transform_point_cloud(points, rotation_matrix, translation_matrix):# 添加齐次坐标 wpoints = np.hstack((points, np.ones((points.shape[0], 1))))# 点云转换transformed_points = np.dot(rotation_matrix, points.T).T + translation_matrixreturn transformed_points

在这个函数中，我们首先将点云添加齐次坐标 w。然后，使用矩阵乘法进行点云的转换，并添加平移矩阵。最后，返回转换后的点云。

使用这个函数，我们可以对点云进行转换。假设我们有一个点云矩阵 points，旋转矩阵 rotation_matrix 和平移矩阵 translation_matrix，我们可以这样调用函数：

transformed_points = transform_point_cloud(points, rotation_matrix, translation_matrix)

这样，我们就得到了转换后的点云 transformed_points。

示例 3

要将三维点云通过旋转和平移转换到另一个坐标系，可以使用齐次坐标表示点云和转换矩阵。齐次坐标是将三维坐标和平移合并到一个4维向量中的一种方法。

点云的齐次坐标表示为 [x, y, z, 1]T，其中T表示转置。转换矩阵是一个4*4的矩阵，将点云从一个坐标系转换到另一个坐标系。转换矩阵的形式如下：

R11 R12 R13 T1
R21 R22 R23 T2
R31 R32 R33 T3
0   0   0   1

其中R表示旋转矩阵，T表示平移矩阵。

点云的转换公式如下：

p_new = M * p

其中p_new是转换后的点云，M是转换矩阵，p是原始点云。

下面是Python代码实现：

import numpy as np# 原始点云
point_cloud = np.array([[x1, y1, z1], [x2, y2, z2], [x3, y3, z3], [x4, y4, z4], [x5, y5, z5]])# 转换矩阵
transform_matrix = np.array([[R11, R12, R13, T1],[R21, R22, R23, T2],[R31, R32, R33, T3],[0,   0,   0,   1]])# 添加齐次坐标
point_cloud_homo = np.hstack((point_cloud, np.ones((point_cloud.shape[0], 1))))# 转换点云
transformed_point_cloud_homo = np.dot(transform_matrix, point_cloud_homo.T).T# 去除齐次坐标
transformed_point_cloud = transformed_point_cloud_homo[:, :3]print(transformed_point_cloud)

这样就可以得到转换后的点云 transformed_point_cloud。

示例 4

假设点云为N * 4的矩阵，每行表示一个点的坐标[x, y, z, 1]，旋转矩阵为R，平移向量为T。

  1. 公式推导
    将点云矩阵与旋转平移矩阵相乘，得到转换后的点云矩阵：
    P’ = P * [R | T]
    其中，P为原始点云矩阵，P’为转换后的点云矩阵，[R | T]为旋转平移矩阵。

  2. 代码实现
    可以使用NumPy库来进行矩阵运算。

import numpy as npdef transform_point_cloud(point_cloud, rotation_matrix, translation_vector):# 将点云矩阵扩展为N * 4矩阵point_cloud = np.hstack((point_cloud, np.ones((point_cloud.shape[0], 1))))# 转换点云transformed_point_cloud = np.dot(point_cloud, np.vstack((rotation_matrix, translation_vector)))return transformed_point_cloud# 示例点云
point_cloud = np.array([[1, 2, 3],[4, 5, 6],[7, 8, 9]])# 示例旋转平移矩阵
rotation_matrix = np.array([[1, 0, 0],[0, 1, 0],[0, 0, 1]])
translation_vector = np.array([0, 0, 0])# 转换点云
transformed_point_cloud = transform_point_cloud(point_cloud, rotation_matrix, translation_vector)print(transformed_point_cloud)

这样，通过传入点云矩阵、旋转矩阵和平移向量，就可以得到转换后的点云矩阵。

根据转换后点云及转换矩阵求原始点云

要求原始点云N3，已知转换后的点云N3和转换矩阵，可以通过反向计算得到原始点云。

假设转换矩阵为T，原始点云为P，转换后的点云为P’，则有以下关系：

P’ = T * P

其中P和P’都是列向量，T是一个4x4的齐次变换矩阵。

要求原始点云P，可以通过反向计算得到：

P = inverse(T) * P’

其中inverse(T)表示T的逆矩阵。

假设旋转平移矩阵为R，平移向量为T，转换后点云为P_transformed。则可以使用以下代码求得原始点云P_original：

import numpy as np# 旋转平移矩阵
R = np.array([[r11, r12, r13],[r21, r22, r23],[r31, r32, r33]])# 平移向量
T = np.array([tx, ty, tz])# 转换后点云
P_transformed = np.array([[x1, y1, z1],[x2, y2, z2],[x3, y3, z3],[x4, y4, z4]])# 求逆矩阵
R_inv = np.linalg.inv(R)# 原始点云
P_original = np.dot(P_transformed - T, R_inv)  # 推荐print(P_original)

其中，r11, r12, r13, r21, r22, r23, r31, r32, r33为旋转矩阵的元素，tx, ty, tz为平移向量的元素，x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4为转换后点云的坐标。将上述代码中的矩阵元素和坐标值替换为实际的数值，并运行代码，即可得到原始点云P_original。

示例 1

在Python中，可以使用numpy库来进行矩阵运算。具体代码如下：

import numpy as npdef calculate_original_points(transform_matrix, transformed_points):inverse_transform_matrix = np.linalg.inv(transform_matrix)original_points = np.dot(inverse_transform_matrix, transformed_points.T).T # 会有误差return original_points

示例数据

transform_matrix = np.array([[1, 0, 0, 1],[0, 1, 0, 2],[0, 0, 1, 3],[0, 0, 0, 1]])transformed_points = np.array([[2, 3, 4],[3, 4, 5],[4, 5, 6]])original_points = calculate_original_points(transform_matrix, transformed_points)
print(original_points)
输出结果为：
[[-1.  0.  1.][-2. -1.  0.][-3. -2. -1.]]

这样就可以得到原始点云的坐标了。

示例 2

要根据旋转平移矩阵及转换后的点云求得原始点云，需要进行逆运算。

假设有旋转矩阵 R 和平移向量 t，以及转换后的点云 P’。那么原始点云 P 可以通过以下公式计算得到：

P = R^(-1) * (P’ - t)

其中，R^(-1) 表示 R 的逆矩阵。

在 Python 中，可以使用 NumPy 库来进行矩阵操作。下面是一个示例代码：

import numpy as np# 定义旋转矩阵 R 和平移向量 t
R = np.array([[1, 0, 0],[0, 1, 0],[0, 0, 1]])  # 假设为单位矩阵
t = np.array([0, 0, 0])  # 假设为零向量# 定义转换后的点云 P'
P_prime = np.array([[1, 2, 3],[4, 5, 6],[7, 8, 9]])# 计算旋转矩阵的逆
R_inv = np.linalg.inv(R)# 计算原始点云 P
P = np.dot(R_inv, (P_prime - t).T).T
# 或者
# P = np.dot((P_prime - t), R_inv)# 打印原始点云 P
print(P)

这个示例代码中，旋转矩阵 R 被定义为单位矩阵，平移向量 t 被定义为零向量。转换后的点云 P’ 被定义为一个 3x3 的矩阵。

可以根据实际情况修改旋转矩阵 R、平移向量 t 和转换后的点云 P’ 来计算对应的原始点云 P。

示例 3

要根据旋转平移矩阵和转换后的点云，求得原始点云，可以使用以下步骤：

  1. 首先，计算旋转平移矩阵的逆矩阵。假设旋转平移矩阵为R，平移向量为T，则逆矩阵为R_inv。

  2. 然后，对于转换后点云的每个点P’，将其转换为齐次坐标形式，即将其表示为一个3维向量[P’x, P’y, P’z, 1]。

  3. 接下来，利用逆矩阵R_inv将齐次坐标形式的点P’转换为原始点P，即P = R_inv * P’。

  4. 最后，将原始点P去除齐次坐标形式，即将其表示为一个3维向量[Px, Py, Pz]。

以下是一个示例代码：

import numpy as np# 转换后的点云，shape为(4, 3)
transformed_points = np.array([[1, 2, 3],[4, 5, 6],[7, 8, 9],[10, 11, 12]])# 旋转平移矩阵，shape为(3, 3)
rotation_matrix = np.array([[0.866, -0.5, 0],[0.5, 0.866, 0],[0, 0, 1]])# 平移向量，shape为(3, 1)
translation_vector = np.array([[1],[2],[3]])# 计算逆矩阵
rotation_matrix_inv = np.linalg.inv(rotation_matrix)# 转换后点云的齐次坐标形式
homogeneous_transformed_points = np.hstack((transformed_points, np.ones((4, 1))))# 原始点云
original_points = np.dot(rotation_matrix_inv, homogeneous_transformed_points.T)
original_points = original_points.T - np.dot(rotation_matrix_inv, translation_vector)# 去除齐次坐标形式
original_points = original_points[:, :3]print(original_points)

这样，原始点云就被计算出来了。注意，这里使用的是3x3的旋转平移矩阵，如果你的矩阵不是这个形式，需要适当修改代码。

示例 4

要根据旋转平移矩阵及转换后的点云来求得原始点云，需要使用逆变换操作。

首先，我们假设旋转平移矩阵为R，平移向量为t，转换后的点云为P_transformed，原始点云为P_original。

逆变换操作可以用以下公式表示：

P_original = R_inv * (P_transformed - t)

其中，R_inv表示矩阵R的逆矩阵。

以下是使用Python代码实现这个操作的示例：

import numpy as np# 定义旋转平移矩阵R和平移向量t
R = np.array([[1, 0, 0],[0, 1, 0],[0, 0, 1]])
t = np.array([[1],[2],[3]])# 定义转换后的点云P_transformed
P_transformed = np.array([[2, 3, 4],[5, 6, 7],[8, 9, 10],[11, 12, 13]])# 计算矩阵R的逆矩阵R_inv
R_inv = np.linalg.inv(R)# 进行逆变换操作，求得原始点云P_original
P_original = np.dot(R_inv, P_transformed.T - t).T# 打印结果
print(P_original)

这样，就可以根据旋转平移矩阵及转换后的点云求得原始点云。

这篇关于python:根据旋转平移矩阵求取原始点云或者转换后点云的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/934010。

相关文章

python: 多模块(.py)中全局变量的导入

python: 多模块(.py)中全局变量的导入

文章目录 global关键字可变类型和不可变类型数据的内存地址单模块(单个py文件)的全局变量示例总结 多模块(多个py文件)的全局变量from x import x导入全局变量示例 import x导入全局变量示例 总结 global关键字 global 的作用范围是模块(.py)级别： 当你在一个模块（文件）中使用 global 声明变量时，这个变量只在该模块的全局命名空
阅读更多...
【Python编程】Linux创建虚拟环境并配置与notebook相连接

【Python编程】Linux创建虚拟环境并配置与notebook相连接

1.创建 使用 venv 创建虚拟环境。例如，在当前目录下创建一个名为 myenv 的虚拟环境： python3 -m venv myenv 2.激活 激活虚拟环境使其成为当前终端会话的活动环境。运行： source myenv/bin/activate 3.与notebook连接 在虚拟环境中，使用 pip 安装 Jupyter 和 ipykernel： pip instal
阅读更多...
【机器学习】高斯过程的基本概念和应用领域以及在python中的实例

【机器学习】高斯过程的基本概念和应用领域以及在python中的实例

引言 高斯过程（Gaussian Process，简称GP）是一种概率模型，用于描述一组随机变量的联合概率分布，其中任何一个有限维度的子集都具有高斯分布 文章目录 引言一、高斯过程1.1 基本定义1.1.1 随机过程1.1.2 高斯分布 1.2 高斯过程的特性1.2.1 联合高斯性1.2.2 均值函数1.2.3 协方差函数（或核函数） 1.3 核函数1.4 高斯过程回归（Gauss
阅读更多...
【学习笔记】 陈强-机器学习-Python-Ch15 人工神经网络（1）sklearn

【学习笔记】 陈强-机器学习-Python-Ch15 人工神经网络（1）sklearn

系列文章目录 监督学习：参数方法 【学习笔记】 陈强-机器学习-Python-Ch4 线性回归 【学习笔记】 陈强-机器学习-Python-Ch5 逻辑回归 【课后题练习】 陈强-机器学习-Python-Ch5 逻辑回归（SAheart.csv） 【学习笔记】 陈强-机器学习-Python-Ch6 多项逻辑回归 【学习笔记 及 课后题练习】 陈强-机器学习-Python-Ch7 判别分析 【学
阅读更多...
2、PF-Net点云补全

2、PF-Net点云补全

2、PF-Net 点云补全 PF-Net论文链接：PF-Net PF-Net （Point Fractal Network for 3D Point Cloud Completion）是一种专门为三维点云补全设计的深度学习模型。点云补全实际上和图片补全是一个逻辑，都是采用GAN模型的思想来进行补全，在图片补全中，将部分像素点删除并且标记，然后卷积特征提取预测、判别器判别，来训练模型，生成的像
阅读更多...
poj 2187 凸包or旋转qia壳法

poj 2187 凸包or旋转qia壳法

题意： 给n（50000）个点，求这些点与点之间距离最大的距离。 解析： 先求凸包然后暴力。 或者旋转卡壳大法。 代码： #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#include <cstring>#include <cmath>#include <s
阅读更多...
nudepy，一个有趣的 Python 库！

nudepy，一个有趣的 Python 库！

更多资料获取 📚 个人网站：ipengtao.com 大家好，今天为大家分享一个有趣的 Python 库 - nudepy。 Github地址：https://github.com/hhatto/nude.py 在图像处理和计算机视觉应用中，检测图像中的不适当内容（例如裸露图像）是一个重要的任务。nudepy 是一个基于 Python 的库，专门用于检测图像中的不适当内容。该
阅读更多...
hdu 4565 推倒公式+矩阵快速幂

hdu 4565 推倒公式+矩阵快速幂

题意 求下式的值： Sn=⌈ (a+b√)n⌉%m S_n = \lceil\ (a + \sqrt{b}) ^ n \rceil\% m 其中： 0<a,m<215 0< a, m < 2^{15} 0<b,n<231 0 < b, n < 2^{31} (a−1)2<b<a2 (a-1)^2< b < a^2 解析 令： An=(a+b√)n A_n = (a +
阅读更多...
pip-tools：打造可重复、可控的 Python 开发环境，解决依赖关系，让代码更稳定

pip-tools：打造可重复、可控的 Python 开发环境，解决依赖关系，让代码更稳定

在 Python 开发中，管理依赖关系是一项繁琐且容易出错的任务。手动更新依赖版本、处理冲突、确保一致性等等，都可能让开发者感到头疼。而 pip-tools 为开发者提供了一套稳定可靠的解决方案。 什么是 pip-tools？ pip-tools 是一组命令行工具，旨在简化 Python 依赖关系的管理，确保项目环境的稳定性和可重复性。它主要包含两个核心工具：pip-compile 和 pip
阅读更多...
HTML提交表单给python

HTML提交表单给python

python 代码 from flask import Flask, request, render_template, redirect, url_forapp = Flask(__name__)@app.route('/')def form():# 渲染表单页面return render_template('./index.html')@app.route('/submit_form',
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2