主要用的g2o的方法。自己定义了一个新的一元边。边的误差项是测量值和由估计得来的x,y对应的灰度值之间的误差。导数为灰度对像素坐标的导数乘以像素坐标对yi*李代数的导数的负数。灰度对于像素坐标的导数矩阵为1*2的矩阵。比如像素坐标为u,v.第一个数就是灰度关于u的导数，为u+1,v对应的灰度值和u-1,v对应的灰度值之差再除以2.v同理。
而像素坐标对yi*李代数的导数为2*6的雅克比矩阵。这个导数可以转化为两个导数的乘积，一个是像素坐标对空间点的导数，一个是空间点对yi*李代数的导数。
像素坐标和空间点有这样的关系
u=x*fx/z+cx
v=y*fy/z+cy.
u对x,y,z进行求导分别为fx/z,0,-x*fx/(z*z)
v对x,y,z进行求导分别为0,fy/z,-y*fy/(z*z).
而空间点x,y,z对yi*李代数的导数为[I,-空间点的反对称矩阵】
这样两者相乘就可以了。
因为求解误差项的时候和求解灰度对于像素坐标的导数的时候都要用到估计值的灰度值，所以要有一个灰度值函数，这个灰度值函数是对图像进行双线性插值得到的。所谓的双线性插值就是
a+bx+cy+dxy.
这里的a可以为（1-xx)*(1-yy),b=xx*(1-yy),c=yy(1-xx),d=xx*yy.其中xx=x-deltax,y=y=deltay.
g2o的顶点就是估计值，可以用_vertices[0]赋值，赋值之后就用这个值计算估计值，有估计值就可以计算误差等。进而迭代找到最优的估计值。
现在来看整个程序是怎么实现的。
1.读图和设置一些需要的变量。
定义图像数据路径，定义associate.txt文件路径，定义图像矩阵color,depth,gray,之后fin可以把aaociate里的每一行来读取相对应的彩色图和深度图。
定义fx,fy,cx,cy,depth_scale.fx,fy,cx,cy都是相机内参矩阵的组成部分，depth_scale为深度图缩放因子。有前四项可以得到K,depth_scale之后待用。
定义Tcw类型和初值，Eigen::Isometry3d::Identity().
定义prev_color,之后就把第一张图片设置成参考图。
2.做一个for循环，在这个循环里，不断地读取associate.txt里对应的数据集下的彩色图和深度图，然后赋值给color和depth.但只有第一张图片为prev_color,也只对第一张图片提取关键点。提取关键点之后，对关键点进行筛选，阈值是20.筛选之后，得到这些关键点的空间点坐标和灰度值，组成一个结构体，放到测量值里。
其中得到空间点要经过
z=d/depth_scale,
x=(u-cx)*z/fx
y=(u-cy)*z/fy
其中d是深度值，需要在深度图上读取，记住坐标是先y后x.而且是(y)[int x]
得到灰度值，首先需要得到灰度图，灰度图是由彩色图得到的。用cv::cvtColor函数。
cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
然后从灰度图里读取像素坐标对应的灰度值，也是先v后u的，而且是(v)[u],其实应该都是u,v，但是这里都表示成了x,y.
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
3.构造稀疏直接法位姿估计函数poseEstimationDirect函数
这里的变量只需要测量值measurements，下一张图片的灰度图&gray，相机内参矩阵K，和要求的Tcw就可以了。
这个函数没有什么特别的，无非还是先设置估计的值的类型，求解器，求解的方法，这里是列文伯格。图模型optimizer,图模型求解方法列文伯格，图模型设置调试输出.setVerbose(true)。
主要来看我们自己造的边。
这个边类里面有几个变量，分别是空间点坐标，fx,fy,cx,cy,*image.空间点坐标之后放的是测量值的空间点坐标，也就是第一张图像的关键点对应的空间点坐标。*image放的是下一张图像的灰度图。
计算误差的时候会把估计值复制给v.然后v是位姿，把空间点映射成第二张图片的空间点，然后经过内参矩阵，映射之后的空间点转换为第二张图片上的像素坐标，对这些像素坐标进行筛选，阈值为4.如果不在范围内，误差值设为0，level设为1.如果在范围内，误差为这些误差值对应的灰度值和测量值的灰度值相减。基于灰度不变假设。
而在边类的计算雅克比矩阵的函数linearizeOplus函数里，如果level=1,就是之前计算得到的像素坐标不在范围内，最终的雅各比矩阵设为0.最终雅克比矩阵形式为1*6的，为
_jacobianOplusXi=Eigen::Matrix<double,1,6>::Zero().
同样，这里吧位姿估计值赋值给vtx,然后计算得到在第二张图片上的关键点的空间点坐标xyz_trans.然后计算得到最终的雅克比矩阵。
然后在protected里定义灰度值函数。
这里灰度值数据访问形式为
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
deltax为floor(x),deltay=floor(y).1,x,y,xy分别为data[0],data[1],data[image_->step],data[image_->step+1]
这个灰度值函数定义为内联函数，方便计算。
最终在public里要定义类变量的类型和初值。这里就是空间点坐标，内参，灰度图了。
然后看函数执行过程
先设置位姿的类型和初值，然后设置位姿的估计值形式为se3quat形式，是由Tcw的旋转矩阵和位移矩阵构成的。setId(0),然后把这个顶点添加到图模型里。
然后构造边，边是一个个进行构造的，每个测量值都可以构造一个边。所以构造一个循环
for (Measurement m:measurements)
{
设置边的类型，边的初值是由m.pos_wolrd,内参，新的灰度图构造的。
边连接的顶点就是pose,测量值就是m.grayscale,信息矩阵就是1*1的单位矩阵，id就是id++.然后图模型把这条边给加上。
记着顶点和边都是指针，定义的时候要带*号，赋初值的时候要带new.
4.画图，这里画图是把第一种图片和之后的其他图片做对比，而且把图片中的每个关键点都用圆圈标注，两张图片对应的关键点连线。
要同时展示两张图片，img_show的行要是图片行的两倍，列还是图片的列。
cv::Mat img_show(color.rows*2,color.cols,CV_8UC3);
把第一种图片prev_color复制到img_show的，从0,0到color.cols,color.rows.
而color则复制到img_show,从0.color.rows开始，行列为color.cols,color.rows.
对于测量值的每一项，既是第一张图片的关键点的空间点坐标和灰度值组成的。
可以知道在第一张图片关键点的空间点坐标，由此得第一张图片关键点的像素坐标pixel_prev，由Tcw可以得关键点在第二张图片的空间点坐标，由此的特征点在第二张图片的像素坐标pixel_now。
筛选如果像素坐标小于0或者像素坐标超出图片行或列，跳出循环。
计算b,g,r值，因为用圆圈标注的时候需要用到b,g,r的值。
值都为255*float ( rand() ) /RAND_MAX;
标注函数是cv::circle,第一种图像标注的变量为img_show,像素坐标，8，cv::Scalar(b,g,r),2
第二张图片标注用img_show,关键点在第二张图片上的像素坐标u,v但是v要加上函数color.rows,8,cv::Scalar(b,g,r),2.
连线，用cv::line函数，变量为img_show,第一张图片像素坐标，第二张像素坐标(v+color.rows)，cv::Scalr(b,g,r),1.
展示图像用cv::imshow()
cv::waitKey(0);
这是暂停，按任意键执行下个循环。