使用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姿態估計

回答

相關問題