[發(fā)明專利]一種基于正八面體的單目6D位姿估計(jì)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110023406.1
• 申請(qǐng)日: 2021-01-08
• 公開（公告）號(hào): CN112734844B
• 公開（公告）日: 2022-11-08
• 發(fā)明（設(shè)計(jì)）人: 孫昊;崔睿;段倫輝;譚英倫;吳夢(mèng)坤
• 申請(qǐng)（專利權(quán)）人: 河北工業(yè)大學(xué)
• 主分類號(hào): G06T7/73
• 分類號(hào): G06T7/73;G06T7/80
• 代理公司: 天津翰林知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 12210
• 代理人: 王瑞

1.一種基于正八面體的單目6D位姿估計(jì)方法，其特征在于，該方法包括以下步驟：

第一步，使用ArUco碼對(duì)正八面體進(jìn)行標(biāo)記，得到ArUco碼標(biāo)記的正八面體；標(biāo)定單目相機(jī)并觀測(cè)ArUco碼標(biāo)記的正八面體；

第二步，分別對(duì)當(dāng)前幀和初始幀中所有可觀測(cè)到的若干個(gè)ArUco碼進(jìn)行定位，再獲得當(dāng)前幀和初始幀中所有觀測(cè)到的ArUco碼的編碼，獲得當(dāng)前幀中所有觀測(cè)到的ArUco碼在相機(jī)坐標(biāo)系下的位姿矩陣T_i,i＝1,2,...,n,T_i∈SE(3)以及初始幀中所有觀測(cè)到的ArUco碼在相機(jī)坐標(biāo)系下的位姿矩陣T_i',i＝1,2,3,...,n,T_i'∈SE(3)，n表示某一幀觀測(cè)到的ArUco碼的數(shù)量；SE(3)表示矩陣的屬性；

第三步，以正八面體體心為原點(diǎn)構(gòu)建正八面體坐標(biāo)系，將8個(gè)ArUco碼引入正八面體坐標(biāo)系中，根據(jù)正八面體每個(gè)面相對(duì)于正八面體體心的空間位置關(guān)系，分別計(jì)算得到正八面體每個(gè)面上的ArUco碼在正八面體坐標(biāo)系下的相對(duì)位姿矩陣T_i^ref,i＝1,2,3,...,8,T_i^ref∈SE(3)及其對(duì)應(yīng)編碼；

第四步，根據(jù)第二步得到的當(dāng)前幀中ArUco碼在相機(jī)坐標(biāo)系下的位姿矩陣T_i,i＝1,2,3,...,n,T_i∈SE(3)及其對(duì)應(yīng)編碼以及第三步得到的相對(duì)位姿矩陣T_i^ref,i＝1,2,3,...,8,T_i^ref∈SE(3)及其對(duì)應(yīng)編碼，通過搜索T_i與T_i^ref的編碼，將編碼相同的T_i與T_i^ref進(jìn)行匹配，計(jì)算得到當(dāng)前幀正八面體在相機(jī)坐標(biāo)系下的空間位姿矩陣T；

根據(jù)第二步得到的初始幀中ArUco碼在相機(jī)坐標(biāo)系下的位姿矩陣T_i',i＝1,2,3,...,n,T_i'∈SE(3)及其對(duì)應(yīng)編碼以及第三步得到的相對(duì)位姿矩陣T_i^ref,i＝1,2,3,...,8,T_i^ref∈SE(3)及其對(duì)應(yīng)編碼，通過搜索T_i'與T_i^ref的編碼，將編碼相同的T_i'與T_i^ref進(jìn)行匹配，計(jì)算得到初始幀正八面體在相機(jī)坐標(biāo)系下的空間位姿矩陣T_init；

則當(dāng)前幀正八面體相對(duì)于初始幀正八面體的位姿矩陣T_rel為：

由于某一幀會(huì)采集到多個(gè)ArUco碼，因此通過檢測(cè)到的ArUco碼的面積計(jì)算得到權(quán)重，再通過權(quán)重計(jì)算某一幀中正八面體在相機(jī)坐標(biāo)系下的空間位姿矩陣；

式(3)中，a_i表示檢測(cè)到的ArUco碼的權(quán)重，A_i表示檢測(cè)到的每個(gè)ArUco碼的面積，n表示當(dāng)前幀觀測(cè)到的ArUco碼的數(shù)量；

因此，當(dāng)前幀正八面體在相機(jī)坐標(biāo)系下的空間位姿矩陣T為：

同理可得，初始幀正八面體在相機(jī)坐標(biāo)系下的空間位姿矩陣T_init為：

第五步，首先構(gòu)建被測(cè)物體坐標(biāo)系，將正八面體固定于被測(cè)物體上，并計(jì)算正八面體相對(duì)于被測(cè)物體的空間位移和旋轉(zhuǎn)矩陣得到正八面體相對(duì)于被測(cè)物體的位姿矩陣

則被測(cè)物體的相對(duì)位姿矩陣T_final：

得到當(dāng)前幀被測(cè)物體相對(duì)于初始幀的空間姿態(tài)變化。

2.根據(jù)權(quán)利要求1所述的基于正八面體的單目6D位姿估計(jì)方法，其特征在于，第一步中，使用ArUco碼對(duì)正八面體進(jìn)行標(biāo)記的過程為：使用OpenCV圖像處理庫(kù)生成8個(gè)ArUco碼，將8個(gè)ArUco碼按編碼順序分別貼附于正八面體的8個(gè)面上，并保證ArUco碼的幾何中心與其貼附的面的幾何中點(diǎn)重合，且ArUco碼的至少一條邊與其貼附的正八面體的面的底邊平行。