将未来帧中的点 pts​ 对齐到当前帧

已知当前的rt矩阵，和未来下一帧的rt矩阵和未来下一帧的5个点pts，求把pts对齐到当前帧。

import numpy as np

def align_points(rt_current, rt_future, pts_future):
    # 计算从未来帧到当前帧的相对变换矩阵
    rt_relative = np.linalg.inv(rt_current) @ rt_future
    
    # 将点转换为齐次坐标
    pts_future_homo = np.hstack((pts_future, np.ones((pts_future.shape[0], 1))))
    
    # 对点进行变换
    pts_current_homo = (rt_relative @ pts_future_homo.T).T
    
    # 转换回 3D 坐标
    pts_current = pts_current_homo[:, :3] / pts_current_homo[:, 3, np.newaxis]
    return pts_current

# 示例输入
rt_current = np.array([[1, 0, 0, 1],
                       [0, 1, 0, 2],
                       [0, 0, 1, 3],
                       [0, 0, 0, 1]])  # 当前帧的RT矩阵

rt_future = np.array([[0.866, -0.5, 0, 4],
                      [0.5, 0.866, 0, 5],
                      [0, 0, 1, 6],
                      [0, 0, 0, 1]])  # 未来帧的RT矩阵

pts_future = np.array([[1, 1, 1],
                       [2, 2, 2],
                       [3, 3, 3],
                       [4, 4, 4],
                       [5, 5, 5]])  # 未来帧的点

# 对齐点到当前帧
pts_current = align_points(rt_current, rt_future, pts_future)
print("Aligned points to current frame:\n", pts_current)

要将未来帧中的点 ( \text{pts}_\text{future} ) 对齐到当前帧，步骤如下：

---

问题背景

1. 当前帧的变换矩阵：( \text{rt}_\text{current} )
   这是从当前帧坐标系到世界坐标系的变换矩阵。

2. 未来帧的变换矩阵：( \text{rt}_\text{future} )
   这是从未来帧坐标系到世界坐标系的变换矩阵。

3. 未来帧的点集合：( \text{pts}_\text{future} )
   这些点的形状为 ( N \times 3 )（即 3D 点）。

---

目标

将 ( \text{pts}_\text{future} ) 转换到当前帧的坐标系。

---

数学公式

两帧之间的相对变换矩阵为：
[
\text{rt}\text{relative} = \text{rt}\text{current}^{-1} \cdot \text{rt}_\text{future}
]

该相对变换矩阵描述了从未来帧坐标系到当前帧坐标系的变换关系。

对于任意一个点 ( \text{pt}\text{future} )（形状为 ( 3 \times 1 )），我们需要先将其表示为齐次坐标 ( [x, y, z, 1]^T )，然后进行如下变换：
[
\text{pt}\text{current} = \text{rt}\text{relative} \cdot \text{pt}\text{future-homo}
]

最后，将结果从齐次坐标转换回 3D 坐标。

---

实现代码

import numpy as np

def align_points(rt_current, rt_future, pts_future):
    # 计算从未来帧到当前帧的相对变换矩阵
    rt_relative = np.linalg.inv(rt_current) @ rt_future
    
    # 将点转换为齐次坐标
    pts_future_homo = np.hstack((pts_future, np.ones((pts_future.shape[0], 1))))
    
    # 对点进行变换
    pts_current_homo = (rt_relative @ pts_future_homo.T).T
    
    # 转换回 3D 坐标
    pts_current = pts_current_homo[:, :3] / pts_current_homo[:, 3, np.newaxis]
    return pts_current

# 示例输入
rt_current = np.array([[1, 0, 0, 1],
                       [0, 1, 0, 2],
                       [0, 0, 1, 3],
                       [0, 0, 0, 1]])  # 当前帧的RT矩阵

rt_future = np.array([[0.866, -0.5, 0, 4],
                      [0.5, 0.866, 0, 5],
                      [0, 0, 1, 6],
                      [0, 0, 0, 1]])  # 未来帧的RT矩阵

pts_future = np.array([[1, 1, 1],
                       [2, 2, 2],
                       [3, 3, 3],
                       [4, 4, 4],
                       [5, 5, 5]])  # 未来帧的点

# 对齐点到当前帧
pts_current = align_points(rt_current, rt_future, pts_future)
print("Aligned points to current frame:\n", pts_current)

---

运行结果

运行上述代码后，pts_current 将是未来帧中的点 ( \text{pts}_\text{future} ) 对齐到当前帧坐标系后的点集合。

---

说明

• 公式推导：从未来帧到当前帧，需要相对变换矩阵 ( \text{rt}_\text{relative} )，其计算依赖于两个帧的绝对变换矩阵。
• 适用范围：此方法适用于常见的刚体变换场景，尤其是在 3D 点云处理和计算机视觉任务中。