在一些有關物體6d位姿估計或者機械人抓取的**中，我們常會聽到乙個詞：物體6d位姿估計 (6d object pose estimation)，那什麼是物體的6d位姿呢？它和slam中的相機6d位姿一樣嗎？
6d是指6個自由度，代表了3個自由度的位移 (也叫平移 (translation))，以及3個自由度的空間旋轉 (rotation)，合起來就叫位姿 (pose)。位姿是乙個相對的概念，指的是兩個座標系之間的位移和旋轉變換。物體6d位姿和相機6d位姿是相似的，區別在於從哪個座標系變換到相機座標系。
相機6d位姿是指拍攝當前影象時刻，相機座標系相對於世界座標系發生的平移和旋轉變換。世界座標系可以定義在任意位置，也可以和當前相機座標系重合。相機6d位姿通常用世界系到相機系的rt變換來表示，也即：t_c = r_cw * t_w + t_cw，其中r_cw代表由世界系到相機系的旋轉，t_cw代表由世界系到相機系的平移，t_c代表相機系下的3d點，t_w代表世界系下的3d點。
而物體6d位姿是指拍攝當前影象時刻，相機座標系相對於原始物體所在的世界系，發生的平移和旋轉變換。原始物體可以放在世界系的任何位置，而且通常將物體本身的重心和朝向與世界系對齊。物體6d位姿通常用原始物體所在世界系到相機系的rt變換來表示，也即：t_c = r_cm * t_m + t_cm，其中r_cm代表由原始物體 (model) 所在的世界系到相機系的旋轉，t_cm代表由物體所在的世界系到相機系的平移，t_c代表相機系下物體的3d點，t_m代表物體所在世界系下物體的3d點。因此，當世界系和物體本身對齊時，相機的6d位姿等價於物體的6d位姿。
具體我們看乙個物體6d位姿的例子，資料來自preprocessed linemod dataset資料集，原始物體3d模型本身的重心和朝向與世界系是對齊的，其座標值為t_m：
使用相機拍攝帶有目標物體的場景後，恢復相機座標系下的點雲如下，此時目標物體的3d座標為t_c：
基於給定的ground truth的物體6d位姿，camrm2c代表物體所在世界系到相機系的旋轉矩陣r，camtm2c代表物體所在世界系到相機系的平移矩陣t：
camrm2c=[0.09630630, 0.99404401, 0.05100790,0.57332098, -0.01350810, -0.81922001,-0.81365103, 0.10814000, -0.57120699]camtm2c=[-105.35775150, -117.52119142, 1014.87701320]，此時可以看到，根據物體的6d位姿 r_cm 和 t_cm 將物體由本身的世界系變換到了相機系下，將物體3d點與場景3d點疊加在一起的效果如下：
此時，也可以結合相機的內參k，cam_k: [572.4114, 0.0, 325.2611, 0.0, 573.57043, 242.04899, 0.0, 0.0, 1.0]，將物體的3d點投影到2d影象，如下圖，可以看到也是正確的。
因此，物體的6d位姿是和物體3d模型本身的世界座標系相關的，同乙個相機系下，不同物體的6d位姿是不一樣的；如果保持物體和相機的相對位置不變，則物體的位姿是不變的；如果物體不動，相機系發生了移動，則新的物體位姿需要在原來的基礎上再疊加相機系的相對移動位姿 △r 和 △t，這也是使用labelfusion構建6d真值資料集的原理。
p.s. 上文中使用的是preprocessed的linemod資料集，物體的3d模型重心在世界系 (0,0,0) 位置，這裡把原始的linemod資料集也測試一下。同樣是ape類別，其3d模型如下圖：
可以看到原始3d模型 (m_o) 與上文的3d模型差別了繞y軸旋轉180°，並且旋轉後的重心需要向座標原點平移。可以計算得到旋轉矩陣r_mm_o為：r_mm_o = [-1,0,0,0,1,0,0,0,-1]
原始物體的中心為 c_m_o=(4.5445,4.6495,-44.5433)，由t_m=r_mm_o*t_m_o+t_mm_o，其中 t_mm_o = c_m - r_mm_o*c_m_o，而 c_m=(0,0,0) 為preprocessed後的物體的中心座標，由此可得 t_mm_o為：t_mm_o = [4.5445,-4.6495,-44.5433]
按照原始linemod提供的真值，有 t_c = r_cm_o * t_m_o + t_cm_o，其中 r_cm_o 代表了由原始物體向相機系物體的旋轉，t_m_o 代表原始物體的座標點，t_cm_o 代表原始物體向相機系物體的平移。可以進行變換：
t_c=[r_cm_o t_cm_o] * t_m_o=[r_cm_o t_cm_o] * [r_mm_o^t -r_mm_o^t*t_mm_o] * [r_mm_o t_mm_o] *t_m_o=[r_cm_o t_cm_o] * [r_mm_o^t -r_mm_o^t*t_mm_o] * t_m=[r_cm t_cm] * t_mr_cm_o =[ -0.0963063, 0.994044, -0.0510079, -0.573321, -0.0135081, 0.81922, 0.813651, 0.10814, 0.571207]t_cm_o = [-111.814, -78.3622, 1036.12]相機位姿估計0 基本原理之如何解PNP問題
用途 各種位姿估計 文章型別 原理 今天給大家講一講相機位姿估計的基本原理，說實話我本人也沒太了解，這裡權當做拋磚引玉了。本來我這個部落格是寫應用型文章的，但雖然不做理論研究，但你要使用別人的方法來解決問題，那麼也還是多多少少要對它的原理有點了解的。關於pnp問題就是指通過世界中的n個特徵點與影象成...
相機深度轉點雲公式原理
1 世界座標到影象的對映過程，考慮世界座標點m xw,yw,zw 對映到影象點m u,v 的過程，如下圖所示 省略推導過程，直接給最終變換公式 u,v為影象座標系下的任意座標點，u0,v0分別為影象的中心座標。xw,yw,zw表示世界座標系下的三維座標點，相機座標和世界座標下的同乙個物體具有相同的深...
深度相機生成點雲資料的原理
2019年8月4日 16 26 40 rgb d 影象中的rgb提供了畫素座標系下的x，y座標，而深度圖直接提供了相機座標系下的 z 座標，也就是相機與點的距離。根據 rgb d 影象的資訊和相機的內參，可以計算出任何乙個畫素點在相機座標系下的座標。根據 rgb d 影象的資訊和相機的內參與外參，可...