本文則試圖打開慣性動(dòng)作捕捉的「眼睛」。通過額外佩戴一個(gè)手機(jī)相機(jī)，我們的算法便有了「視覺」。它可以在捕獲人體運(yùn)動(dòng)的同時(shí)感知環(huán)境信息，進(jìn)而實(shí)現(xiàn)對(duì)人體的精確定位。該項(xiàng)研究來自清華大學(xué)徐楓團(tuán)隊(duì)，已被計(jì)算機(jī)圖形學(xué)領(lǐng)域國(guó)際頂級(jí)會(huì)議SIGGRAPH2023接收。

• 論文地址：https://arxiv.org/abs/2305.01599
• 項(xiàng)目主頁(yè)：https://xinyu-yi.github.io/EgoLocate/
• 開源代碼：https://github.com/Xinyu-Yi/EgoLocate

簡(jiǎn)介

隨著計(jì)算機(jī)技術(shù)的發(fā)展，人體感知和環(huán)境感知已經(jīng)成為現(xiàn)代智能應(yīng)用中不可或缺的兩部分。人體感知技術(shù)通過捕捉人體運(yùn)動(dòng)和動(dòng)作，可以實(shí)現(xiàn)人機(jī)交互、智能醫(yī)療、游戲等應(yīng)用。而環(huán)境感知技術(shù)則通過重建場(chǎng)景模型，可以實(shí)現(xiàn)三維重建、場(chǎng)景分析和智能導(dǎo)航等應(yīng)用。兩個(gè)任務(wù)相互依賴，然而國(guó)內(nèi)外現(xiàn)有技術(shù)大多獨(dú)立地處理它們。研究團(tuán)隊(duì)認(rèn)為，人體運(yùn)動(dòng)和環(huán)境的組合感知對(duì)于人類與環(huán)境互動(dòng)的場(chǎng)景非常重要。首先，人體和環(huán)境同時(shí)感知可以提高人類與環(huán)境互動(dòng)的效率和安全性。例如，在自動(dòng)駕駛汽車中，同時(shí)感知駕駛員的行為和周圍環(huán)境可以更好地保證駕駛的安全性和順暢性。其次，人體和環(huán)境同時(shí)感知可以實(shí)現(xiàn)更高級(jí)別的人機(jī)交互，例如，在虛擬現(xiàn)實(shí)和增強(qiáng)現(xiàn)實(shí)中，同時(shí)感知用戶的動(dòng)作和周圍環(huán)境可以更好地實(shí)現(xiàn)沉浸式的體驗(yàn)。因此，人體和環(huán)境同時(shí)感知可以為我們帶來更高效、更安全、更智能的人機(jī)交互和環(huán)境應(yīng)用體驗(yàn)。

基于此，清華大學(xué)徐楓團(tuán)隊(duì)提出了僅使用6個(gè)慣性傳感器（IMU）和1個(gè)單目彩色相機(jī)的同時(shí)實(shí)時(shí)人體動(dòng)作捕捉、定位和環(huán)境建圖技術(shù)（如圖1所示）。慣性動(dòng)作捕捉（mocap）技術(shù)探索人體運(yùn)動(dòng)信號(hào)等「內(nèi)部」信息，而同時(shí)定位與建圖（SLAM）技術(shù)主要依賴「外部」信息，即相機(jī)捕捉的環(huán)境。前者具有良好的穩(wěn)定性，但由于沒有外部正確的參考，全局位置漂移在長(zhǎng)時(shí)間運(yùn)動(dòng)中會(huì)累積；后者可以高精度地估計(jì)場(chǎng)景中的全局位置，但當(dāng)環(huán)境信息不可靠時(shí)（例如沒有紋理或存在遮擋），就容易出現(xiàn)跟蹤丟失。

因此，本文有效將這兩種互補(bǔ)的技術(shù)（mocap和SLAM）結(jié)合起來。通過在多個(gè)關(guān)鍵算法上進(jìn)行人體運(yùn)動(dòng)先驗(yàn)和視覺跟蹤的融合，實(shí)現(xiàn)了魯棒和精確的人體定位和地圖重建。

圖1 本文提出同時(shí)人體動(dòng)作捕捉與環(huán)境建圖技術(shù)

具體地，本研究將6個(gè)IMU穿戴在人的四肢、頭和后背上，單目彩色相機(jī)固定在頭部并向外拍攝。這種設(shè)計(jì)受到真實(shí)人類行為的啟發(fā)：當(dāng)人類處于新環(huán)境中時(shí)，他們通過眼睛觀察環(huán)境并確定自己的位置，從而在場(chǎng)景中計(jì)劃他們的運(yùn)動(dòng)。

在我們的系統(tǒng)中，單目相機(jī)充當(dāng)人類的眼睛，為本技術(shù)提供實(shí)時(shí)場(chǎng)景重建和自我定位的視覺信號(hào)，而IMU則測(cè)量人體四肢和頭部的運(yùn)動(dòng)。這套設(shè)置兼容現(xiàn)有VR設(shè)備，可利用VR頭顯中的相機(jī)和額外佩戴的IMU進(jìn)行穩(wěn)定無漂移的全身動(dòng)捕和環(huán)境感知。整個(gè)系統(tǒng)首次實(shí)現(xiàn)了僅基于6個(gè)IMU和1個(gè)相機(jī)的同時(shí)人體動(dòng)作捕捉和環(huán)境稀疏點(diǎn)重建，運(yùn)行速度在CPU上達(dá)到60fps，并在精度上同時(shí)超過了兩個(gè)領(lǐng)域最先進(jìn)的技術(shù)。該系統(tǒng)的實(shí)時(shí)示例如圖2和圖3所示。

圖2 在70米的復(fù)雜運(yùn)動(dòng)中，本系統(tǒng)精確跟蹤人體位置并捕捉人體動(dòng)作，無明顯位置漂移。

圖3 本系統(tǒng)同時(shí)重建人體運(yùn)動(dòng)和場(chǎng)景稀疏點(diǎn)的實(shí)時(shí)示例。

方法介紹

圖4 方法總體流程

系統(tǒng)的任務(wù)是從6個(gè)IMU傳感器的朝向和加速度測(cè)量值和相機(jī)拍攝的彩色圖片中實(shí)時(shí)重建出人體運(yùn)動(dòng)、三維場(chǎng)景稀疏點(diǎn)云、并定位人在場(chǎng)景中的位置。我們?cè)O(shè)計(jì)了一個(gè)深度耦合的框架，以充分利用稀疏慣性動(dòng)作捕捉和SLAM技術(shù)的互補(bǔ)優(yōu)勢(shì)。在這個(gè)框架中，人體運(yùn)動(dòng)先驗(yàn)與SLAM的多個(gè)關(guān)鍵組件相結(jié)合，SLAM的定位結(jié)果也回饋給人體運(yùn)動(dòng)捕捉。如圖4所示，根據(jù)功能，我們將系統(tǒng)劃分為四個(gè)模塊：慣性動(dòng)作捕捉模塊（Inertial Motion Capture）、相機(jī)跟蹤模塊（Camera Tracking）、建圖和閉環(huán)檢測(cè)模塊（Mapping & Loop Closing）和人體運(yùn)動(dòng)更新模塊（Body Translation Updater）。以下分別介紹各個(gè)模塊。

慣性動(dòng)作捕捉

慣性動(dòng)作捕捉模塊從6個(gè)IMU測(cè)量值中估計(jì)人體姿態(tài)和運(yùn)動(dòng)。本模塊的設(shè)計(jì)基于我們之前的PIP[1]工作，但本工作中不再假設(shè)場(chǎng)景是一個(gè)平坦的地面，而是考慮在3D空間中捕捉自由人體運(yùn)動(dòng)。為此，本文對(duì)PIP的優(yōu)化算法進(jìn)行了適應(yīng)性的修改。

具體地，本模塊首先通過多階段循環(huán)神經(jīng)網(wǎng)絡(luò)從IMU測(cè)量值中預(yù)測(cè)人體關(guān)節(jié)旋轉(zhuǎn)、速度、腳和地面接觸概率。利用PIP提出的雙重PD控制器，求解人體關(guān)節(jié)最優(yōu)控制角加速度和線加速度。隨后，本模塊優(yōu)化人體的位姿加速度，使其在滿足接觸約束條件C下實(shí)現(xiàn)PD控制器給出的加速度：

其中J為關(guān)節(jié)雅可比矩陣，為與地面接觸的腳的線速度，約束C要求接觸地面的腳的速度要?。ú话l(fā)生滑動(dòng)）。該二次規(guī)劃問題求解可以參考PIP[1]。通過姿態(tài)加速度積分從而獲得人體姿態(tài)和運(yùn)動(dòng)后，可獲得綁定在人體上的相機(jī)的位姿，用于后續(xù)模塊。

相機(jī)跟蹤

相機(jī)跟蹤模塊以慣性動(dòng)作捕捉模塊給出的初始相機(jī)位姿和相機(jī)拍攝的彩色圖像作為輸入，利用圖像信息優(yōu)化相機(jī)位姿，消除位置漂移。具體地，本模塊基于ORB-SLAM3[2]設(shè)計(jì)，首先提取圖像的ORB特征點(diǎn)，并和已經(jīng)重建的稀疏地圖點(diǎn)（下文介紹）利用特征相似度進(jìn)行特征匹配，得到匹配的2D-3D點(diǎn)對(duì)，進(jìn)而通過優(yōu)化重投影誤差的方法優(yōu)化相機(jī)位姿。值得注意的是，僅優(yōu)化重投影誤差可能會(huì)受錯(cuò)誤匹配影響，導(dǎo)致相機(jī)位姿優(yōu)化結(jié)果較差。因此，本文在相機(jī)跟蹤優(yōu)化中融合人體運(yùn)動(dòng)先驗(yàn)信息，以慣性動(dòng)捕結(jié)果作為約束，限制重投影誤差的優(yōu)化過程，以及時(shí)發(fā)現(xiàn)并排除錯(cuò)誤的特征點(diǎn)-地圖點(diǎn)匹配。

記地圖點(diǎn)的世界坐標(biāo)為，與之匹配的2D圖像特征點(diǎn)的像素坐標(biāo)為表示所有的匹配關(guān)系。用

表示優(yōu)化前的初始相機(jī)位姿，則本模塊優(yōu)化相機(jī)位姿R,t：

其中，為魯棒Huber核函數(shù)，將三維旋轉(zhuǎn)映射到三維向量空間，為透視投影運(yùn)算，動(dòng)作捕捉旋轉(zhuǎn)和平移項(xiàng)的控制系數(shù)。該優(yōu)化進(jìn)行3次，每次都根據(jù)重投影誤差將2D-3D匹配分類成正確或錯(cuò)誤，在下一次優(yōu)化中只有正確匹配被使用，錯(cuò)誤匹配則被刪除。通過動(dòng)作捕捉約束提供的強(qiáng)有力的先驗(yàn)知識(shí)，本算法可以更好地區(qū)分正確和錯(cuò)誤匹配，從而提高相機(jī)跟蹤精度。解出相機(jī)位姿后，本模塊提取出正確匹配的地圖點(diǎn)對(duì)數(shù)量，并將其作為相機(jī)位姿的可信度。

建圖和閉環(huán)檢測(cè)

建圖和閉環(huán)檢測(cè)模塊利用關(guān)鍵幀重建稀疏地圖點(diǎn)并檢測(cè)人體是否到達(dá)曾去過的位置以修正累計(jì)誤差。在建圖過程中，我們使用動(dòng)作捕捉約束的光束平差法（Bundle Adjustment，BA）同時(shí)優(yōu)化稀疏地圖點(diǎn)位置和關(guān)鍵幀相機(jī)位姿，并引入地圖點(diǎn)置信度以動(dòng)態(tài)平衡動(dòng)作捕捉約束項(xiàng)和重投影誤差項(xiàng)的相對(duì)強(qiáng)弱關(guān)系，從而提高結(jié)果精度。當(dāng)人體運(yùn)動(dòng)發(fā)生閉環(huán)時(shí)，進(jìn)行動(dòng)作捕捉輔助的位姿圖優(yōu)化（Pose Graph Optimization）以修正閉環(huán)誤差。最終得到優(yōu)化后的稀疏地圖點(diǎn)位置和關(guān)鍵幀位姿 ，用于下一幀算法運(yùn)行。

具體地，本模塊首先根據(jù)地圖點(diǎn)的觀測(cè)情況計(jì)算其置信度，用于后續(xù)BA優(yōu)化。如下圖5所示，根據(jù)觀測(cè)到地圖點(diǎn)的關(guān)鍵幀的位置，本模塊計(jì)算關(guān)鍵幀基線長(zhǎng)度bi和觀測(cè)視角θi以確定地圖點(diǎn)i的置信度，其中k為控制系數(shù)。

圖5 （a）地圖點(diǎn)置信度計(jì)算。（b）相同的基線長(zhǎng)度b1=b2，更大的觀測(cè)視角（藍(lán)色）可以更好地抵抗相機(jī)位姿的擾動(dòng)，導(dǎo)致更小的地圖點(diǎn)位置誤差（e1<e2）。

隨后，同時(shí)優(yōu)化最近20個(gè)關(guān)鍵幀相機(jī)位姿和它們觀測(cè)到的地圖點(diǎn)。其他看見這些地圖點(diǎn)的關(guān)鍵幀位姿則在優(yōu)化中固定不變。記所有可優(yōu)化的關(guān)鍵幀集合為K0，所有固定的關(guān)鍵幀集合為Kf，關(guān)鍵幀j測(cè)到的地圖點(diǎn)的集合記為Xj。記

表示需要優(yōu)化的關(guān)鍵幀朝向和三維位置，

表示地圖點(diǎn)位置。則該動(dòng)作捕捉約束的光束平差法優(yōu)化定義為：

其中，

表示關(guān)鍵幀j的上一個(gè)關(guān)鍵幀，

為動(dòng)作捕捉約束項(xiàng)的系數(shù)。該優(yōu)化要求地圖點(diǎn)的重投影誤差要小，且每個(gè)關(guān)鍵幀的旋轉(zhuǎn)、相對(duì)位置要和動(dòng)作捕捉的結(jié)果相近，其中地圖點(diǎn)置信度ci動(dòng)態(tài)決定了動(dòng)作捕捉約束項(xiàng)和地圖點(diǎn)重投影項(xiàng)之間的相對(duì)權(quán)重關(guān)系：對(duì)于未充分重建的區(qū)域，系統(tǒng)更傾向于相信動(dòng)作捕捉的結(jié)果；反之，若一個(gè)區(qū)域被反復(fù)觀測(cè)到，系統(tǒng)則更相信視覺跟蹤。該優(yōu)化的因子圖表示如下圖6所示。

圖6 動(dòng)作捕捉約束的光束平差法優(yōu)化因子圖表示。

當(dāng)軌跡閉環(huán)被檢測(cè)到后，系統(tǒng)進(jìn)行閉環(huán)優(yōu)化?；贠RB-SLAM3[2]，記位姿圖中的頂點(diǎn)集合為F，邊的集合為C。則動(dòng)作捕捉約束的位姿圖優(yōu)化定義為：

其中，為關(guān)鍵幀j的位姿，為位姿圖優(yōu)化之前關(guān)鍵幀i和j之間的相對(duì)位姿，為動(dòng)作捕捉獲得的相機(jī)位姿初值，將位姿映射到六維向量空間，為動(dòng)作捕捉約束項(xiàng)的相對(duì)系數(shù)。該優(yōu)化以動(dòng)作捕捉先驗(yàn)為指導(dǎo)，將閉環(huán)誤差分散到各個(gè)關(guān)鍵幀上。

人體運(yùn)動(dòng)更新

人體運(yùn)動(dòng)更新模塊利用相機(jī)跟蹤模塊優(yōu)化后的相機(jī)位姿及可信度，更新動(dòng)作捕捉模塊給出的人體全局位置。該模塊使用Kalman濾波器的預(yù)測(cè)-校正算法實(shí)現(xiàn)。其中，動(dòng)作捕捉模塊提供恒定方差的人體運(yùn)動(dòng)加速度，可用于人體全局位置的預(yù)測(cè)（先驗(yàn)分布）；而相機(jī)跟蹤模塊給出相機(jī)位置觀測(cè)和置信度，用于人體全局位置的校正（后驗(yàn)分布）。其中，相機(jī)位置觀測(cè)的協(xié)方差矩陣近似通過匹配的地圖點(diǎn)數(shù)量計(jì)算為如下的對(duì)角陣：

其中為小數(shù)避免除數(shù)為0。即相機(jī)跟蹤中成功匹配的地圖點(diǎn)數(shù)量越多，相機(jī)位姿觀測(cè)的方差越小。利用Kalman濾波算法，最終預(yù)測(cè)人體全局位置。

更詳細(xì)的方法介紹和公式推導(dǎo)請(qǐng)參考論文原文及附錄部分。

實(shí)驗(yàn)

對(duì)比Mocap

本文方法主要解決了稀疏慣性動(dòng)作捕捉（Mocap）中全局位置漂移的問題，因此選取主要測(cè)試指標(biāo)為人體全局位置誤差。在TotalCapture和HPS兩個(gè)公開數(shù)據(jù)集上和SOTA mocap方法TransPose[3]、TIP[4]和PIP[1]的定量測(cè)試結(jié)果對(duì)比如下表1所示，定性測(cè)試結(jié)果對(duì)比如下圖7和圖8所示?？梢钥吹奖疚姆椒ㄔ谌侄ㄎ痪壬洗蠓^前人慣性動(dòng)作捕捉方法（在TotalCapture和HPS上分別提升41%和38%），軌跡與真值的相似度最高。

表1 和慣性動(dòng)作捕捉工作的全局位置誤差定量對(duì)比（單位：米）。TotalCapture數(shù)據(jù)集以動(dòng)作進(jìn)行分類，HPS數(shù)據(jù)集以場(chǎng)景進(jìn)行分類。針對(duì)我們的工作，我們測(cè)試9次并匯報(bào)中位數(shù)和標(biāo)準(zhǔn)差。

圖7 和慣性動(dòng)作捕捉工作的全局位置誤差定性對(duì)比。真值用綠色表示，不同方法預(yù)測(cè)結(jié)果用藍(lán)色表示。每個(gè)圖片的角落中展示了人體的運(yùn)動(dòng)軌跡和當(dāng)前位置（橙色圓點(diǎn)）。

圖8 和慣性動(dòng)作捕捉工作的全局位置誤差定性對(duì)比（視頻）。真值用綠色表示，本文方法為白色，前人工作的方法使用其他不同顏色（見圖例）。

對(duì)比SLAM

本文從定位精度和地圖重建精度兩個(gè)角度分別和SOTA SLAM工作ORB-SLAM3[2]的單目和單目慣性版本進(jìn)行了對(duì)比。定位精度的定量對(duì)比結(jié)果如表2所示。地圖重建精度的定量對(duì)比結(jié)果如表3所示，定性對(duì)比結(jié)果如圖9所示?？梢钥吹剑疚姆椒ㄏ啾萐LAM大幅提高了系統(tǒng)魯棒性、定位精度和地圖重建精度。

表2 和SLAM工作的定位誤差定量對(duì)比（誤差單位：米）。M/MI分別表示ORB-SLAM3的單目/單目慣性版本，On/Off表示SLAM的實(shí)時(shí)和離線結(jié)果。由于SLAM經(jīng)常跟蹤丟失，針對(duì)SLAM我們分別匯報(bào)了完整序列（Full）和成功跟蹤的幀（Tracked）上的平均定位誤差；本文方法不存在跟蹤丟失情況，因此我們匯報(bào)完整序列的結(jié)果。每個(gè)方法測(cè)試9次并匯報(bào)中位數(shù)和標(biāo)準(zhǔn)差。對(duì)于成功跟蹤的幀上的誤差，我們額外匯報(bào)了成功的百分比。如果一個(gè)方法失敗過多次，我們標(biāo)記它為失?。ㄓ谩?”表示）。

表3 和SLAM工作的地圖重建誤差定量對(duì)比（誤差單位：米）。M/MI分別表示ORB-SLAM3的單目/單目慣性版本。針對(duì)三個(gè)不同場(chǎng)景（辦公室、室外、工廠），我們測(cè)試所有重建的3D地圖點(diǎn)距離場(chǎng)景表面幾何的平均誤差。每個(gè)方法測(cè)試9次并匯報(bào)中位數(shù)和標(biāo)準(zhǔn)差。如果一個(gè)方法失敗過多次，我們標(biāo)記它為失?。ㄓ谩?”表示）。

圖9 和SLAM工作的地圖重建誤差定性對(duì)比。我們展示了不同方法重建的場(chǎng)景點(diǎn)，顏色表示每個(gè)點(diǎn)的誤差。

除此之外，本系統(tǒng)通過引入人體運(yùn)動(dòng)先驗(yàn)，針對(duì)視覺跟蹤丟失的魯棒性大幅提高。在視覺特征較差時(shí)，本系統(tǒng)可以利用人體運(yùn)動(dòng)先驗(yàn)持續(xù)跟蹤，而不會(huì)像其他SLAM系統(tǒng)一樣跟蹤丟失并重置或創(chuàng)建新地圖。如下圖10所示。

圖10 和SLAM工作的遮擋魯棒性比較。右上角展示了真值軌跡參考。由于SLAM初始化的隨機(jī)性，全局坐標(biāo)系和時(shí)間戳沒有完全對(duì)齊。

關(guān)于更多的實(shí)驗(yàn)結(jié)果，請(qǐng)參考論文原文、項(xiàng)目主頁(yè)及論文視頻。

總結(jié)

本文提出了第一個(gè)將慣性動(dòng)作捕捉 (inertial mocap) 與 SLAM 相結(jié)合，實(shí)現(xiàn)實(shí)時(shí)同時(shí)進(jìn)行人體動(dòng)作捕捉、定位和建圖的工作。該系統(tǒng)足夠輕量，只需要人體穿戴稀疏的傳感器，包括 6 個(gè)慣性測(cè)量單元和一個(gè)手機(jī)相機(jī)。對(duì)于在線跟蹤，mocap 和 SLAM 通過約束優(yōu)化和 Kalman 濾波技術(shù)進(jìn)行融合，從而實(shí)現(xiàn)更準(zhǔn)確的人體定位。對(duì)于后端優(yōu)化，通過將人體運(yùn)動(dòng)先驗(yàn)融入SLAM 中的光束平差優(yōu)化和閉環(huán)優(yōu)化，進(jìn)一步減少定位和建圖誤差。

本研究旨在將人體感知與環(huán)境的感知融合在一起。盡管本工作主要關(guān)注定位方面，但我們相信，這項(xiàng)工作邁出了聯(lián)合運(yùn)動(dòng)捕捉和精細(xì)環(huán)境感知與重建的第一步。