機器視覺小教室 Aurora Vision Studio 大家一起玩 3D【17】利用QRCode加PnP透視投影求解，賦能2D相機使之辨識模治具的3D位姿

我現在要介紹一個就是用2D相機辨識這個QR Code 然後可以推測這個3D姿態的一種應用方式 就是可以看到說這邊有一個2D的相機 它這個解析度是5472x3648 那這邊搭配了一個16mm的鏡頭 然後它是一個2D的相機 可以看到這是一個2D影像 那我們的目標就是 我們現在有一個類似像這樣子的模具板 這個模具板呢 我把它拿起來靠近 這個上面被貼了4個QR Code 那第一個目標就是我們可以辨識這個QR Code的ID 因為我們都知道說QR Code 我可以去裡面去藏任意的文字 我們就會辨識到這個QR Code的ID 第二個呢 我們可以辨識這個4個QR Code 在2D影像上的這個座標點 把這個4個座標點把它連起來 連起來以後傳給這個2D的視覺系統 然後辨識出在3D的姿態 3D的姿態把它辨識出來 這個3D姿態它是相對於哪裡 我們來解釋一下 這個框框就是這個QR Code 在相對於這個2D相機的相對關係 那2D相機在哪裡 2D相機可以看到這邊有兩個X X朝右這邊有看到一個X 0跟0的這個交叉點 所以你的2D相機有一個XY 然後Z是朝這個方向 假設你2D相機 這個是2D的相機 以自己為中心往前面看 你可以看到跟這個視野沒同 你可以看到這個東西在這裡 這個東西在這個地方 那我把這個框框隱藏起來 你也可以認為說這個東西它的XYZ 這個東西的XYZ的座標系 橘色叫做X 橘色叫X 然後綠色叫Y 然後紫色叫Z 那我在圖上畫的相當於這個東西的X 這個東西的Y 這個東西的Z Y不要寫在這 Y寫在這 這個東西就相當於在圖上看到的這個 那這個淺藍色這個是什麼 淺藍色這個是什麼 這個是假設你說有 現在有一個抓取的路徑 抓取的路徑或是軌跡 這個東西你想要抓它的時候 因為你的東西是擺在 比如說我現在擺的這個 我這個料盤 這個料盤裡面一定是固定擺的這個料 那你的抓取要在這個中心 所以我故意設計 我從這樣外面走過來 到中心把它抓起來 這時候你現在換另外一個料盤 你現在換另外一個 我現在換成2號的料盤 這上面寫2號的料 然後這個2號的料盤 2號的料盤 其實要抓的東西 它其實在這裡 所以我的抓取路徑 我會到這個地方 比較前面一點 就把它抓起來 到這個地方把它抓起來 所以每一個料盤 有對應它自己該放置的物體 你怎麼知道這個物體叫做2號 怎麼知道這個 因為QR Code可以藏資訊 所以從QR Code裡面找出來 好 那我們已經知道說 QR Code可以達到這個效果 好 那它這個3D是怎麼來 這個3D 我們可以看到這個下面 這邊有一個四層次的矩陣 四層次矩陣就是說 你辨識到這四個框 這個四個QR Code 包圍的這個矩形框 它可以幫你算出這個 相機這個包圍的這個矩形 跟相機的這個相對關係 好 那我們再解釋清楚一點 其實就是指這個座標 這個座標 指這個座標 以及這個座標到相機這個座標 它們兩個的 這個四層次指的是這個意思 相機到這個座標系的這個相對關係 好 那至於你的相機 到機器手這個相對關係 這個不在目前的這個示範的討論範圍 我們現在指的是 我只可以算出 它跟相機的這個相對關係 而且這是三維的 我們是三維化的 是用四等式來表示 然後由左邊的三乘三代表旋轉 右邊的這個代表平移 好 那這個具體來說 它其實在原理來說 是在解什麼樣的問題 我們可以看一下 它其實是在解一個叫做PNP 它其實是在解這個PNP的問題 好 那如果上網查的話 就是你可以查到這個PNP問題 那什麼叫PNP 不是指那個sensor或是PLC 接點的那個什麼NPNP 跟這個沒有關係 它指的是這個perspective N point的縮寫 perspective叫透視 透視的意思 這一個P是指透視 然後NP是有N個點 它這個原理是說 我現在已經知道 我現在已經知道這個K 這個K K是指相機的內參矩陣 相機的內參矩陣 意味著我的2D相機 我的2D相機 必須要事先做一個校正 事先要做一個校正 有了校正以後 我就要獲得這個K 好 那我現在辨識到這個QR code 我現在有四個QR code 這四個QR code 有四個造型點 好 那表示我有四個pixel 好 一 二 三 四 四組pixel UV代表pixel 它的叫做相機上的投影 好 有四個pixel以後 我們就可以假想出四個XYZ 那其實我就把它當作說 這邊叫0 0 0 這邊叫做 看真實它這邊量出來是多少 100 0 0 這個叫做200 200 0 0 200 0 這個叫做100 200 0 就基於這個座標式 基於這個物體的座標式 好 那表示 再重劃一下 這個是已知的 它其實是在求解這個 rotation跟translation 它是要求解這個rotation跟translation 那它基於什麼樣的理論去求解 基於透視的變形理論 基於透視的原理 那透視的參數哪裡呢 藏在這個內參矩這裡面 好 是為什麼它原理可以做到這個 我們可以知道說 假設我從側面看 我相機朝這邊拍 好 這是相機 然後你拍一個板子 拍一個板子 正方形的板子 但是你的畫面 我畫面畫在這裡 我的畫面拍起來都會是這樣 會變成T型的 它會變成T型 好 那你如果再歪一個角度 歪一個角度 它不只是會T型 也會變成這樣子 我們在畫那個什麼 那個美術的時候 不是都有學什麼 透視投影 它是基於這個 用這個四邊形 它這個變形量去反推相機 跟這個代測物 在空間中的一個相對關係 那要求解這個東西 至少要三個點 比如說三組XYZ 三組V 好 那我們這邊設計了四個點 好 那第一個步驟 就是我們要先做一個內參校正 內參校正類似像這樣 好 內參校正就是 你必須要拿這個校正板 必須要拿這個校正板 偵測 好 那這個整個畫面 整個畫面必須要都涵蓋到 而且必須要有一點傾斜的 好 那這個其實就是為了 讓它正確可以去 偵測到這個相機的這個 Perspective的這個效益 好 那這個校正完了以後 校正完了以後 它就會把這個 把這個存在這個參數檔裡面 存在參數檔裡面 可以看到這個程式 校正完了以後 Save 把這個內參跟內參友檔 把它存在這個參數檔裡面 好 那這個會跑到這個地方來 會有當時校正的這個圖檔 圖檔就是每個位置都要拍 還有一些旋轉跟傾斜 好 那會把這個程式已經寫好了 把這個東西存下來 然後最主要就是這個Camera Matrix 這個東西 好 那在接下來 就是我們去 去讀取這個校正檔 好 那 假設我現在沒有相機 我用讀圖檔的方式 好 這個是 假設這是相機拍的圖 相機拍的圖 好 那讀取這個校正檔 好 我就把這個東西讀起來 讀起來以後存在權益辨識裡面 好 然後拍照 拍照的第一個步驟就是 我們要 辨識這個QR Code 好 辨識這個QR Code 好 所以用這個 內建的這個辨識QR Code的這個工具 好 那接下來我們要 排序這個QR Code 好 這有四個嘛 那你怎麼知道哪一個是哪一個 你要排序 好 那這個讀出來的順序是什麼 我們可以看一下 我們看一下讀出來是什麼東西 好 這四個QR Code 我們依序讀出來是 好 這個四個QR Code 那我們故意讓這個D碼 D碼 好 這個D碼都一樣 全部都是1 那這個1叫做ID 模具的ID 好 那後面會有4個叫做 1 2 3 4 那我們希望呢 我們希望就是 這個叫1 好 ID我們已經知道 ID叫做1 好 那1 我們希望這個左上角 這個叫1 這個是2 這個是3 這個是4 這個是 但是有時候 好 他現在是先 所以他是排在這個 他先抓到這一顆 再抓到3的這一顆 再抓到這一顆 然後他會變成 這樣子 這樣子 這樣子 好 這個順序我們不要 我們要想要改成 讓他變成 這樣子 這樣子 這樣子 我們要這樣順序把他排過來 所以這邊做了一個排序的動作 排序的動作 Sort 好 那我們固定一定要抓4個 少一個都不行 因為少一個就 演算法就 不吻合了 會算錯 所以這邊做了一些防呆的處理 讓少一個贏 如果是少一個的話 就要辨識成NG 好 那接下來的重點 就是這個PMP Project Landmark 3D 這個 好 PMP的話演算法是在這 那這個是套用這個CV 的這個PMP的演算法 好 那你給定這個 Object Point跟Image Point Image Point就是這4個 QR Code的中心點 那Object Point 這個是虛擬出來的 虛擬出來的 好 怎麼虛擬出來 就是我們這裡的這個前面 我們要先用尺去量 或者說你在做設計的時候 好 目前我 因為我這個是移導型 移出來的 沒有很準 就是 XP取樣出來是10.5mm 什麼叫10.5 10.5就是 這個中心點到這個中心點 好 100.5mm 我本來想要100 可是量出來 印出來它多了0.5 好 那接下來這個 我這個叫做Y-Pitch 好 Y-Pitch 我們要知道這個實際的尺寸 好 這個Y-Pitch是多少 好 Y-Pitch目前量出來 是201mm 量出來沒有 那我在設計的時候 在設計稿上面 是設計的200 因為印表機沒有很準 那這個 如果是用加工的話 就會比較準 好 弄出來以後 那我們就可以去造假 也不說造假 就是在這裡 好 我們去虛擬這個 虛擬這個LANDMARK LANDMARK中心點的這個3D的 座標 好 所以就是 第一個座標叫做0 LANDMARK Y 0 2 0 1 然後Z都是0 因為我就要假想 這個LANDMARK是在 這個模具的平面上 好 第二個點叫做 LANDMARK X 跟LANDMARK Y 然後0 第三個是0 0 0 然後LANDMARK X 0 0 這樣子 然後就一組 然後把這一組呢 最後就把它傳出來 傳出來去給這個PMP 去使用 好 PMP使用了以後 它會獲得兩個 會獲得什麼東西 這個算出來以後 它會獲得 這個 R的向量跟V的向量 R的方向叫做旋轉向量 V的話是平 好 那我們用這個 矩陣工具 去把它 轉成這個四乘式的矩陣 轉成四乘式的矩陣 就是這個東西 那最後就可以畫圖 就可以畫圖 畫圖的話就是這個 把這個XYZ XYZ 就是這個 這個座標軸 把這個座標軸 模具的這個座標軸 把它畫上去 以及這個框框 LANDMARK的框框 好 那畫上去的時候 但是我們都是用 物體的座標去畫 就是X 0 0 0 X 0 0 0 X 然後模具也是 直接畫框框的 理想的焦點 好 那 我們因為已經用PMP 算出這個四乘式矩陣 所以這個理想焦點 乘上這個四乘式矩陣 就會跑出相對於 它就把它丟到 這個相機座標繫上面 丟上去以後 它才會畫在這個地方 好 那這個路徑呢 我們有這個LANDMARK 那LANDMARK裡面可以讀出 我們可以看一下 LANDMARK裡面 它帶的什麼資訊 LANDMARK XPG YPG 還有一組的 2D的Pixel的 就是這四個點 一組的3D的這四個點 好 那我們要這個ID 這個ID的話 我們就在這個路徑檔 ID加點Text 這ID加點Text 就是這個路徑檔 1號的Text跟2號的Text 那你如果 就可以到1到99 1到99 那如果覺得位數太少的話 就是這個QR Code裡面 資訊再加一點 多加一點 目前是取兩位 好 這個 第一個路徑點 第二個路徑點 第三個路徑點 第四個路徑點 這個事先可以把它設計 好 這個路徑點 是沒有個數的限制 就是XYZ XYZ 照順序排 好 就讀取對應的路徑檔 好 讀取對應路徑檔以後 這個路徑檔 其實在這個地方 好 這個也都是 相對於模具的 這個理想的路徑檔 好 理想的路徑檔 你經過這個相對關係 轉換 就要把它丟到 相機的座標系 然後就產生這個 叫做Robopass 產生這個Robopass 我們可以使用 如果少一個 它就會仿代 就是辨識失敗 好 那我們連接 真實的相機 連接真實的相機 好 這個是ID2 然後再換一個ID1 好 就可以切換 不同的路徑檔