半稠密直接法和稀疏直接法求解过程基本上是一样的，只是稀疏直接法提取的是fast关键点，一般是几千个，而半稠密直接法提取的是梯度明显的像素，一般是几万个，这里是提取了一万多个。

所谓的梯度明显的像素，就是像素的灰度值同它周围的像素有明显的区别，这里是拿(v)[u+1]和(v)[u-1],还有(v+1)[u]和(v-1)[u]处的像素的灰度值进行比较的，之前造的边的图像关于像素坐标的导数也是这么求的。

比如设这两个比较值为a,b,如果a*a+b*b>=50，说明u,v处的像素梯度是明显的。

具体程序中

1.主要是在对第一张图片的处理上，for(index==0)

造两个循环，x和y,筛选了一些像素坐标，阈值是10.

定义delta值为灰度的两个比较值的组合

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]

);

如果delta.norm()小于50，就跳出这个循环，这个x,y值就不要了。如果大于等于50，继续循环

再读取x,y对应的深度值，如果深度值为0，这个x,y也不要了。如果不为0，继续。

ushort d=depth.ptr<ushort>(y)[x];

if(d==0)

continue;

这时候满足条件的像素坐标x,y就筛选出来了。求出这些像素坐标的空间点坐标和灰度值，组成测量结构待用。

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));

用g2o求解的相机位姿。