Optimizer::PoseOptimization

用於Tracking中勻速運動模型跟蹤等

EdgeSE3ProjectXYZOnlyPose

class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

測量值是2維的Vector2d資料，即畫素座標，這條邊連線著pose節點，是一元Unary邊，只優化SE3的頂點變數，地圖點XYZ固定，世界座標系下的地圖點是成員變數Xw，在Optimizer::PoseOptimization(Frame *pFrame)中遍歷當前幀可視的地圖點呼叫，一個頂點N條邊。 （一元邊裡面只有_jacobianOplusXi

觀測二維，所以殘差也是二維，即當前幀的畫素觀測和地圖點在當前幀的投影位置的殘差，從而優化位姿。殘差對位姿的求導所以雅克比維度是2X6。

void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
  VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
  Vector3d xyz_trans = vi->estimate().map(Xw);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double invz = 1.0/xyz_trans[2];
  double invz_2 = invz*invz;

  _jacobianOplusXi(0,0) =  x*y*invz_2 *fx;
  _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
  _jacobianOplusXi(0,2) = y*invz *fx;
  _jacobianOplusXi(0,3) = -invz *fx;
  _jacobianOplusXi(0,4) = 0;
  _jacobianOplusXi(0,5) = x*invz_2 *fx;

  _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
  _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
  _jacobianOplusXi(1,2) = -x*invz *fy;
  _jacobianOplusXi(1,3) = 0;
  _jacobianOplusXi(1,4) = -invz *fy;
  _jacobianOplusXi(1,5) = y*invz_2 *fy;
}
 

加完邊後進行4次迭代，每次迭代優化10次，每次迭代完會有一次野值的篩選（根據卡方分佈），TODO：野值邊setLevel(1)，內點setLevle(0), 1代表不優化。

Optimizer::BundleAdjustment

3D-2D BA，在GlobalBundleAdjustemnt中呼叫，計算量比較大

EdgeSE3ProjectXYZ

EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() {
}

這條邊連著3D地圖點和位姿，測量和誤差函式和EdgeSE3ProjectXYZOnlyPose一樣，分別是畫素點和 $e$

void EdgeSE3ProjectXYZ::linearizeOplus() {
  ……

  // 2*3
  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

  // 2*6 the same as EdgeSE3ProjectXYZOnlyPose::linearizeOplus()
  _jacobianOplusXj(0,0) =  x*y/z_2 *fx;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
  _jacobianOplusXj(0,2) = y/z *fx;
  _jacobianOplusXj(0,3) = -1./z *fx;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *fx;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
  _jacobianOplusXj(1,1) = -x*y/z_2 *fy;
  _jacobianOplusXj(1,2) = -x/z *fy;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *fy;
  _jacobianOplusXj(1,5) = y/z_2 *fy;
}

然後迭代一次，優化10次。然後setpose和setWorldPos成優化後的結果

Optimizer::LocalBundleAdjustment

得到當前幀連線的KF和這些KF中的地圖點後（lLocalKeyFrames和lLocalMapPoints），和能觀測到這些地圖點但沒有和當前幀相連的KF（lFixedCameras）。 在加Pose頂點時（其中lFixedCameras的vSE3->setFixed(true)，而lLocalKeyFrames只用第一幀fix就可以）。

邊也是EdgeSE3ProjectXYZ。和BA一樣，只不過這裡的資料來源是Local的，比較快一些。

開始優化，迭代一次優化5次。然後根據卡方分佈篩選閾值（這個閾值見多檢視幾何），去除野值後再優化。然後去除誤差比較大的KF和地圖點的互相觀測。最後更新優化後的位姿和地圖點。

Optimizer::OptimizeEssentialGraph

閉環檢測後的優化 首先拿到地圖中所有KF和地圖點，宣告vScw和vCorrectedSwc，分別代表未sim3優化前的sim3位姿和優化後的。

    for(vKFs)
    {
        g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
        如果是sim3調整過的KF，VSim3->setEstimate(it->second);用調整過的位姿
        否則用當前pose。VSim3->setEstimate(Siw);
        如果是當前幀對應的閉環幀，fixed
        optimizer.addVertex(VSim3);
        
        並給vScw賦值供後面使用
    }
    
    // Set Loop edges
    // Set normal edges
    這邊加的各種邊型別都是EdgeSim3，區別只是不同的來源

EdgeSim3

  class G2O_CORE_API EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeSim3();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
    void computeError()
    {
      const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]);
      const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]);

      Sim3 C(_measurement);
      Sim3 error_=C*v1->estimate()*v2->estimate().inverse();
      _error = error_.log();
    }

    virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}
    virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
    {
      VertexSim3Expmap* v1 = static_cast<VertexSim3Expmap*>(_vertices[0]);
      VertexSim3Expmap* v2 = static_cast<VertexSim3Expmap*>(_vertices[1]);
      if (from.count(v1) > 0)
  v2->setEstimate(measurement()*v1->estimate());
      else
  v1->setEstimate(measurement().inverse()*v2->estimate());
    }
  };

可以看到是連線兩個Sim3節點的二元邊，頂點0的位姿是Tiw,1的位姿是Tjw，那麼邊的測量是Tji，即from i to j。 $e = log( Measurement * Tiw * Tjw^{-1} )$ 沒有過載::linearizeOplus()方法，似乎是用g2o的自動求導，即差分。會不會慢一些？ 優化後， 1.SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]，重新setpose，即取R，t/S。 2.Correct points. Transform to “non-optimized” reference keyframe pose and transform back with optimized pose。把世界系下的地圖點投到未校正的相機系下再用矯正後的pose投到世界系，得到心得世界系座標。

Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));

Optimizer::OptimizeSim3

是在篩選閉環候選幀的時候用的。

int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)

輸入時當前幀、閉環候選幀和候選幀對應的地圖點、優化前的sim3（當前幀和閉環候選幀之間Scm，m-候選幀）。輸出時優化後的sim3位姿。

EdgeSim3ProjectXYZ

和前面提到的EdgeSE3ProjectXYZ有些不同，從SE3變成了SIM3，7自由度。ORB也用了g2o自動求導，把線性化函式註釋掉了。觀測一樣，是畫素座標。如果自己寫的話，應該是一個2X3的對點的雅可比，一個2X7的對sim3的雅可比。

ORB LoopClosing

        if(CheckNewKeyFrames())
        {
            // Detect loop candidates and check covisibility consistency
            if(DetectLoop())
            {
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())    //用到Optimizer::OptimizeSim3篩選候選幀
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();   //如果找到了，用到Optimizer::OptimizeEssentialGraph 優化。
               }
            }
        }

Sim3

$T = \begin{bmatrix} sR & t \\ 0 & 1 \end{bmatrix}$ $T^{-1} = \begin{bmatrix} R/s & -R^{T}t/s \\ 0 & 1 \end{bmatrix}$ 在ORB實現的sim3節點中，逆的表示為 $q' = q*; t'=q*(-t/s);s'=1/s$