1. 程式人生 > >SLAM從入門到放棄:SLAM十四講第十章習題(1-4)

SLAM從入門到放棄:SLAM十四講第十章習題(1-4)

以下均為簡單筆記,如有錯誤,請多多指教。

  1. 證明式 K = P k

    C k T ( C k P
    k
    C k T + Q
    k
    ) 1 K=\overline{P}_kC_k^T(C_k\overline{P}_kC_k^T+Q_k)^{-1}
    答:由於課本上已經推導了大部分的公式,則已知
    P ^ k = ( 1 K C k ) P k \widehat{P}_k=(1-KC_k)\overline{P}_k
    K = P ^ k C k T Q k 1 K=\widehat{P}_kC_k^TQ_k^{-1}
    故而得到 K = ( 1 K C k ) P k C k T Q k 1 = P k C k T Q k 1 K C k P k C k T Q k 1 K=(1-KC_k)\overline{P}_kC_k^TQ_k^{-1}=\overline{P}_kC_k^TQ_k^{-1}-KC_k\overline{P}_kC_k^TQ_k^{-1}
    稍微處理一下得到 K ( 1 + C k P k C k T Q k 1 ) = P k C k T Q k 1 K(1+C_k\overline{P}_kC_k^TQ_k^{-1})=\overline{P}_kC_k^TQ_k^{-1}
    方程兩邊同時乘以 Q k Q_k ,則 K ( Q k + C k P k C k T ) = P k C k T K(Q_k+C_k\overline{P}_kC_k^T)=\overline{P}_kC_k^T
    故而 K = P k C k T ( Q k + C k P k C k T ) 1 K=\overline{P}_kC_k^T(Q_k+C_k\overline{P}_kC_k^T)^{-1}

  2. 對比g2o和Ceres的優化後目標函式的數值。指出為什麼兩者在Meshlab中效果一樣但數值卻不同。
    答:個人感覺這個是自然而然的,即便是同一個程式在不同的執行時間其數值都可能不太一樣。

  3. 對Ceres當中的部分點雲進行Schur消元,看看結果會有什麼區別。
    答:Schur消元后,Ceres執行的時間會得到一定程度的提高。本文使用了http://grail.cs.washington.edu/projects/bal/final.html中961張影像的資料,完全不邊緣化的執行時間為1812s,邊緣化所有點的執行時間為1629s。但是總體感覺似乎時間變化不多,具體原因還不太清楚。
    部分點雲進行消元要修改的程式碼也非常簡單,即在ch10/ceres_custombundle中的SetOrdering稍微修改一下即可,例如只消元 1 / 3 1/3

for(int i=0; i<bal_problem->num_points(); i++)
{
  if(i%3==0)
  {
    ordering->AddElementToGroup(bal_problem->mutable_point_for_observation(i),0);
  }
  else
  {
    ordering->AddElementToGroup(bal_problem->mutable_point_for_observation(i),1);  
  } 
}
  1. 證明 S S 矩陣為半正定矩陣。
    答:假設
    H = [ B E E T C ] H=\begin{bmatrix} B &amp; E \\ E^T &amp; C \end{bmatrix}