機器視覺小教室 Aurora Vision Studio 大家一起玩 3D【18】利用QRCode加PnP透視投影求解+2D匹配，賦能2D相機使之辨識平面任意擺放物的3D位姿

我們事先先校正這個2D相機取向 然後去讀取這個4個QR code 然後用這個PMP的演算法 然後這演算法我們可以算出根據透視的關係 算出這個transformation這個矩陣4x4的矩陣 這個矩陣描述的是一個相機周標系 相機周標系在這個地方 到這個四邊形 到這個四邊形的這個相對關係 這個描述的是這個東西 這個相對關係 因此透過這樣子的關係 那我們就可以把這個四邊形 以及load讀取一個Golden Pass 這邊這個Golden Pass有4個節點 1、2、3、4 這個Golden Pass 那這個Golden Pass怎麼load進來的 就是讀取這個QR code 這個QR code目前這個ID叫做編號1 編號1以後可以讀取1號的Golden Pass 它是存在檔案裡面 把它讀進來以後 這個Golden Pass它可以根據這個模擬板 這個模擬板到哪裡它就跟著跑到哪裡 跑到哪裡可以跟蹤這個模擬板 那你會不會想說 那既然這樣的話 我可不可以不要用Golden Pass 我可以不要Golden Pass 那我能不能直接辨識這個上面的物體 比如說我現在丟了一個物體在這 我可不可以 這物體其實不是很確定說 它在這個模擬上的哪個位置 我們二視覺不是可以去做這個 matching或是叫匹配的這個行為 好 那我能不能匹配這個 比如說這個L型的這個弓箭 匹配這個L型弓箭 那從2D上匹配以後 幫我算出這個3D的點在哪 好 這個 好 那這樣子的話 這個的話我已經把這個應用寫好了 類似是像這樣 好 這個我是用這個匹配的這個演算法 好 那匹配到這個L型 好 這個L型辨識到以後 我可以辨識到這個2D的座標點 辨識到這個2D的座標點 好 那比如說我現在做一個 好 這個亂序的這個抓去 亂序抓去 但是我們限制在平面的 好 平面的 這在模具版上平面的亂序抓去 但是用了個2D相機 好 但是我需要提供一個三維的座標 好 但是我的相機是架設傾斜的 好 我的相機是架設傾斜 我沒有暫時沒有辦法架設 這個五視角的這種情況 那如果是這種情況 剛好我們已經架設這樣子的一個場景 我們有四個這個QR Code 好 那這個 這個比如說假設它是一個 AGB的這個臺車 然後它走過來 你辨識到這個QR Code 辨識到這個QR Code取得它 相對於相機的這個相對座標了以後 好 那我要從這個載盤 這個AGB臺車這個載盤上面 抓取這兩個弓箭 好 這個弓箭呢 相對於相機 它有這個XY 這一組座標 第一個弓箭是這一組 第二個弓箭是這一組 下面這種座標 那這個是怎麼算出來的呢 怎麼算出來 好 我這個是即時的 我移動這個弓箭 它都是一直有在計算 我們可以看下往左 往右 都相對於這個 這個QR Code 這個長方形藍色框框 是QR Code的包圍框 這個橘色是QR Code的X軸 綠色是QR Code的Y軸 紫色是QR Code的Z軸 好 那這個是怎麼實現出來 我們就來說明一下這個原理 這個原理呢 前面都是一樣的 前面都一樣 好 讀取這個校正檔 然後取向 然後辨識這個QR Code 這在上一個階段 我們都已經知道 然後利用這個QR Code這個Landmark 去Project出這個 相機的這個四乘四 相機相對於這四個QR Code 包圍這個矩形這個Transformation 好 那我們順便把這個 裡面這個PnP的這個R跟T 把它拋出來 這個等一下會用到 把它拋出來 R是一個三乘三的矩陣 T是一個三乘一的矩陣 我們把它拋出來 好 這個也是上次就有 好 那我們要做一個動作 我們要做一個動作 這個動作叫做 因為我們現在相機是斜的拍 相機是斜的拍 好 我們拍攝出來它這個東西 它是一個透視投影 它是一個 假設它可能接近一個 T型的這種狀態 我們可以看到這個行李箱 它不是正方形 不是直角的 那這會造成什麼 我們要做算匹配的時候 會造成什麼問題 就是我可能 這個弓箭放的 離鏡頭比較近的地方的時候 這比例會比較大 那離鏡頭遠的時候 比例會變小 那我們在2D匹配的時候 可能會匹配不到 所以我們要先做一個矯正的動作 矯正 那怎麼矯正 就是我要把這樣子的圖形 把它矯正成這個樣子 下面這張 然後用下面這張圖 再去匹配這兩個東西 匹配這兩個東西 那怎麼矯正 我們就要用到這個叫做 Remove Perspective 這樣子的一個算法 好那這算法怎麼實現 我們實現的方式 其實就是用這兩個工具 這個叫做 第一個叫做 Create Perspective Map 這個東西要怎麼用 就是它要你給它兩個Path 一般來說是一個四邊形的Path 這個四邊形的Path 那這四邊形的Path怎麼來 對不對 因為你現在剛好有這四個焦點 所以我們就是這樣子 看你是要這樣過來 還是這樣過來 OK 這樣都OK 反正它是要一個包起來 好那第二個動作 就是我們需要有一個輸出 好輸入要是原始的Path 好原始Path 好原始Path你要這樣 1 2 3 4 好回去 好那輸出的話就是 1 2 3 4 好回去 好所以你要給它一個 那個原始Path 跟一個輸出的Path 好原始Path怎麼來 我們上面已經解釋過 我們就直接把這個 直接把這個Landmark2D Landmark2D就這四個點 這四個點 我們直接Make Path 好把它 連成這個Path的資料結構 好那輸出的這個Path 輸出的Path 我們不是有Landmark X的Pitch 跟Landmark Y的Pitch 這個指的是什麼意思 指的是我這個的 真實世界的尺寸 跟這個的真實世界的尺寸 好這個尺寸 我們給它一個比例尺 給它一個比例尺 好比例尺 好這比例尺目前是20 目前是20 好所以我們真實世界尺寸 我們換算得到乘上20個Pixel 這個自己去懂 就是我們 你換算完以後 會影響影像大概有多大 好那我們就做一個計算 好新的影像的大小是 20乘以Landmark X 好新的影像的高度是 20乘以Landmark Y 好所以 這個新的影像尺寸都出來 好那我們就要算算這個 新的Path的四個點 第一個點叫做0 0 第二個點叫做 好這個是Width 0 好第三個叫做0 Height 好第四個點叫做 Width Height 好然後把這個四個點 變成是一個集合 好變成集合了以後 我們就掌住這個新的Path 然後丟給這個Perspective Map 就是讓他伸出這個Map出來 好有這個Map以後 你就可以Remap Image 他就得到 會得到一個這張影像 好這個是Remap完的影像 好那你得到影像以後 不要忘記就是要把它 要把這個Map 把它丟到外面去 因為等一下會用到 因為你轉完還要再還原回去 好那我們接下來就是 我們要用這一張影像 去做一個2D匹配的行為 我們要做一個2D匹配 好所以2D匹配 我們是針對 好這張影像 好用這張影像去做一個2D匹配 好那這匹配很簡單 要怎麼用的話 就是詳細可以參考我們這個 2D的這個教學 在另外一個地方 影片裡面會有這個2D的教學 我們就建立這個2D的模板 我們要用這個矯正完影像 建立2D模板 好建立完2D模板以後 我們去匹配這兩個東西 好那我們輸出的這個 我們輸出的這個特徵點 他是在 我們故意把它輸出在這個地方 好這個是我們 故意想要輸出的特徵 故意在這個圓 中間那個圓的空間 好那我們就把這個圓 把這個圓 用這個rectangle center的 圓心的座標把它算出來 把圓心的座標 把它算出來 好算出來以後 我們前面不是有這個MAP 好把這個MAP把它丟進來 那我們算出了這個圓心座標以後 我們就onMAP onMAP 我們有這MAP以後算出 變形矯正變形過的圓心座標 那我們就把onMAP回 沒有矯正變形的影像上 好這就是這兩個點 好你要在這個 這個影像Domain 辨識到兩個點 你當然要把它丟回去 丟回原本的沒有矯正變形 因為我們等一下要拋回3D 是用原始圖去拋的 好所以要onMAP回來 onMAP回來以後就會出現這兩個 好那最後一步就是 Transform 2D points to 3D 這個是 這個單元最重要的一個部分 好它的原理是利用 矯正內場以及外場資訊 將2D匹配點投移到3D的空間 好這個是比較複雜一點 好那我們來看一下 這裡面有很多的這個矩正的 相乘的運算 矩正相乘運算 好是用一些數學的方法 去把它解出來 好那這個原理大致說明一下 好那這個原理 原理大概是這樣 我們這個相機 這個是相機的pixel 我們剛才辨識這個點擊 就是辨識到它的pixel pixel在這個地方 好那我們一般來說 我們這個叫做正向投影的話 是從這邊要投過來 我們有這個 剛纔有這個camera matrix是這個 然後還有這個RT 我們形成一個Transformation一個外層 好這個我們已知我真實世界有個點 有個SYZ 我們乘上這一堆了以後 我們就可以知道它投影在什麼地方 這個叫正向投影 但是如果我們要做逆向投影的時候 你什麼叫逆向投影 我們知道這個pixel 知道這個pixel 也知道知道 也知道 但是我想要知道這個XYZ在哪裡 好這個就是逆向投影 好逆向投影 但是逆向投影呢 我們要有一些約束條件 我們有一些約束條件 就是我們把Z等於0 我們要把Z等於0 好這樣才能求解出來 好這個是直到這個是直到 然後這是直到 好那首先你要先把這個S也求解出來 求解出來 好最後我們就可以知道這個XY 並且Z等於0 好求得Z等於XY 並且Z等於0以後 再用剛才這個相機座標器 跟弓箭的座標器這個轉換關係 把它丟到世界3D的座標上 好那我們就回到來 稍微解析一下這個算法 好這個算法的第一個就是 我們要從這個全域變數裡面 全域變數在這個地方 就是我們有這個camera matrix camera matrix就是內參 那我們另內參的M 大寫的M 好這個是內參camera matrix 好另它的M 好接下來就是 我們從外面不是拋出一個R 就是我們解PMP用到這個 好我們先把這個R做一個反矩陣 好因為我們要求解這個方程 好反矩陣 好接下來把這個內參也求解一個反矩陣 好那現在把這兩個反矩陣相乘 好相乘 乘起來乘起來以後會得到這個 invert R跟invert M 這個相乘的矩陣 三乘三的矩陣 好那接下來再準備一個東西 把這個invert R乘上這個 PMP得到這個平移量T 好會得到一個invert R 再乘上T的一個矩陣 好那接下來 下一個步驟就是把2D的XY座標 好這是2DXY 2DXY座標 我們是從外面丟進來 就是這個這兩個弓箭 好看你要三個四個都可以 好把它丟進來 那我們先把它轉換成矩陣的格式 好轉換成矩陣格式 矩陣格式因為我們只有XY 好第三為 補一 好然後變成一個三乘一的矩陣 好那這個矩陣的名稱叫做pixel 就是剛才這個公式裡面這個uv 好那接下來就是 我們剛纔不是有這個invert R乘invert M 好就把這個跟這個uv把它乘在一起 好那乘完了以後 我們就獲得 我們要開始求解這個S S是一個比例關係 好S是一個比例關係 好那求解的時候 我們就因為我們有約束條件 約束條件 好所以約束條件 你自己去推導的時候 你就會發現S 它就等於invert R乘T的第三個元素 好它是矩陣的第三個元素 除以invert R乘invert M 再乘pixel的這個矩陣的第三個元素 這是這兩個東西 好這個第三個元素 除以另外一個第三個元素 就可以獲得這個S 好那最後呢 我們有S有一堆矩陣 我們就可以把這個point的3D把它求出來 求出來就是S再乘上這個矩陣乘上這個矩陣 再乘上這個uv 再減掉invert R乘T 就可以把它求解出來 那求解出來以後 我們就可以make一個point的3D make完以後最後呢 它其實是算在這個 它會跑到這個座標繫上面 跑到這個座標繫上 那你要把它丟到這個空間上 所以我們就把這個最後面那個丟過去 跟那個golden pass一樣 一樣要把它丟上去 丟上去我們就可以把它輸出 丟上去有輸出就會在這個地方 這個就是我們用2D視覺 2D視覺 好2D視覺 然後透過這個透視投影 透視投影 然後加上這個netmark 然後用2D匹配去產生這個任意的抓取點 就是直接去匹配這個弓箭 那這弓箭可以隨 只要在這個料盤的裡面 你就可以隨意的擺放 然後獲得這個弓箭的這個三維座標系 那三維座標系是基於哪裡 基於這個相機的這個000 好我們就可以知道 這個74的 Z74的意思是說 它跟相機距離大概是742mm 然後這個799 跟相機距離大概是799mm 大概是這個意思