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

我目前正在尝试解决RGBD SLAM问题，但我正在通过RANSAC遇到一些估计姿势的问题。我已经正确地从2D转换点至3d通过：使用OpenCv和python进行RGB-D姿态估计

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函数来获取失真PARAM

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

请帮帮忙！我真的想让我的机器人移动:)

来源

2015-12-15 spedy

首先，您应该避免在每一步调用calibrateCamera。这应该只从棋盘格等校准模式中完成一次。校准过程应与您的主程序无关，您只需为相机执行一次，只要您信任它们即可坚持这些参数。您可以找到现有的程序来评估这些参数。如果您想快速启动某些功能，可以输入焦距的理论值（制造商给出的该类型相机的近似值）。您还可以假设在图像中心具有理想cx和cy的完美相机。这会给你粗略的姿势估计，但不完全错误。然后您可以稍后使用更好的校准值对其进行优化。

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

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

看来你从集2（2D）和SET1（3D）混合点，因此它看起来并不一致。

希望它有帮助。

来源

2016-02-01 14:43:56

首先，我要感谢你进行这个好的和彻底的分析，你甚至无法想象我的伟大：D按照你的建议，我想出了这个https://gist.github.com/spedy/747a7f6d74df718713cc。这给了我一个不协调的点云，看起来像https://goo.gl/OOZ4AE。不太明白我提到的错误出错的地方，我试图找到3d点（image1）和2d点（image2）的特征匹配之间的ransac对应关系。 – spedy

对不起，但我并不真正了解你的SLAM的内容以及你想要在3d和2d之间进行匹配的内容。乍一看，它看起来很奇怪，试图匹配不同种类的数据。通常你会匹配2个连续的帧，在时间n的图像1，在时间n-1的图像2。根据您的匹配方法和主要基于您的功能，您将使用3d或2d（您的功能是什么？）。你应该在这里保持一致，不要混淆你的搭配。然后重建应该使用3d数据。 –

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

回答

相关问题