半稠密直接法求解相機位姿思路

半稠密直接法和稀疏直接法求解過程基本上是同樣的，只是稀疏直接法提取的是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求解的相機位姿。