python - 如何使用 QR/Aruco 代码(Python CV)将点云从相机框架转换为世界框架?
问题描述
我想将来自多个深度相机的点云合并在一起。为了实现这一点,我试图将点云从每个相机帧转换为单个世界帧,由 aruco 代码(类似于 QR 代码)定义。
现在,我只是从一台相机开始。
假设我有
k = #camera intrinsic matrix (3x3)
aruco_world_coords = #coordinates (meters) of the aruco code corners in the real world (4x3)
e.g. [[0, 0, 0],
[.1, 0, 0],
[.1, -.1, 0],
[0, -.1, 0]]
rgb_image = #camera's color image (width x height x 3)
cam_pc = #point cloud in camera's frame (nx3)
我目前执行以下操作:
import cv2 as cv
import numpy as np
from cv2 import aruco
#######################################################################
##### Detect Aruco code and extract rvec and tvec ######
#######################################################################
gray = cv.cvtColor(rgb_image, cv.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
params = aruco.DetectorParameters_create()
corners, _, _ = aruco.detectMarkers(gray, aruco_dict, parameters=params)
rvec, tvec = cv.solvePnP(QR_world_coords,
corners[0],
K, None, None, None,
False, cv.SOLVEPNP_ITERATIVE)
rmatrix, _ = cv.Rodrigues(rvec)
#######################################################################
##### Calculate new point cloud ######
#######################################################################
new_pc = np.matmul(np.linalg.inv(rmatrix), cam_pc.T-tvec).T
#######################################################################
##### Same math in a different way to verify result #####
#######################################################################
'''
t = -rmatrix.T.dot(tvec)
rmatrix_ = rmatrix.T
new_pc = cam_pc.dot(rmatrix_.T) + tvec.squeeze()
'''
#######################################################################
##### Same math, in yet another way, to be super sure it's right #####
#######################################################################
'''
m = np.hstack([rmatrix, trans])
m = np.vstack([m, np.array([[0,0,0,1]])])
m = np.linalg.inv(m)
cam_pc_homog = np.concatenate([cam_pc, np.ones((pc.shape[0],1))], axis=1)
new_pc = np.matmul(m, cam_pc_homog.T).T
new_pc /= new_pc[:,3].reshape(-1,1)
new_pc = new_pc[:,:3]
'''
然后,我在原点 (0,0,0) 处绘制轴的场景中显示新的点云。
我现在期望当我将相机对准 Aruco 代码时,这些点应该会发生变化,而且确实会发生变化。
但是我也希望点云现在应该保持静止,即使我旋转相机,因为这些点已经转换到世界框架中,并且世界相对于世界框架没有移动。这不会发生,当我移动相机时,这些点会继续移动(尽管以不同的方式)。
我相信旋转和平移是正确的,我使用它们将轴显示到 rgb_image 中的 aruco 代码上,并且它们位于正确的位置。
任何帮助将不胜感激!
解决方案
推荐阅读
- ios - SwiftUI - 如何获取 ScrollView 内容的大小(高度)
- asciidoc - Java 源文件也可以是带有目录的有效 AsciiDoc 文档吗?
- python - Python Base64 影响输出数据
- java - Intellij (java) 运行/调试配置“未指定主类”Azure Functions
- webpack - 根据某些条件更改 webpackMode
- docker - /etc/nginx/conf.d/default.conf:1 中的上游“9000”中没有端口
- c - 根据大小使用科学计数法打印浮点数
- autodesk-forge - 设计自动化 LogTrace onProgress 无法匹配自定义调用中的参数
- jestjs - 开玩笑,toClick() 在使用 Puppeteer 标记时失败
- unity3d - Unity Occlusion Culling 在全封闭房间中中断