使用 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 function 来检索失真参数

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/并尝试通过做来表达姿势

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

希望能帮助到你。

Answer 2

我认为你应该检查 (x, y) 的顺序。 您混合了从关键点返回的 (x, y) 的含义和用于访问深度图像值的索引。