使用 OpenCv 和 python 进行 RGB-D 姿态估计

Question

我目前正在尝试解决 RGBD SLAM 问题，但在通过 RANSAC 估计姿势时遇到了一些问题。我已通过以下方式正确地将点从 2d 转换为 3d：

def transform3d(x, y, depth):
    Z = depth[x][y] / scalingFactor
    X = (x - centerX) * Z / focalX
    Y = (y - centerY) * Z / focalY
    return (X,Y,Z)

def transform(matches, depth1, depth2, kp1, kp2):
    points_3d, points_2d = [], []
    temp = np.zeros((1, 2))
    for mat in matches:
        img1_idx = mat.queryIdx
        img2_idx = mat.trainIdx
        (y1, x1) = kp1[img1_idx].pt
        (y2, x2) = kp2[img2_idx].pt
        if depth[x1][y1] == 0:
            continue
        points_2d.append(kp2[img2_idx].pt)
        points_3d.append(np.array(transform3d(x1, y1, depth)))

    return (np.array(points_3d, np.float32), np.array(points_2d, np.float32))

然后我调用 calibrateCamera 函数来检索失真参数

mtx = np.array([[focalX, 0, centerX], [0, focalY, centerY], [0, 0, 1]], np.float32)

cv2.calibrateCamera(np.array([points_3d]), np.array([points_2d]), rgb1.shape[::-1], None, None, flags=1)

并做了RANSAC，得到旋转和平移矩阵：

cv2.solvePnPRansac(np.array([points_3d]), np.array([points_2d]), mtx, dist)

对于以上内容，我通过 OpenCVs 教程来估算姿势。

我也关注了这篇文章http://ksimek.github.io/2012/08/22/extrinsic/，尝试表达pose 通过做

R = cv2.Rodrigues(rvecs)[0].T
pose = -R*tvecs

我的姿势肯定是错的！但我不知道问题出在哪里。

我还用这个 C++ 实现的 RGBD SLAM http://www.cnblogs.com/gaoxiang12/p/4659805.html 交叉检查了我的代码

请帮忙！我真的很想让我的机器人动起来:)

Answer 1

首先，您应该避免在每一步都调用 calibrateCamera。这应该只从像棋盘这样的校准模式中完成一次。这个校准过程应该独立于你的主程序，你为你的相机做一次，只要你信任它们，你就坚持这些参数。您可以找到现有的程序来评估这些参数。如果您想快速入门，您可以输入焦距的理论值（制造商给出的该类型相机的近似值）。您还可以假设一个完美的相机，图像中心具有理想的 cx 和 cy。这将为您提供对姿势的粗略估计，但并非完全错误。然后您可以稍后使用更好的校准值对其进行细化。

对于其余的代码，这里可能有错误：

points_2d.append(kp2[img2_idx].pt)
points_3d.append(np.array(transform3d(x1, y1, depth)))

您似乎混合了 set2 (2d) 和 set1 (3d) 的点，所以看起来不一致。

希望对你有帮助。