由之前的PnP,可以求出一個R,t,K又是已知的。而且空間點的世界坐標知道，第二個相機位姿的像素坐標也是知道的。就可以利用它們進行優化。
首先確定變量為const vector<Point3f> points_3d,const vector<Point2f>,const Mat& K,Mat& R,Mat& t.
因為之后放進去的pts_3d,pts_2d是我們自己計算出來的，所以不用用&。
開始函數：
1.初始化g2o.這個可以是固定的方式。基本上都是相似的。
定義矩陣塊的類型為Block.這樣方便之后的寫入。它有兩個參數，第一個參數為優化變量的維度，這里為6，第二個參數為誤差值的維度，這里為3。
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;
定義線性求解器的類型linearsolver要帶指針，有稠密Dense和CSparse兩種類型。前面要加new.參數為矩陣塊里的位姿矩陣類型
LinearSolverType* linearsolver=new g2o::LinearSolver::CSparse<Block::PoseMatrixType>();
定義矩陣塊的求解器solver_ptr。要帶指針。也要帶new.g2o賦初值的都要帶new.
Block* solver_ptr=new Block(linearsolver);
定義梯度下降方法為levenberg方法solver.
g2o::OptimizationAlgorithmLevenberg* solver=new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
步驟就是，先把復雜的矩陣塊類型在這里定義為Block.然后定義線性方程求解器linearsolver,再定義矩陣塊求解器solver_ptr和linearsolver有關，再定義梯度下降方法solver和solver_ptr有關。
定義圖模型optimizer.類型為g2o::SparseOptimizer.
然后設置圖模型的求解器，也就是之前定義的梯度下降方法。
optimizer.setAlgorithm(solver);
設置要不要打開調試輸出，設置為true.這個選項有的時候可以不設
optimizer.setVerbose(true)
2.設置頂點有關的東西，這里有兩個頂點，一個是李代數位姿pose，一個是空間點位置p。
李代數位姿頂點類型為g2o::VertexSE3Expmap*,初值為new g2o::VertexSE3Expmap();
頂點要設置兩個東西，一個是Id,一般設為0，一個是Estimate,這里是由R,t組成的SE3Quat形式。只要把R,t放進去就可以了。但是R必須是矩陣形式。由之前的R.at<double>(0,0)等轉化。t必須是向量，由t.at<double>(0,0)等轉化。
至於空間點式的頂點，就需要一個for循環了，因為空間點有很多啊。、
for(const Point3f p:points_3d)
頂點設為point,類型為g2o::VertexSBAPointXYZ,初值為new g2o::VertexSBAPointXYZ();
Id設為index++,估計值設為p.x,p.y,p.z的組成的向量形式。還設置了一個setMarginalized(true).
3相機參數
里面還用到了相機參數camera.類型為g2o::CameraParmetersI,值為K.at<double>(0,0),和cx,cy組成的2維向量，0組成的。
Id設置為0,
4.邊。
重點是邊了。邊肯定在一個for循環里，這里是for(const Point2f p:points_2d)
邊的類型為g2o::EdgeProjectXYZ2UV,初值為new g2o::EdgeProjectXYZ2UV().
設置id為index,
設置頂點0為空間點位置，而且里面用了一個dyanmic_cast把圖模型的頂點坐標設置為空間點頂點類型。
edge->setVertex(0,dynamic_cat<g2o::VertexSBAPointXYZ*>(optimizer.vertex(index)));
設置頂點1為位姿。
edge->setVertex(1,pose);
設置測量值為p.x,p.y組成的2維向量模式。
設置參數ID為(0,0).
edge->setMeasurement(Eigen::Vector2d(p.x,p.y));
edge->setParameterId(0,0);
設置信息矩陣為2維的單位矩陣。信息矩陣的維度是根據誤差值的維度定的。
5.求解
求解就特別簡單，先把圖模型給初始化一下。
optimizer.IntializeOptimization();
直接用優化函數optimize()進行優化。
optimizer.optimize(100)
由優化可以得到位姿的估計值。pose->estimate.要想把它轉成常見形式，可以用歐式變換矩陣Eigen::Isometry3d，是4*4矩陣，
T=Eigen::Isometry3d(pose->estimate()).matrix