1. <strong id="7actg"></strong>
    2. <table id="7actg"></table>

    3. <address id="7actg"></address>
      <address id="7actg"></address>
      1. <object id="7actg"><tt id="7actg"></tt></object>

        SLAM就業(yè)問(wèn)題匯總復(fù)習(xí)

        共 21768字,需瀏覽 44分鐘

         ·

        2023-11-07 12:52

        相似變換仿射變換射影變換的區(qū)別


        相似變換7自由度是因?yàn)?+1,多了一個(gè)尺度。


        仿射變換12自由度是因?yàn)橐粋€(gè)平移變換(t)和一個(gè)非均勻變換(A)的復(fù)合,A是可逆矩陣,并不要求是正交矩陣。9+3=12。不變的是平行線(xiàn)比例和長(zhǎng)度,體積比。


        射影變換15自由度是因?yàn)?6-1=15,扣除一個(gè)全局尺度(整個(gè)矩陣除以右下角為1)。不變的是重合關(guān)系。



        單應(yīng)矩陣和基礎(chǔ)矩陣的區(qū)別


        視差和深度的關(guān)系


        z=fb/d,f是焦距,b是基線(xiàn)(baseline,兩相機(jī)光圈中心的距離),d是視差(disparity)。視差最小為一個(gè)深度,因此雙目深度存在理論上的最大值,由fb確定。



        二值圖,最大聯(lián)通區(qū)域


        兩次掃描法:


        這塊復(fù)制維基百科的內(nèi)容:連通分量標(biāo)記 (內(nèi)網(wǎng)打不開(kāi),看我下面的)





        最后連起來(lái)。



        注意這個(gè)4領(lǐng)域和8領(lǐng)域,只是上面的點(diǎn),不要下面的。


        區(qū)域生長(zhǎng)法:



        梯度下降法,牛頓法和高斯牛頓法優(yōu)劣




        邊緣檢測(cè)算子


        1.sobel 產(chǎn)生的邊緣有強(qiáng)弱,抗噪性好。用 f '(x) = f(x + 1) - f(x - 1) 近似計(jì)算一階差分。


        2.laplace 對(duì)邊緣敏感,可能有些是噪聲的邊緣,也被算進(jìn)來(lái)了。用二階差分計(jì)算邊緣,f '(x) = f(x - 1) - 2 f(x)+ f(x + 1)


        3.canny 產(chǎn)生的邊緣很細(xì),可能就一個(gè)像素那么細(xì),沒(méi)有強(qiáng)弱之分。


        canny方式:


        1.高斯濾波器平滑圖像。
        2.一階差分偏導(dǎo)計(jì)算梯度值和方向。
        3.對(duì)梯度值不是極大值的地方進(jìn)行抑制。
        4.用雙閾值連接圖上的聯(lián)通點(diǎn)。


        Harris:


        R=det(M) - k(trace(M))^2其中,det(M) = λ1λ2,trace(M) = λ1+λ2


        當(dāng)|R|很小時(shí),即λ1和λ2很小時(shí),該區(qū)域是平面。


        當(dāng) R < 0時(shí),即λ1遠(yuǎn)遠(yuǎn)大于λ2或者λ2遠(yuǎn)遠(yuǎn)大于λ1時(shí),該區(qū)域是邊緣。


        當(dāng) R很大時(shí),即λ1和λ2都很大且近似相等,該區(qū)域是角點(diǎn)。



        BA算法的流程


        構(gòu)造代價(jià)函數(shù)(比如重投影誤差)


        令雅克比矩陣為


        雅克比子矩陣為



        之后構(gòu)建H矩陣,可以利用高斯牛頓法求解。使用舒爾補(bǔ)操作加速求解。


        如果限制窗口大小,用舒爾補(bǔ)操作marg掉前面不再需要的變量,同時(shí)把其中的聯(lián)系逐步遞推下來(lái)。



        SVO中深度濾波器原理


        基本SLAM14講中已經(jīng)提過(guò)。


        極線(xiàn)搜索與塊匹配。之后,新到的數(shù)據(jù)是,以如下方式融合:



        其中,μ為塊匹配找到像素的深度,σ為擾動(dòng)一個(gè)像素的深度改變,當(dāng)成不確定度。


        當(dāng)不確定度每次改變小于一個(gè)閾值的時(shí)候,就認(rèn)為是收斂了,加入地圖。



        某個(gè)SLAM框架工作原理,優(yōu)缺點(diǎn),改進(jìn)


        1.MonoSLAM:以擴(kuò)展卡爾曼濾波器為后端,追蹤前端的稀疏特征點(diǎn),里程碑式的工作


        2.PTAM:第一個(gè)使用非線(xiàn)性?xún)?yōu)化而不是濾波器作為后端的方案。


        3.ORB-SLAM:現(xiàn)代SLAM系統(tǒng)中做的最完善易用的系統(tǒng)之一,在特征點(diǎn)SLAM中達(dá)到頂峰。


        三個(gè)線(xiàn)程,一個(gè)實(shí)時(shí)跟蹤特征點(diǎn),一個(gè)局部BA,一個(gè)全局Pose Graph優(yōu)化與回環(huán)檢測(cè)。缺點(diǎn)是對(duì)硬件要求比較高。



        4.LSD-SLAM:將直接法應(yīng)用到了半稠密單目SLAM中。(所以擁有直接法的優(yōu)缺點(diǎn),對(duì)特征缺失的地方不敏感,但是對(duì)相機(jī)內(nèi)參和曝光比較敏感)


        5.SVO:沒(méi)有后端優(yōu)化和回環(huán)檢測(cè)部分。優(yōu)勢(shì)是速度極快,針對(duì)的是應(yīng)用平臺(tái)為無(wú)人機(jī)的俯視相機(jī)。


        6.RTAB-MAP:集成度高,方便使用但是不適合研究。



        Ransac框架的實(shí)現(xiàn)




        簡(jiǎn)單實(shí)現(xiàn)cv::Mat()


        這個(gè)題目網(wǎng)上好幾個(gè)記錄帖都說(shuō)會(huì)問(wèn)(雖然可能都是抄來(lái)抄去同一個(gè)),但我實(shí)在不曉得具體需求是什么,要實(shí)現(xiàn)到什么程度。


        暫且跳過(guò),現(xiàn)場(chǎng)發(fā)揮吧?;蛘咭院笱a(bǔ)上。知道的幫忙留言,告訴我這個(gè)題目啥意思,到底是想怎么個(gè)實(shí)現(xiàn)。



        卡爾曼濾波、粒子濾波、貝葉斯濾波


        卡爾曼濾波

        • 粒子濾波


        這塊放兩個(gè)大佬的文章:


        怎樣從實(shí)際場(chǎng)景上理解粒子濾波(Particle Filter)?- 青青子衿的回答 - 知乎


        https://www.zhihu.com/question/25371476/answer/31552082


        • 貝葉斯濾波


        它做的工作就是根據(jù)不斷接收到的新信息和我提供的一些已經(jīng)知道的統(tǒng)計(jì)值,來(lái)不斷更新概率。


        【易懂教程】我是如何十分鐘理解與推導(dǎo)貝葉斯濾波(Bayes Filter)算法?- Ai醬的文章 - 知乎


        https://zhuanlan.zhihu.com/p/75880143



        SIFT、SURF特征點(diǎn)


        SIFT特征檢測(cè)的步驟:


        檢測(cè)尺度空間的極值點(diǎn):搜索所有尺度上的圖像位置,通過(guò)高斯微分函數(shù)來(lái)識(shí)別潛在的對(duì)于尺度和旋轉(zhuǎn)不變的候選關(guān)鍵點(diǎn)。


        精確定位特征點(diǎn)(Keypoint):在每個(gè)候選的位置上,通過(guò)一個(gè)擬合精細(xì)的模型來(lái)確定位置和尺度,同時(shí)關(guān)鍵點(diǎn)的選擇依據(jù)于它們的穩(wěn)定程度。


        設(shè)定特征點(diǎn)的方向參數(shù):基于圖像局部的梯度方向,分配給每個(gè)關(guān)鍵點(diǎn)位置一個(gè)或多個(gè)方向。


        所有后面的對(duì)圖像數(shù)據(jù)的操作都相對(duì)于關(guān)鍵點(diǎn)的方向、尺度和位置進(jìn)行變換,從而提供對(duì)于這些變換的不變性。


        生成特征點(diǎn)的描述子(128維向量):在每個(gè)關(guān)鍵點(diǎn)周?chē)泥徲騼?nèi),在選定的尺度上測(cè)量圖像局部的梯度。梯度被變換成一種表示。


        這塊內(nèi)容屬實(shí)抽象,看了也他娘的記不住。更別說(shuō)叫我去推公式,那是萬(wàn)萬(wàn)不可能的,招聘在即,這種花時(shí)間推了最后也只能記個(gè)大概的我絕對(duì)不干,浪費(fèi)寶貴時(shí)間。這里講的比較好:


        1.高斯金字塔則是由圖像經(jīng)過(guò)逐層下采樣(步長(zhǎng)為2的采樣)而成,而每一層(稱(chēng)之為octave)中則含有多張由不同高斯函數(shù)卷積得到。


        接著同一層(octave)的相鄰兩個(gè)圖像進(jìn)行差分,即可得到高斯差分金字塔。(DOG)


        之后,在高斯差分金字塔中,中間的檢測(cè)點(diǎn)和它同尺度的8個(gè)相鄰點(diǎn)和上下相鄰尺度對(duì)應(yīng)的9×2個(gè)點(diǎn)共26個(gè)點(diǎn)比較,以確保在尺度空間和二維圖像空間都檢測(cè)到極值點(diǎn)。


        一個(gè)點(diǎn)如果在DOG尺度空間本層以及上下兩層的26個(gè)領(lǐng)域中是最大或最小值時(shí),就認(rèn)為該點(diǎn)是圖像在該尺度下的一個(gè)特征點(diǎn)。



        2.對(duì)尺度空間DoG函數(shù)進(jìn)行曲線(xiàn)擬合,計(jì)算其極值點(diǎn),從而實(shí)現(xiàn)關(guān)鍵點(diǎn)的精確定位。



        擬合函數(shù)一般采用二次函數(shù):對(duì)其求導(dǎo)取0,可得偏移量。一般使(x,y,σ)任一維小于0.5即可停止選擇。


        而對(duì)于所確定的點(diǎn)還需要進(jìn)行篩選,如通過(guò)設(shè)定閾值去除對(duì)比度低的點(diǎn),或者h(yuǎn)essian矩陣的跡與行列式的比值判斷關(guān)鍵點(diǎn)是否在邊緣,剔除這些不穩(wěn)定的邊緣響應(yīng)點(diǎn)。


        3.完成關(guān)鍵點(diǎn)的梯度計(jì)算后,使用直方圖統(tǒng)計(jì)鄰域內(nèi)像素的梯度和方向。


        梯度直方圖將0~360度的方向范圍分為36個(gè)柱(bins),其中每柱10度。


        隨著距中心點(diǎn)越遠(yuǎn)的領(lǐng)域其對(duì)直方圖的貢獻(xiàn)也響應(yīng)減小(為簡(jiǎn)化,圖中只畫(huà)了45度一柱,8個(gè)方向的直方圖)。


        而在主方向的計(jì)算中,還存在主方向的校正,通過(guò)一個(gè)旋轉(zhuǎn)矩陣將坐標(biāo)軸旋轉(zhuǎn)為特征點(diǎn)的主方向。


        至此,圖像的關(guān)鍵點(diǎn)已經(jīng)檢測(cè)完畢,每個(gè)關(guān)鍵點(diǎn)有三個(gè)信息:位置、尺度、方向,這些關(guān)鍵點(diǎn)即是該圖像的SIFT特征點(diǎn)。



        4.計(jì)算描述子:


        左圖的中央為當(dāng)前關(guān)鍵點(diǎn)的位置,每個(gè)小格代表為關(guān)鍵點(diǎn)鄰域所在尺度空間的一個(gè)像素,求取每個(gè)像素的梯度幅值與梯度方向,箭頭方向代表該像素的梯度方向,長(zhǎng)度代表梯度幅值。


        然后利用高斯窗口對(duì)其進(jìn)行加權(quán)運(yùn)算。最后在每個(gè)2×2的小塊上繪制8個(gè)方向的梯度直方圖,計(jì)算每個(gè)梯度方向的累加值,即可形成一個(gè)種子點(diǎn),如右圖所示。


        每個(gè)特征點(diǎn)由4個(gè)種子點(diǎn)組成,每個(gè)種子點(diǎn)有8個(gè)方向的向量信息,那么向量的維數(shù)為4×8=32維。將其按照順時(shí)針?lè)较蜻M(jìn)行展開(kāi),即可得到一個(gè)1×32的向量。


        對(duì)每個(gè)關(guān)鍵點(diǎn)使用16×16的窗口,4×4共16個(gè)種子點(diǎn)來(lái)描述,這樣一個(gè)關(guān)鍵點(diǎn)就可以產(chǎn)生128維的SIFT特征向量。



        SIFT特征點(diǎn)(Scale Invariant Feature Transform,尺度不變特征變換):圖像的局部特征,對(duì)旋轉(zhuǎn)、尺度縮放、亮度變化保持不變,對(duì)視角變化、仿射變換、噪聲也保持一定程度的穩(wěn)定性。


        缺點(diǎn)是實(shí)時(shí)性不高,并且對(duì)于邊緣光滑目標(biāo)的特征點(diǎn)提取能力較弱。


        SURF特征點(diǎn)的步驟:


        1. 構(gòu)建Hessian(海塞矩陣),生成所有的興趣點(diǎn),用于特征的提??;

        2. 構(gòu)建尺度空間

        3. 特征點(diǎn)定位

        4. 特征點(diǎn)主方向分配

        5. 生成特征點(diǎn)描述子

        6. 特征點(diǎn)匹配


        跟SIFT的區(qū)別在于:


        1.構(gòu)建Hessian矩陣形成興趣點(diǎn),用盒式濾波器代替高斯濾波器。(上面是高斯濾波器 下面是盒式濾波器



        2.在構(gòu)建尺度空間中,SIFT算法下一組圖像的長(zhǎng)寬均是上一組的一半,同一組不同層圖像之間尺寸一樣,但是所使用的尺度空間因子(高斯模糊系數(shù)σ)逐漸增大;


        而在SURF算法中,不同組間圖像的尺寸都是一致的,不同的是不同組間使用的盒式濾波器的模板尺寸逐漸增大,同一組不同層圖像使用相同尺寸的濾波器,但是濾波器的尺度空間因子逐漸增大。


        3.特征點(diǎn)定位過(guò)程和SIFT一樣,但是特征點(diǎn)主方向計(jì)算方法不一樣:


        SIFT算法特征點(diǎn)的主方向是采用在特征點(diǎn)鄰域內(nèi)統(tǒng)計(jì)其梯度直方圖,橫軸是梯度方向的角度,縱軸是梯度方向?qū)?yīng)梯度幅值的累加,取直方圖bin最大的以及超過(guò)最大80%的那些方向作為特征點(diǎn)的主方向。



        而在SURF算法中,采用的是統(tǒng)計(jì)特征點(diǎn)圓形鄰域內(nèi)的Harr小波特征,即在特征點(diǎn)的圓形鄰域內(nèi),統(tǒng)計(jì)60度扇形內(nèi)所有點(diǎn)的水平、垂直Harr小波特征總和。


        然后扇形以0.2弧度大小的間隔進(jìn)行旋轉(zhuǎn)并再次統(tǒng)計(jì)該區(qū)域內(nèi)Harr小波特征值之后,最后將值最大的那個(gè)扇形的方向作為該特征點(diǎn)的主方向。該過(guò)程示意圖如下:



        4.生成特征描述不一樣:


        在SIFT算法中,為了保證特征矢量的旋轉(zhuǎn)不變性,先以特征點(diǎn)為中心,在附近鄰域內(nèi)將坐標(biāo)軸旋轉(zhuǎn)θ(特征點(diǎn)的主方向)角度。


        然后提取特征點(diǎn)周?chē)?×4個(gè)區(qū)域塊,統(tǒng)計(jì)每小塊內(nèi)8個(gè)梯度方向,這樣一個(gè)關(guān)鍵點(diǎn)就可以產(chǎn)生128維的SIFT特征向量。


        SURF算法中,也是提取特征點(diǎn)周?chē)?×4個(gè)矩形區(qū)域塊,但是所取的矩形區(qū)域方向是沿著特征點(diǎn)的主方向,而不是像SIFT算法一樣,經(jīng)過(guò)旋轉(zhuǎn)θ角度。


        每個(gè)子區(qū)域統(tǒng)計(jì)25個(gè)像素點(diǎn)水平方向和垂直方向的Haar小波特征,這里的水平和垂直方向都是相對(duì)主方向而言的。


        該Harr小波特征為水平方向值之和、垂直方向值之和、水平方向值絕對(duì)值之和以及垂直方向絕對(duì)值之和4個(gè)方向。該過(guò)程示意圖如下:


        把這4個(gè)值作為每個(gè)子塊區(qū)域的特征向量,所以一共有4×4×4=64維向量作為SURF特征的描述子,比SIFT特征的描述子減少了一半。


        4*4,其中每個(gè)塊中25個(gè)點(diǎn),用這25個(gè)點(diǎn)統(tǒng)計(jì)四個(gè)值)


        詳情參見(jiàn)這里。優(yōu)勢(shì)是速度比SIFT提高了,對(duì)于旋轉(zhuǎn)模糊魯棒性也比SIFT提高了。


        對(duì)于速度來(lái)說(shuō),ORB最快,SURF次之,SIFT最慢。


        對(duì)于尺度變換的魯棒性來(lái)說(shuō):SURF>SIFT>ORB(沒(méi)有)對(duì)于旋轉(zhuǎn)模糊魯棒性來(lái)說(shuō):SURF>ORB~SIFT



        如何求解Ax=b?


        回顧矩陣與數(shù)值分析中的內(nèi)容,可以分解A:


        1.Gauss消元法:傳統(tǒng)方式。


        2.LU分解,本質(zhì)上也是Gauss消元,令A(yù)=LU,L對(duì)角線(xiàn)為1,上半部分為0,其余記錄gauss消元的化簡(jiǎn)過(guò)程,U記錄原矩陣的變化。


        3.Crout分解,U對(duì)角線(xiàn)1,U上三角,L下三角。


        4.LDU分解,L單位下三角(對(duì)角為1),U單位上三角,D對(duì)角矩陣。


        5.cholesky分解:A=LL^T,要求A為對(duì)稱(chēng)正定矩陣。(順序主子式都大于0)


         Vector4d x2 = A.llt().solve(B);


        也可以L(fǎng)DL^T分解,L為下三角矩陣,D為對(duì)角元素均為正數(shù)的對(duì)角矩陣。


        Vector4d x3 = A.ldlt().solve(B);


        6.QR分解,A=QR,用正交矩陣代替Gauss消元的下三角矩陣。步驟是求Householder矩陣。


        A分塊為 A=(α1,α2,α3),取w1=α1-||α2||*e1


        Q=H(w1)=



        把Q左乘A,會(huì)得到第一行是111,下面右下角一個(gè)小塊,再對(duì)這個(gè)小塊重復(fù)操作。


        直到把A變成上三角,然后把中途的Q一路左乘,得到一個(gè)QT是正交矩陣。


        eigen中:


         Vector4d x1 = A.colPivHouseholderQr().solve(B);


        7.Schur分解,A=URU^H,U為酉陣,R為上三角矩陣。


        8.Jordan標(biāo)準(zhǔn)型。A=TJT^(-1),用Jordan鏈求,代數(shù)重復(fù)度,幾何重復(fù)度。


        9.奇異值分解,不再提。


        JacobiSVD<MatrixXd> svd(J, ComputeThinU | ComputeThinV);    U = svd.matrixU();    V = svd.matrixV();    A = svd.singularValues();


        如何對(duì)匹配好的點(diǎn)做進(jìn)一步的處理,更好保證匹配效果:


        1.確定匹配最大距離,漢明距離小于最小距離的兩倍


        2.使用KNN-matching算法,令K=2。則每個(gè)match得到兩個(gè)最接近的descriptor,然后計(jì)算最接近距離和次接近距離之間的比值,當(dāng)比值大于既定值時(shí),才作為最終match。


        3.RANSAC(使用RANSAC找到最佳單應(yīng)性矩陣。由于這個(gè)函數(shù)使用的特征點(diǎn)同時(shí)包含正確和錯(cuò)誤匹配點(diǎn),因此計(jì)算的單應(yīng)性矩陣依賴(lài)于二次投影的準(zhǔn)確性)



        描述PNP


        Perspective-n-Points, PnP(P3P)提供了一種解決方案,它是一種由3D-2D的位姿求解方式,即需要已知匹配的3D點(diǎn)和圖像2D點(diǎn)。


        目前遇到的場(chǎng)景主要有兩個(gè):


        其一是求解相機(jī)相對(duì)于某2維圖像/3維物體的位姿;


        其二就是SLAM算法中估計(jì)相機(jī)位姿時(shí)通常需要PnP給出相機(jī)初始位姿。


        在場(chǎng)景1中,我們通常輸入的是物體在世界坐標(biāo)系下的3D點(diǎn)以及這些3D點(diǎn)在圖像上投影的2D點(diǎn),因此求得的是相機(jī)坐標(biāo)系相對(duì)于世界坐標(biāo)系(Twc)的位姿


        在場(chǎng)景2中,通常輸入的是上一幀中的3D點(diǎn)(在上一幀的相機(jī)坐標(biāo)系下表示的點(diǎn))和這些3D點(diǎn)在當(dāng)前幀中的投影得到的2D點(diǎn),所以它求得的是當(dāng)前幀相對(duì)于上一幀的位姿變換



        FLANN算法



        對(duì)這些特征點(diǎn)描述子集中建立索引。


        K-dimensional Tree(Kd樹(shù))是一種常用的建立特征向量索引的方法。建立Kd樹(shù)時(shí),要不斷地選取切分維度(當(dāng)前子集內(nèi)的所有點(diǎn)之間方差最大的維度),然后確定切分位置(按照切分維度排序選擇中位數(shù))。


        這方面已經(jīng)有比較成熟的代碼庫(kù)可以使用,例如FLANN。FLANN既可以單獨(dú)使用,同時(shí)也被集成到了OpenCV、PCL等代碼庫(kù)中。


        根據(jù)特征向量建立好Kd樹(shù)后,查找Kd樹(shù)中的最近鄰居(K Nearest Neighbor)便可以得到目標(biāo)圖中匹配的特征點(diǎn)了。


        EPNP


        求解PnP問(wèn)題目前主要有直接線(xiàn)性變換(DLT),P3P,EPnP,UPnP以及非線(xiàn)性?xún)?yōu)化方法。


        DLT


        直接構(gòu)建一個(gè)12個(gè)未知數(shù)的[R|t]增廣矩陣(先不考慮旋轉(zhuǎn)矩陣的自由度只有3),取六個(gè)點(diǎn)對(duì),去求解12個(gè)未知數(shù)(每一個(gè)3D點(diǎn)到歸一化平面的映射給出兩個(gè)約束),最后將[R|t]左側(cè)3*3矩陣塊進(jìn)行QR分解,用一個(gè)旋轉(zhuǎn)矩陣去近似(將3*3矩陣空間投影到SE(3)流形上)。


        P3P


        P3P方法是通過(guò)3對(duì)3D/2D匹配點(diǎn),求解出四種可能的姿態(tài),在OpenCV calib3d模塊中有實(shí)現(xiàn)。


        EPnP


        需要4對(duì)不共面的(對(duì)于共面的情況只需要3對(duì))3D-2D匹配點(diǎn),是目前最有效的PnP求解方法。


        EPnP的原理有些獨(dú)特,參考這里:


        https://zhuanlan.zhihu.com/p/46695068

        之后把t也寫(xiě)到其中,變成:



        g2o流程


        1.自己定義頂點(diǎn)類(lèi)、邊類(lèi)或者用已經(jīng)有的。


        class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual void setToOriginImpl() // 重置{_estimate << 0,0,0;} virtual void oplusImpl( const double* update ) // 更新{_estimate += Eigen::Vector3d(update);}// 存盤(pán)和讀盤(pán):留空virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}};


        2.定義邊。


        class CurveFittingEdge :public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}// 計(jì)算曲線(xiàn)模型誤差void computeError(){const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);const Eigen::Vector3d abc = v->estimate();_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;}virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}public:double _x; // x 值, y 值為 _measurement};


        之后很重要的一點(diǎn),還有l(wèi)inearizeOplus函數(shù),這個(gè)函數(shù)是求雅克比矩陣,也可以沒(méi)有這個(gè)函數(shù)。


        雅克比矩陣也就是誤差函數(shù)對(duì)頂點(diǎn)的求導(dǎo)值。_jacobianOplusXi(i,j)=;


        3.定義圖模型


        typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每個(gè)誤差項(xiàng)優(yōu)化變量維度為3,誤差值維度為1Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 線(xiàn)性方程求解器Block* solver_ptr = new Block( linearSolver ); // 矩陣塊求解器// 梯度下降方法,從GN, LM, DogLeg 中選g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );g2o::SparseOptimizer optimizer; // 圖模型optimizer.setAlgorithm( solver ); // 設(shè)置求解器optimizer.setVerbose( true ); // 打開(kāi)調(diào)試輸出


        即:

        • typedef一個(gè)矩陣塊求解器 Block類(lèi)型。

        • 定義線(xiàn)性方程求解器。

        • Block 一個(gè)指針,定義矩陣塊求解器的實(shí)例(形參傳入線(xiàn)性方程求解器)。

        • 定義一個(gè)圖模型優(yōu)化器optimizer。

        • 定義梯度下降法solver,將矩陣塊求解器的實(shí)例傳入形參。

        • 圖模型optimizer.setAlgorithm(梯度下降法solver)

        • 打開(kāi)調(diào)試輸出。


        4.圖模型添加頂點(diǎn)。


        CurveFittingVertex* v = new CurveFittingVertex();v->setEstimate( Eigen::Vector3d(0,0,0) );v->setId(0);optimizer.addVertex( v );


        5.添加邊


        for ( int i=0; i<N; i++ ){CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );edge->setId(i);edge->setVertex( 0, v ); // 設(shè)置連接的頂點(diǎn)edge->setMeasurement( y_data[i] ); // 觀(guān)測(cè)數(shù)值edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣:協(xié)方差矩陣之逆optimizer.addEdge( edge );}


        6.圖模型求解


        optimizer.initializeOptimization();optimizer.optimize(100);



        Ceres流程



        1. 代價(jià)函數(shù):


        struct CURVE_FITTING_COST{  CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}  template <typename T>  bool operator()(const T* const abc,T* residual)const  //待估計(jì)參數(shù),誤差項(xiàng){    residual[0]=_y-ceres::exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);    return true;  }  const double _x,_y;};


         2.構(gòu)建優(yōu)化問(wèn)題:


         ceres::Problem problem;  for(int i=0;i<1000;i++)  {//自動(dòng)求導(dǎo)法,輸出維度1,輸入維度3,    problem.AddResidualBlock(      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(        new CURVE_FITTING_COST(x_data[i],y_data[i])      ),      nullptr,  //核函數(shù)      abc  //待估計(jì)參數(shù),這個(gè)在main函數(shù)中要定義,然后這樣傳入進(jìn)來(lái)    );  }


        3.配置求解問(wèn)題這塊就是套路了:


        Solver::Options options;  options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法  options.minimizer_progress_to_stdout = true;//輸出到cout  Solver::Summary summary;//優(yōu)化信息  Solve(options, &problem, &summary);//求解!!!


        創(chuàng)建一個(gè)Option,配置一下求解器的配置,創(chuàng)建一個(gè)Summary。最后調(diào)用Solve方法,求解。 



        LM算法和dogleg算法


        LM:



        求解的△xm為:



        這個(gè)求解結(jié)果可以證明,但是我這里先不放。


        其中,阻尼因子μ的作用是,它大于0保持正定,μ大就接近最速下降法,μ小就接近高斯牛頓法


        μ的初值選取JTJ對(duì)角線(xiàn)元素的最大值乘以一個(gè)τ(一般取10e-8到1):

        根據(jù)上面求解出的△xlm,之后如果迭代△x導(dǎo)致Fx增大,那么就要增大分母里的μ,讓△x減小一些。


        反之讓△x增大一些。(這個(gè)是定性分析)


        真正的定量分析,是通過(guò)比例因子ρ來(lái)確定:



        分子代表實(shí)際下降的值,分母代表雅克比的估計(jì)值。


        分母應(yīng)該始終大于0,如果分子小于0,導(dǎo)致ρ小于零,說(shuō)明該增大μ了。


        如果ρ是大于0的:


        如果比較大,說(shuō)明實(shí)際下降的大于估計(jì)下降的,那么步長(zhǎng)應(yīng)該大一些,所以需要減小μ。


        如果比較小,說(shuō)明實(shí)際下降的小于估計(jì)下降的,那么步長(zhǎng)應(yīng)該放小一些,所以需要增大μ。


        接下來(lái)就涉及到步長(zhǎng)的更新了:


        這是馬爾夸特的更新策略:



        但是g2o,ceres里面用的不是這個(gè)更新策略,而是叫Nielsen策略:(因?yàn)樵瓉?lái)的策略會(huì)導(dǎo)致阻尼因子上下震蕩,浪費(fèi)了一些計(jì)算次數(shù))



        意思是如果ρ大于0,就按這個(gè)策略去更新。如果小于0了,就該增大μ,用一個(gè)v來(lái)控制,讓步長(zhǎng)快點(diǎn)減小到一個(gè)合適的值。


        Dogleg:


        GaussNewton和最快下降法混合方法是Dog-Leg法,代替阻尼項(xiàng),用trust region:




        對(duì)α求導(dǎo)令其為0,得:



        到此為止,如果我們采用高斯牛頓法,那么表示為:

        如果采取最速下降法,那么表示為:

        記住上面的這兩個(gè)符號(hào)。


        狗腿算法的主要內(nèi)容就可以理解為:根據(jù)最速下降法和高斯牛頓法的兩種迭代步長(zhǎng)進(jìn)行均衡,自動(dòng)選擇比較好的步長(zhǎng)!

        ρ如果偏小,說(shuō)明實(shí)際更新的小于預(yù)測(cè)的,說(shuō)明步子邁大了,應(yīng)該減小步長(zhǎng)。



        ORB-SLAM初始化流程





        也就是根據(jù)公式算分值SH和SF,然后根據(jù)下述的公式,決定選哪個(gè):


        當(dāng)RH>0.45的時(shí)候選擇單應(yīng)矩陣。


        注意,那SH和SF怎么算呢?



        這是吳博的PPT內(nèi)容。


        TF和TH那里標(biāo)誤了,應(yīng)該反過(guò)來(lái),TF=5.99,TH=3.84。這個(gè)代表閾值,意思是本身根據(jù)F和H矩陣的性質(zhì),2范數(shù)內(nèi)部應(yīng)該是0的。


        至于這兩個(gè)數(shù)值,是對(duì)于x,一個(gè)誤差的錯(cuò)誤所影響的結(jié)果。 


        最后選好模型后,分解得到對(duì)應(yīng)的位姿。(不過(guò)F是要先變成本質(zhì)矩陣E,再分解的)


        三角化測(cè)量初始的特征點(diǎn)云深度:


        用線(xiàn)性三角形法中的齊次方法。



        執(zhí)行一個(gè)全局BA,以?xún)?yōu)化初始重構(gòu)得到的點(diǎn)云地圖


        ORB-SLAM的其他注意內(nèi)容



        三個(gè)線(xiàn)程,一個(gè)Tracking,一個(gè)Local mapping,一個(gè)loop closing。


        Tracking線(xiàn)程:初始化->相機(jī)位姿跟蹤(可以選擇要不要插入關(guān)鍵幀即局部地圖要不要工作)->局部地圖跟蹤 (EPNP)->判斷是否生成關(guān)鍵幀->生成關(guān)鍵幀



        LocalMapping線(xiàn)程:檢查隊(duì)列 -> 處理新關(guān)鍵幀(更新MapPoints與KeyFrame的關(guān)聯(lián))->剔除MapPoints(剔除地圖中新添加的但質(zhì)量不好的MapPoints。


        比如觀(guān)測(cè)到該點(diǎn)的關(guān)鍵幀太少)->生成MapPoint( 共視程度比較高的關(guān)鍵幀通過(guò)三角化恢復(fù)出一些MapPoints)->Mappoint融合(檢查當(dāng)前關(guān)鍵幀與相鄰幀(兩級(jí)相鄰)重復(fù)的MapPoints)->Local BA(和當(dāng)前關(guān)鍵幀相連的關(guān)鍵幀及MapPoints做局部BA優(yōu)化)


        Loop closing線(xiàn)程:這個(gè)三言?xún)烧Z(yǔ)說(shuō)不清了,放個(gè)圖吧:


        這塊學(xué)習(xí)的時(shí)候可以參考東北大學(xué)吳博和360人工智能研究院的謝曉佳老師一起注釋的ORBSLAM的內(nèi)容:


        https://gitee.com/paopaoslam/ORB-SLAM2

        來(lái)源:古月居


        聲明:部分內(nèi)容來(lái)源于網(wǎng)絡(luò),僅供讀者學(xué)習(xí)、交流之目的。文章版權(quán)歸原作者所有。如有不妥,請(qǐng)聯(lián)系刪除。

        —THE END—


        瀏覽 462
        點(diǎn)贊
        評(píng)論
        收藏
        分享

        手機(jī)掃一掃分享

        分享
        舉報(bào)
        評(píng)論
        圖片
        表情
        推薦
        點(diǎn)贊
        評(píng)論
        收藏
        分享

        手機(jī)掃一掃分享

        分享
        舉報(bào)
        1. <strong id="7actg"></strong>
        2. <table id="7actg"></table>

        3. <address id="7actg"></address>
          <address id="7actg"></address>
          1. <object id="7actg"><tt id="7actg"></tt></object>
            91香蕉视频污下载 | 成人无码AAA | 啊别了快cao我啊h女同 | 激情午夜网 | 啊轻点灬太粗嗯太深小蓝视频 | 久久成人久久爱 | 日韩精品一区二区三区高清免费 | 国产精品久久久久久久久久久痴汉 | 欧美专区在线 | 欧美操逼逼视频 |