g2o代码阅读 高翔Slambook第七讲：3d2d非线性优化
g2o代码阅读 高翔Slambook第七讲：3d2d非线性优化
首先熟悉一下这里g2o是要做一个什么样的非线性优化的工作，可以由bundleAdjustment这个函数的形参定义来回忆一下：
void bundleAdjustment (
const vector< Point3f > points_3d,
const vector< Point2f > points_2d,
const Mat& K,
Mat& R, Mat& t )可以看出传入了五个参数，其中三个不可修改，分别是points_3d：存储着3d点坐标的point3f类容器； points_2d：存储着2d点坐标的point2f类容器；K：相机内参矩阵。以及两个直接引用可被修改的量R和t，分别存储之前通过PnP直接求取得到的位姿变换R、t，在此作为非线性优化的迭代初值。可以对着书上的图看一下，更为直观：
在本程序中所用到的节点、边在主函数中没有被定义，使用的是g2o内置的节点和边的类型。在梳理主函数中的g2o相关语句之前，先搞清楚一会要用到的节点和边是什么：
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSE3Expmap();
bool read(std::istream& is);
bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate = SE3Quat();
}
virtual void oplusImpl(const number_t* update_) {
Eigen::Map<const Vector6> update(update_);
setEstimate(SE3Quat::exp(update)*estimate());
}
};这里定义了一个用来表示位姿的节点VertexSE3Expmap，也是g2o求解相机位姿一定会用到的节点。<6, SE3Quat>表示这个节点内部包含了6个待优化变量（平移t、旋转R各3个），SE3Quat是这6个变量的存储类型。可以看出类型是g2o定义的一个特殊欧式群SE3类的变量，至于其是前三平移后三旋转还是前三旋转后三平移可以去深入了解下SE3Quat的存储形式，这里咱们用不到，如果需要构造一个SE3Quat类的变量只需使用g2o提供的构造函数即可。
class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSBAPointXYZ();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate.fill(0);
}
virtual void oplusImpl(const number_t* update)
{
Eigen::Map<const Vector3> v(update);
_estimate += v;
}
};这里定义了一个用来存储特征点所对应的物体的3d坐标的节点VertexSBAPointXYZ，这里3d坐标应相对于参考帧（这里是前一帧）的相机坐标系。<3, Vector3>代表内部存储的待优化变量个数为3，存储格式为g2o::Vector3,不同于Eigen所定义的Vector3d或Vector3f。这里将物体的3d坐标也定义为节点，目的在于通过非线性优化的方式，在求取相机位姿R、t的同时，将物体的3d坐标也当做优化变量进行优化。
在主函数中也使用了一个代表相机内参的节点，这个节点并不与所使用的边进行连接，因此只起到在求解器内部提供边进行误差运算时所需的参数的作用，其内部的参数并不会像位姿节点或者物体位置节点一样被优化或修改。这里展示下这个相机节点内部最重要的一小部分：
CameraParameters(number_t focal_length,
const Vector2 & principle_point,
number_t baseline)
: focal_length(focal_length),
principle_point(principle_point),
baseline(baseline){}也就是在这个节点的带参构造函数，在实例化一个CameraParameters类的对象的同时，将输入的三个参数传入这个对象内部的变量focal_length焦距、principle_point焦点、baseline基线位置。
最后是使用到的边的定义：
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjectXYZ2UV();
bool read(std::istream& is);
bool write(std::ostream& os) const;
void computeError() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
const CameraParameters * cam
= static_cast<const CameraParameters *>(parameter(0));
Vector2 obs(_measurement);
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
}
virtual void linearizeOplus();
CameraParameters * _cam;
};这是一个二元边，边内部所计算的误差的维度为2，误差的存储形式为g2o::Vector2，所连接的0号节点为一个物体的3d位置节点，1号节点为相机位姿节点。同样，误差计算还是为了完成以下的工作：
这里具体的Y-real由setMeasurement()传入的真实特征点2d像素坐标提供，Y-estimated则由位置节点提供的3d坐标、位姿节点提供的估计R、t和内参节点提供的相机内参，使用内部定义好的函数map()来计算得到。
下面来看主程序：
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
// pose 维度为 6, landmark 维度为 3
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
// 线性方程求解器
// Block* solver_ptr = new Block ( linearSolver ); // 矩阵块求解器
// g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );这里g2o::BlockSolverTraits<6,3>中，6代表位姿节点中待优化变量个数为6。而3呢？再上一篇第6讲g2o曲线拟合中，所设置的求解器的参数为<3,1>，3代表有三个参数待求，1代表误差的维度为1；那这里误差是个Vector2类型的量，维度是2但为何求解器的维度却是<6,3>呢？小绿认为这里的3其实是另一类节点中待优化参数的维度，也就是物体3d位置节点的3个待优化参数，因为这里g2o所使用的边是一个二元边，所以<m,n>的第二个变量的意义变为第二类节点的待优化参数个数。同样，在下一篇即将提到的3d3d非线性优化问题中使用的g2o求解器依然为<6,3>，这里由于使用的是一元边，边内部所定义的误差维度为3，因此这里的3依然指误差的维度（以上说法纯属个人见解）。类似的问题在csdn上也有人提过（https://ask.csdn.net/questions/697766），但可能问题太捞大佬们不屑解答也就放在那里了。
后面的语句依然都是套路，选择稀疏线性求解器、迭代算法选择LM。
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat (
R_mat,
Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
) );
optimizer.addVertex ( pose );这里定义了一个位姿节点pose，并将前文PnP计算得到的R、t通过形式转换并进行构造，得到了一个SE3Quat类变量，通过setEstimate()输入进节点作为初始估计值，即迭代优化过程的初始值。最后将这个节点pose添加到求解器中去。
int index = 1;
for ( const Point3f p:points_3d ) // landmarks
{
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
optimizer.addVertex ( point );
}这里开始物体3d位置节点point的构造。节点个数取决于具备深度信息的特征点的个数，即存放特征点像素坐标与深度信息的容器points_3d的大小。在每个节点构造过程中，将其从1开始编号（求解器中的0号节点刚才已经给位姿节点了），并分别将特征点像素坐标与深度信息转换成一个Vector3d类的变量，通过setEstimate()输入进节点作为初始估计值。可以看出，Eigen::Vector3d与g2o::Vector3虽不是一类变量，但是可以直接进行运算。进而设置边缘化为true以便稀疏化求解，最后将这个节点point添加到求解器中去。
g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
camera->setId ( 0 );
optimizer.addParameter ( camera );这里通过在内参矩阵K内寻找相应的值，将相机的焦距、焦点坐标输入到相机参数节点中去，基线位置设置为0。
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
// edge->setVertex ( 0, optimizer.vertex(index) );
edge->setVertex ( 1, pose );
//我就想让他达到我的p.x\\p.y
edge->setMeasurement ( Eigen::Vector2f ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}这段代码实现了边edge的构造。同样，边的个数与物体3d位置节点的个数相同，也取决于points_2d的大小。对应每个edge，同样给其赋予不同的id（这里同样是从index=1开始赋id的，是为了方便，从0开始亦可），同时按照边的构造顺序将这条边对应的0号节点设置为id为index的物体3d位置节点（方便在这了），对应的1号节点设置为相机位姿节点pose（这里直接使用pose是因为这个节点有他自己的名字，也可以使用optimizer.vertex(0)）。最后，将真实的特征点像素坐标通过setMeasurement()传入边作为Y-real，设置参数id为（0,0）（网上有关于这两个0的意义：“第二个参数是优化器内添加的参数的id。当你调用addEdge来添加这条边时，会根据第二个参数的id，把相应的参数地址给边，以后边内的成员函数，就根据第一个参数，拿到这个地址。”，这里的参数指相机内参节点提供的参数），并设置信息矩阵为二维单位阵，最终将这个边edge添加到求解器。
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
optimizer.optimize ( 100 );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;
cout<<endl<<"after optimization:"<<endl;
cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;最后同样设置迭代次数为100次，开始迭代求解，并将优化结果输出至屏幕。
至此，g2o的求解器设置、节点定义、边定义已结束，可以用以下图模型展示其原理：
当然，也可以取消优化物体3d位置的过程，将其设置为固定3d坐标作为已知信息，在构造边时将其通过列表初始化的方式输入到边内部，并赋值给内部变量参与误差的计算。此时，边只连接一个位姿节点，成为一元边，那么此时用到的边需要自己重新定义：
class EdgeProjectXYZ2UVPoseOnly : public g2o::BaseUnaryEdge<2, Eigen::Vector2f, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjectXYZ2UVPoseOnly(const Eigen::Vector2f& point):_point(point) {}
void computeError() {
const VertexSE3Expmap* v = static_cast<const VertexSE3Expmap*>(_vertices[0]);
const CameraParameters * cam
= static_cast<const CameraParameters *>(parameter(0));
Eigen::Vector2f obs(_measurement);
_error = obs-cam->cam_map(v->estimate().map(_point));
}
virtual void linearizeOplus();
CameraParameters * _cam;
bool read ( istream& in ) {}
bool write ( ostream& out ) const {}
protected:
Eigen::Vector2f _point;
};此时，可以用以下图模型展示其原理：
