半稠密直接法

跟稀疏的相比就是在循环中的第一帧取一些点的时候，又稀疏的特征点取成了有明显梯度的点，就是增加了一些点。同样，点的增加就会往g2o中增加一些边，在后面绘制对比图时，也没有了线，而只有点，因为太多了，画线看不清了。

        if ( index ==0 )        {            // select the pixels with high gradiants            //双层循环遍历像素点,边缘的就不要了            for ( int x=10; x<gray.cols-10; x++ )                for ( int y=10; y<gray.rows-10; y++ )                {                    //这里就是梯度向量delta，可以看一下，                    //它是以(x,y)像素右减左像素灰度差为x方向梯度值，                    //　　以(x,y)像素下减上像素灰度差为y方向梯度值。                    //发现(x,y)处的像素梯度跟(x,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]                    );                    //如果模长小于50,即任务就是梯度不明显，continue掉，其他的就开始对应深度和空间点，往measurements中push了                    //说白了跟稀疏的比就是在第一帧中多取了一些点而已。稠密的就是不用说了，所有点全push进measurements                    if ( delta.norm() < 50 )                        continue;                    ushort d = depth.ptr<ushort> (y)[x];                    if ( d==0 )                        continue;                    Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );                    float grayscale = float ( gray.ptr<uchar> (y) [x] );                    measurements.push_back ( Measurement ( p3d, grayscale ) );                }            prev_color = color.clone();            cout<<"add total "<<measurements.size()<<" measurements."<<endl;            continue;        }