使用深度 map 计算未归一化的平移向量

Question

目标

我正在尝试计算不同帧中相机之间的旋转和平移。 由于我有可用的深度 map 我尝试找到缩放的平移而不是标准化的平移。

我试过的

1)

首先，我使用 cv2（ORB 和 BFMatcher）在两个帧中找到对应点。 然后我计算这些点的3d position（如图所示）

def calculatePointObjectSpace(.....):
    factor = 1
    u = point[0]
    v = point[1]
    Z = depthImg[int(v), int(u)] / factor;
    Z=Z[0]
    X = (u - cx) * Z / fx;
    Y = (v - cy) * Z / fy;
    return np.array([X,Y,Z])

有了这个，我使用solvePnPRansac找到相机平移和旋转到第2帧（我有可用的相机参数）

    objectPositions = []
    for pt in pts1:
        objectPosition = extractor.calculatePointObjectSpace(self.frame1.cameraParams,self.frame1.depthImg,pt)
        objectPositions.append(objectPosition)

    objectPositions = np.asarray(objectPositions)
    retVal, rVector, self.Trans, inLiers = solvePnPRansac(objectPositions,pts2,self.K,None)
    self.Trans = np.concatenate(self.Trans)
    self.R, _ = Rodrigues(rVector)

然后我计算新的 position 和旋转： R _new = R*R _old and P _new = P _old + R _new *T

但这些值似乎是不正确的。

2)

我使用 opencv 和 RANSAC 计算基本矩阵

def getEssentialMatrix2(pts1,pts2,K):
    E, mask = cv2.findEssentialMat(pts1,pts2,K,method=cv2.RANSAC,prob=0.999,threshold=3)
    pts1 = pts1[mask.ravel() == 1]
    pts2 = pts2[mask.ravel() == 1]
    return E,pts1,pts2

然后我使用恢复姿势来尝试找到旋转和平移

retVal, R, t, mask = cv2.recoverPose(E,pts1,pts2,K)

由于这会导致归一化的平移向量 t，因此我尝试使用深度 map 的信息来找到未归一化的向量，如下所示。 然后像以前一样更新相机 position 和旋转。 这也会产生错误的结果。

我使用 open3d 在 3d 中呈现我的观点

def getRenderedPointCloud(frame, cameraParams):
    color_raw = o3d.io.read_image(frame.path)
    depth_raw = o3d.io.read_image(frame.depthPath)
    rgbd_image = o3d.geometry.RGBDImage.create_from_tum_format(
        color_raw, depth_raw, convert_rgb_to_intensity=False)

    intr = o3d.open3d.camera.PinholeCameraIntrinsic(cameraParams.width, cameraParams.height, fx=cameraParams.focalx, fy=cameraParams.focaly, cx=cameraParams.centerx, cy=cameraParams.centery)

    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        rgbd_image,intr)
    pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) #so that it is up right
    return pcd

问题

这些产量：

但是在这两种情况下，即使实际的相机几乎没有移动，相机的位置也会越来越高。 经过几次迭代后，相机完全错误（放置在远离点云的某个地方）。

如果有帮助，这是我正在使用的数据集

此外，当我两次使用同一帧时，第一种方法给出了 0 的平移和一个标识矩阵旋转，正如预期的那样。 但是第二种方法给出了旋转矩阵

Rotation matrix [[-0.33333333 -0.66666667  0.66666667]
 [-0.66666667 -0.33333333 -0.66666667]
 [ 0.66666667 -0.66666667 -0.33333333]]

平移向量也不为 0，但这可以通过它的计算方式来解释。

任何帮助将非常感激。