使用深度贴图计算非标准化的平移向量

Question

目标

我试图在不同的帧中计算相机之间的旋转和平移。因为我有一个可用的深度图，所以我试图找到缩放的平移，而不是标准化的平移

我试过的

(一)

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

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)

然后我计算新的位置和旋转：R_new=R*R_old和p_new=p_old+R_new*T

但这些值似乎不正确

(二)

我使用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，所以我尝试使用深度映射的信息来查找未规范化的向量，如here所示。 然后像以前一样更新相机位置和旋转。这也会产生错误的结果

我使用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

问题

这些产量：

但在这两种情况下，相机的位置越来越高，即使实际的相机几乎不移动。 经过几次迭代后，摄影机完全错误（放置在远离点云的位置）

如果有任何帮助，这就是我正在使用的dataset

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

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

此外，平移向量不是0，但这可以通过如何计算来解释

任何帮助都将不胜感激