[發(fā)明專利]一種基于點(diǎn)特征的單目視覺位姿測(cè)量方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201911034265.2
• 申請(qǐng)日: 2019-10-29
• 公開（公告）號(hào): CN110686650B
• 公開（公告）日: 2020-09-08
• 發(fā)明（設(shè)計(jì)）人: 王中宇;李亞茹;程銀寶
• 申請(qǐng)（專利權(quán)）人: 北京航空航天大學(xué)
• 主分類號(hào): G01C11/04
• 分類號(hào): G01C11/04
• 代理公司: 暫無(wú)信息
• 代理人: 暫無(wú)信息

1.一種基于點(diǎn)特征的單目視覺位姿測(cè)量方法，其特征在于包括以下步驟：

步驟一：獲取攝像機(jī)拍攝的圖像，用張正友標(biāo)定法對(duì)攝像機(jī)進(jìn)行標(biāo)定，獲取攝像機(jī)內(nèi)參數(shù)矩陣；

步驟二：提取圖像中任意4個(gè)不共面的特征點(diǎn)P_i(i＝1,2,3,4)，計(jì)算特征點(diǎn)在成像平面坐標(biāo)系中的坐標(biāo){q_i(x_i,y_i,f),i＝1,2,3,4}，根據(jù)兩點(diǎn)間距離公式計(jì)算出任意兩個(gè)特征點(diǎn)之間的距離：d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

步驟三：設(shè)P₁為世界坐標(biāo)系的原點(diǎn)，O_c為光心點(diǎn)，分析給定的任意不共線的4個(gè)特征點(diǎn)之間的相對(duì)位置關(guān)系：當(dāng)4個(gè)特征點(diǎn)所在的兩條直線平行時(shí)，定義特征點(diǎn)相關(guān)距離因子{k_i,i＝1,2,3,4}；當(dāng)4個(gè)特征點(diǎn)所在的兩條直線相交時(shí)，分別計(jì)算兩條直線交點(diǎn)在世界坐標(biāo)系中的坐標(biāo)及成像坐標(biāo)系中的坐標(biāo)；

步驟四：分別求解出世界坐標(biāo)系中X_w、Y_w、Z_w軸上的向量變換到攝像機(jī)坐標(biāo)系中的單位向量(r₁₁,r₂₁,r₃₁)；

步驟五：根據(jù)世界坐標(biāo)系的原點(diǎn)在攝像機(jī)中的坐標(biāo)求解出物體相對(duì)于攝像機(jī)的初始位置，并通過(guò)LM算法對(duì)初始位姿進(jìn)行優(yōu)化，得到物體相對(duì)于攝像機(jī)的初始姿態(tài)。

2.如權(quán)利要求1所述的一種基于點(diǎn)特征的單目視覺位姿測(cè)量方法，其特征在于，分析給定的4個(gè)共面不共線特征點(diǎn)P_i(i＝1,2,3,4)的位置關(guān)系：P₁、P2兩點(diǎn)所在的直線P₁P₂與P₃、P₄兩點(diǎn)所在的直線P₃P₄之間平行和相交。

3.如權(quán)利要求1所述的一種基于點(diǎn)特征的單目視覺位姿測(cè)量方法，其特征在于，所述步驟四中當(dāng)P₁、P₂兩點(diǎn)所在的直線P₁P₂與P₃、P₄兩點(diǎn)所在的直線P₃P₄平行時(shí)：

世界坐標(biāo)系中的X_w軸上的向量變換到攝像機(jī)坐標(biāo)系中的單位向量為

世界坐標(biāo)系中的Z_w軸上的向量變換到攝像機(jī)坐標(biāo)系中的單位向量(r₁₃,r₂₃,r₃₃)為

世界坐標(biāo)系中的Y_w軸上的向量變換到攝像機(jī)坐標(biāo)系中的單位向量(r₁₂,r₂₂,r₃₂)為(r₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。