前面用了好几篇文章介绍特征点法的相机位姿估计，本文则换一种思路，介绍近年来日渐流行的直接法。
一、直接法
与“光流法”类似，直接法也不需要特征匹配，甚至不需要提取特征。显然，这会节约大量时间，但与此同时，由于缺少准确的匹配，无法得知两张图像间像素的对应关系，所以必须依靠大量像素点的整体优化才有可能得到正确的结果。
这里提到了“大量像素点”，不是特征点也不是全部像素点。这是因为直接法可以根据使用像素点的多少分为三类：稀疏直接法，半稠密直接法，稠密直接法。稀疏直接法仍然使用特征点，不过不必计算描述子，可以节约一些时间。半稠密直接法使用像素梯度大的像素点，舍弃那些像素梯度非常小的点。稠密直接法使用所有像素点（一般几十万至几百万个），计算量非常大，无法在CPU上实时计算，但好处是可以建立稠密地图。
在特征点法中，我们优化相机位姿使重投影误差最小。而在直接法中，由于没有匹配点对，无法计算重投影误差，取而代之的是光度误差。光度误差是两张图片中由变换矩阵关联起来的两个像素点的灰度差异。理想情况下，灰度差异应该为0，它们应该是同一个点（仍然基于灰度不变假设）。但实际中由于相机位姿变换矩阵不准确会造成一些差异，我们据此构建一个非线性优化问题，把大量像素点的光度误差的平方和作为总误差，优化相机位姿使该误差最小。
构建这样一个非线性优化问题需要计算误差对相机位姿的导数。这里的理论性又很强了，笔者也是搞不定，建议大家看高翔博士《视觉SLAM十四讲》的第8讲原文。接下来我们还是看看怎么使用g2o实现直接法吧。
二、使用g2o实现直接法
由于稠密直接法需要借助GPU编程，所以这里不予考虑。只介绍稀疏直接法和半稠密直接法的实现。
仍然可以借助g2o提供的VertexSE3Expmap类作为顶点，但需要自己编写代表误差项的边，代码如下。
// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3ProjectDirect() {}
EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
{}
virtual void computeError()
{
const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
float x = x_local[0]*fx_/x_local[2] + cx_;
float y = x_local[1]*fy_/x_local[2] + cy_;
// check x,y is in the image
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
{
_error ( 0,0 ) = 0.0;
this->setLevel ( 1 );
}
else
{
_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
}
}
// plus in manifold
virtual void linearizeOplus( )
{
if ( level() == 1 )
{
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
return;
}
VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book
double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;
float u = x*fx_*invz + cx_;
float v = y*fy_*invz + cy_;
// jacobian from se3 to u,v
// NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
jacobian_uv_ksai ( 0,4 ) = 0;
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
jacobian_uv_ksai ( 1,3 ) = 0;
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
}
// dummy read and write functions because we don't care...
virtual bool read ( std::istream& in ) {}
virtual bool write ( std::ostream& out ) const {}
protected:
// get a gray scale value from reference image (bilinear interpolated)
inline float getPixelValue ( float x, float y )
{
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
float xx = x - floor ( x );
float yy = y - floor ( y );
return float (
( 1-xx ) * ( 1-yy ) * data[0] +
xx* ( 1-yy ) * data[1] +
( 1-xx ) *yy*data[ image_->step ] +
xx*yy*data[image_->step+1]
);
}
public:
Eigen::Vector3d x_world_; // 3D point in world frame
float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
cv::Mat* image_=nullptr; // reference image
};
这是一个一元边，因为我们每次只对一个相机位姿做优化。覆写了两个函数，在computeError中计算光度误差，并在linearizeOplus中计算误差关于相机位姿的导数，即雅克比矩阵。另外，getPixelValue中对图像亮度做了插值，以得到更准确的结果。
在主函数中，初始化g2o求解器，添加顶点和边，开始优化等等。稀疏直接法和半稠密直接法的区别是，前者事先提取FAST关键点，而后者遍历所有像素，找出其中像素梯度大的点。再往后的内容就趋于一致了。下面给出程序运行效果。
可以看到，半稠密直接法使用了更多的像素点，准确度更高，但是速度比稀疏直接法慢了许多，大概需要100多毫秒，而稀疏直接法只需要十几毫秒。
完整代码下载地址：https://github.com/jingedawang/DirectMethod
三、直接法的优缺点
总结起来，直接法的优点有：
- 省去计算特征点和描述子的时间。
- 不需要特征点，只需要有像素梯度。因此可以用于特征缺失的场合，比如渐变的图案。
- 可以构建半稠密乃至稠密地图。
同时也有如下的缺点：
- 非凸性。优化很容易进入局部极小。
- 单个像素没有区分度。应当使用图像块来代替单个像素的计算。
- 灰度值不变假设难以满足。相机的曝光参数会自动调整，导致图像整体变亮或变暗。
四、参考资料
《视觉SLAM十四讲》第8讲 视觉里程计2 高翔
