BA优化PnP的思路

由之前的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