本技術涉及位姿估計，尤其涉及一種位姿估計方法、裝置和電子設備。

背景技術：

1、目前，在對對稱平面物體進行作業(yè)過程中，通常采用傳統(tǒng)的定向包圍盒（orientedbounding?box，obb）算法求解對稱平面物體的位姿，該方式求解出的位姿中，對稱平面物體的主方向可能與對稱平面物體的邊長方向存在誤差，例如，當對稱平面物體的長寬比較小時，其主方向會沿著對稱平面物體的對角線方向，從而導致對稱平面物體的位姿估計出現(xiàn)偏差，進而影響對稱平面物體的作業(yè)精準度。

技術實現(xiàn)思路

1、本技術提供了一種位姿估計方法、裝置和電子設備，用以解決現(xiàn)有技術針對對稱平面物體的位姿估計準確度較差的問題。

2、本技術提供的技術方案如下：

3、一方面，本技術提供了一種位姿估計方法，包括：

4、獲取對稱平面物體的平面點云數(shù)據(jù)；其中，平面點云數(shù)據(jù)包括對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)；

5、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，確定對稱平面物體對應的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；

6、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定對稱平面物體對應的最大擬合平面；

7、基于對稱平面物體的表面上的各個原始采樣點各自投影到最大擬合平面上的投影采樣點的點云數(shù)據(jù)，確定最大擬合平面的邊長方向向量；

8、基于最大擬合平面的邊長方向向量，對旋轉(zhuǎn)矩陣進行校正得到目標旋轉(zhuǎn)矩陣；

9、基于質(zhì)心位置和目標旋轉(zhuǎn)矩陣，確定對稱平面物體的目標位姿。

10、可選的，基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，確定對稱平面物體對應的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣，包括：

11、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，采用主成分分析算法分析出對稱平面物體的特征值和特征向量；

12、基于對稱平面物體的特征值和特征向量，確定對稱平面物體對應的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣。

13、可選的，基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，確定對稱平面物體對應的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣，包括：

14、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，迭代執(zhí)行有向包容盒求解操作，直至確定滿足迭代終止條件時，將當前執(zhí)行有向包容盒求解操作時求解出的當前有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣確定為最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；其中，有向包容盒求解操作包括：

15、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，計算各個原始采樣點各自投影到指定平面上的投影采樣點的點云數(shù)據(jù)；

16、基于各個原始采樣點各自投影到指定平面上的投影采樣點的點云數(shù)據(jù)，采用凹包算法提取對稱平面物體的凹邊界上的各個輪廓采樣點的點云數(shù)據(jù)；其中，在首次執(zhí)行有向包容盒求解操作時，凹包算法的學習參數(shù)為初始化值；在非首次執(zhí)行有向包容盒求解操作時，凹包算法的學習參數(shù)為上一次執(zhí)行有向包容盒求解操作時調(diào)整的目標值；

17、基于對稱平面物體的凹邊界上的各個輪廓采樣點的點云數(shù)據(jù)，采用主成分分析算法分析出對稱平面物體的特征值和特征向量；

18、基于對稱平面物體的特征值和特征向量，確定對稱平面物體對應的當前有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；

19、提取當前有向包圍盒的盒邊界上的各個輪廓采樣點的點云數(shù)據(jù)；

20、將對稱平面物體的凹邊界上的各個輪廓采樣點的點云數(shù)據(jù)與當前有向包圍盒的盒邊界上的各個輪廓采樣點的點云數(shù)據(jù)進行點云配準，得到對稱平面物體的凹邊界與當前有向包圍盒的盒邊界之間的匹配分數(shù)；

21、確定匹配分數(shù)小于設定分數(shù)閾值時，基于對稱平面物體的凹邊界與當前有向包圍盒的盒邊界之間的差異，將凹包算法的學習參數(shù)調(diào)整為目標值。

22、可選的，基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定對稱平面物體對應的最大擬合平面，包括：

23、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，迭代執(zhí)行平面擬合操作，直至確定當前執(zhí)行平面擬合操作時計算出的當前內(nèi)點比例不小于設定內(nèi)點比例閾值時，將當前執(zhí)行平面擬合操作時擬合出的當前擬合平面確定為對稱平面物體的最大擬合平面；其中，平面擬合操作包括：

24、從對稱平面物體的表面上的各個原始采樣點中，隨機選取設定數(shù)量的原始采樣點作為候選采樣點；

25、基于各個候選采樣點的點云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定當前擬合平面；

26、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，計算對稱平面物體的表面上的各個原始采樣點各自到達當前擬合平面的距離；

27、將距離小于設定距離閾值的各個原始采樣點分別確定為內(nèi)點，并基于內(nèi)點的數(shù)量以及對稱平面物體的表面上的各個原始采樣點的數(shù)量，計算當前內(nèi)點比例。

28、可選的，基于對稱平面物體的表面上的各個原始采樣點各自投影到最大擬合平面上的投影采樣點的點云數(shù)據(jù)，確定最大擬合平面的邊長方向向量，包括：

29、基于對稱平面物體的表面上的各個原始采樣點各自投影到最大擬合平面上的投影采樣點的點云數(shù)據(jù)，提取最大擬合平面的各條輪廓線；

30、基于各條輪廓線中相交輪廓線對應的交點的點云數(shù)據(jù)，計算最大擬合平面的邊長方向向量。

31、可選的，基于對稱平面物體的表面上的各個原始采樣點各自投影到最大擬合平面上的投影采樣點的點云數(shù)據(jù)，提取最大擬合平面的各條輪廓線，包括：

32、基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，計算各個原始采樣點各自投影到最大擬合平面上的投影采樣點的點云數(shù)據(jù)；

33、基于各個原始采樣點各自投影到最大擬合平面上的投影采樣點的點云數(shù)據(jù)，采用凹包算法提取最大擬合平面的凹邊界上的各個輪廓采樣點的點云數(shù)據(jù)；

34、對各個輪廓采樣點的點云數(shù)據(jù)進行直線提取得到最大擬合平面的各條輪廓線。

35、可選的，最大擬合平面為方形；基于各條輪廓線中相交輪廓線對應的交點的點云數(shù)據(jù)，計算最大擬合平面的邊長方向向量，包括：

36、從最大擬合平面的四條輪廓線中選擇一條輪廓線作為基準輪廓線，計算基準輪廓線與四條輪廓線中其余的三條輪廓線之間的距離；

37、將三條輪廓線中距離最大的輪廓線與基準輪廓線組成第一組對邊，以及將三條輪廓線中剩余的兩條輪廓線組成第二組對邊；

38、基于第一組對邊中的一條輪廓線與第二組對邊中的兩條輪廓線之間的交點的點云數(shù)據(jù)，計算最大擬合平面的邊長方向向量。

39、可選的，基于最大擬合平面的邊長方向向量，對旋轉(zhuǎn)矩陣進行校正得到目標旋轉(zhuǎn)矩陣，包括：

40、基于邊長方向向量和旋轉(zhuǎn)矩陣中的豎軸方向向量，采用向量叉乘右手定則計算橫軸方向向量；

41、基于邊長方向向量、豎軸方向向量和橫軸方向向量，確定目標旋轉(zhuǎn)矩陣。

42、另一方面，本技術提供了一種位姿估計裝置，包括：

43、點云獲取單元，用于獲取對稱平面物體的平面點云數(shù)據(jù)；其中，平面點云數(shù)據(jù)包括對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)；

44、包圍盒確定單元，用于基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，確定對稱平面物體對應的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；

45、平面擬合單元，用于基于對稱平面物體的表面上的各個原始采樣點的點云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定對稱平面物體對應的最大擬合平面；

46、校正預備單元，用于基于對稱平面物體的表面上的各個原始采樣點各自投影到最大擬合平面上的投影采樣點的點云數(shù)據(jù)，確定最大擬合平面的邊長方向向量；

47、姿態(tài)校正單元，用于基于最大擬合平面的邊長方向向量，對旋轉(zhuǎn)矩陣進行校正得到目標旋轉(zhuǎn)矩陣；

48、位姿確定單元，用于基于質(zhì)心位置和目標旋轉(zhuǎn)矩陣，確定對稱平面物體的目標位姿。

49、另一方面，本技術提供了一種電子設備，包括存儲器、處理器和存儲在存儲器上并可在處理器上運行的計算機程序，處理器執(zhí)行計算機程序時實現(xiàn)上述位姿估計方法。

50、本技術的有益效果如下：

51、本技術通過利用對稱平面物體對應的最大擬合平面的邊長方向向量，校正對稱平面物體對應的最小有向包圍盒的旋轉(zhuǎn)矩陣，能夠使旋轉(zhuǎn)矩陣中的主方向始終沿著對稱平面物體的邊長方向，從而降低了對稱平面物體的位姿估計偏差，提高了對稱平面物體的作業(yè)精準度。

52、本技術的其它特征和優(yōu)點將在隨后的說明書中闡述，并且，部分地可以從說明書中變得顯而易見，或者通過實施本技術而了解。本技術的目的和其他優(yōu)點可通過在所寫的說明書、權利要求書、以及附圖中特別指出的結(jié)構(gòu)來實現(xiàn)和獲得。