????????本算法是一個(gè)經(jīng)典的，針對(duì)SLAM（simultaneous localization and mapping 即時(shí)定位與地圖構(gòu)建）問(wèn)題而提出的算法。該算法的提出者是Feng Lu和Evangelos Milios，他們?cè)诒舅惴ㄖ虚_創(chuàng)了通過(guò)全局優(yōu)化方程組以減少約束引入的誤差來(lái)進(jìn)一步優(yōu)化地圖配準(zhǔn)結(jié)果的先例

????????

????????探索位置環(huán)境的機(jī)器人需要根據(jù)傳感器測(cè)量的數(shù)據(jù)建立周圍環(huán)境的地圖模型。為了整合傳感器不同幀的數(shù)據(jù)，必須對(duì)數(shù)據(jù)進(jìn)行正確的對(duì)齊。先前的方法通常使用增量的方式進(jìn)行對(duì)齊，即每個(gè)局部數(shù)據(jù)幀與累積的全局?jǐn)?shù)據(jù)幀進(jìn)行對(duì)齊，然后合并到模型中。由于模型的每個(gè)部分都是獨(dú)立更新的，而數(shù)據(jù)在進(jìn)行配準(zhǔn)時(shí)又存在誤差；因此，這種方法可能會(huì)最終產(chǎn)生一個(gè)各部分不一致的模型。為了保持這種一致性，Lu和Milios建立了一個(gè)基于極大似然準(zhǔn)則的算法流程來(lái)最優(yōu)地組合所有的空間關(guān)系。該算法使用所有的空間關(guān)系作為約束來(lái)同時(shí)求解不同數(shù)據(jù)幀的姿態(tài)，優(yōu)化全局對(duì)齊的結(jié)果。

通過(guò)使用所有數(shù)據(jù)幀之間的位姿關(guān)系（pose relations）作為約束來(lái)估計(jì)不同數(shù)據(jù)幀的全局位姿（global pose）。這里，“全局位姿”代表每一個(gè)數(shù)據(jù)幀在采集時(shí)對(duì)應(yīng)的機(jī)器人（或者說(shuō)傳感器）在全局坐標(biāo)系下的位姿。而“位姿關(guān)系”代表兩個(gè)姿態(tài)之間的估計(jì)空間關(guān)系。 給定成對(duì)的姿態(tài)關(guān)系，可以構(gòu)建起一個(gè)網(wǎng)絡(luò)（圖結(jié)構(gòu)）。在形式上，這個(gè)網(wǎng)絡(luò)由節(jié)點(diǎn)和節(jié)點(diǎn)對(duì)之間的鏈接組成。網(wǎng)絡(luò)上的節(jié)點(diǎn)代表機(jī)器人（或傳感器）在其軌跡上的位姿。節(jié)點(diǎn)之間的鏈接代表不同節(jié)點(diǎn)之間的位姿關(guān)系。該方法將節(jié)點(diǎn)之間的鏈接分為兩種：首先，如果兩個(gè)姿態(tài)沿機(jī)器人運(yùn)動(dòng)的路徑相鄰，則稱這兩個(gè)節(jié)點(diǎn)之間存在一個(gè)弱連接。其次，如果在兩個(gè)姿態(tài)下進(jìn)行的掃描有足夠的重疊，則稱這兩個(gè)節(jié)點(diǎn)之間存在強(qiáng)鏈接。 從該網(wǎng)絡(luò)中構(gòu)造一個(gè)目標(biāo)函數(shù)，以所有的位姿作為變量。網(wǎng)絡(luò)中的每一條邊都可以被轉(zhuǎn)化為目標(biāo)函數(shù)中的一個(gè)函數(shù)項(xiàng)。本算法最后通過(guò)最小化這個(gè)目標(biāo)函數(shù)來(lái)一次性求解出所有的位姿。

假設(shè)給定一個(gè)不確定測(cè)量的網(wǎng)絡(luò)，該網(wǎng)絡(luò)包含n+1個(gè)節(jié)點(diǎn) ? ? ? ? ? ? ? ? ? ? ? ? ? ?而兩個(gè)結(jié)點(diǎn)和之間的邊 ? ?表示這兩個(gè)位置間可測(cè)量的差異。 ?給定一組節(jié)點(diǎn)之間的觀測(cè)值 ? ? ? 和協(xié)方差 ? ? 。如果假設(shè)所有的觀測(cè)誤差都是符合高斯分布且相互獨(dú)立的，那么該算法的優(yōu)化準(zhǔn)則就相當(dāng)于最小化下面的馬氏距離： ? ? ? ? ? ? ? ? 考慮利用上面的目標(biāo)函數(shù)公式給出簡(jiǎn)單線性問(wèn)題的解析解，上述優(yōu)化準(zhǔn)則可以轉(zhuǎn)化為：

點(diǎn)云庫(kù)PCL 的LUM類集成了本算法，可以通過(guò)這個(gè)模塊兒實(shí)現(xiàn)多視角點(diǎn)云的對(duì)齊。上圖對(duì)樓道場(chǎng)景掃描點(diǎn)云和兔子點(diǎn)云對(duì)齊的結(jié)果。