半稠密直接法

跟稀疏的相比就是在循环中的第一帧取一些点的时候，又稀疏的特征点取成了有明显梯度的点，就是增加了一些点。同样，点的增加就会往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;
        }