基于OpenCv和python的RGBD姿态估计

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函数来检索失真参数

^{pr2}$

为了得到旋转和平移矩阵：

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

对于上面的内容，我通过OpenCVs教程来估计姿势。在

我也关注了这篇文章http://ksimek.github.io/2012/08/22/extrinsic/，并试图表达这个姿势 通过做

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

我的姿势绝对不对！但我不知道问题出在哪里。在

<>我也用RGBD SLAM ^ {A2}

的C++实现交叉检查了我的代码

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

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）的点，所以看起来不一致。在

希望有帮助。在