SLAM从入门到放弃：SLAM十四讲第十章习题（1-4）

以下均为简单笔记，如有错误，请多多指教。

1. 证明式$K={\overline{P}}_k{C}_k^T(C_k{\overline{P}}_k{C}_k^T+Q_k{)}^{-1}$。
   答：由于课本上已经推导了大部分的公式，则已知
   ${\hat{P}}_k=(1-K{C}_k){\overline{P}}_k$
   $K={\hat{P}}_k{C}_k^T{Q}_k^{-1}$
   故而得到$K=(1-K{C}_k){\overline{P}}_k{C}_k^T{Q}_k^{-1}={\overline{P}}_k{C}_k^T{Q}_k^{-1}-K{C}_k{\overline{P}}_k{C}_k^T{Q}_k^{-1}$；
   稍微处理一下得到$K(1+C_k{\overline{P}}_k{C}_k^T{Q}_k^{-1})={\overline{P}}_k{C}_k^T{Q}_k^{-1}$
   方程两边同时乘以$Q_k$，则$K(Q_k+C_k{\overline{P}}_k{C}_k^T)={\overline{P}}_k{C}_k^T$
   故而$K={\overline{P}}_k{C}_k^T(Q_k+C_k{\overline{P}}_k{C}_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$：

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);  
  } 
}

4. 证明$S$矩阵为半正定矩阵。
   答：待解决