把先前的稀疏特征点改成带有明显梯度的像素,代码实现如下所示,于是在图优化中会增加许多的边。这些边都会参与估计相机位姿的优化问题,利用大量的像素而不单单是稀疏的特征点。由于我们并没有使用所有的像素,所以这种方式又称为半稠密方法(Semi-dense)。
// select the pixels with high gradiants
for ( int x=10; x<gray.cols-10; x++ )
for ( int y=10; y<gray.rows-10; y++ )
{
Eigen::Vector2d delta (
gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1],
gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
);
if ( delta.norm() < 50 )
continue;
ushort d = depth.ptr<ushort> (y)[x];
if ( d==0 )
continue;
Eigen::Vector3d p3d = project2Dt