SLAM程序阅读（第8讲 半稠密直接法）
SLAM程序阅读（第8讲 半稠密直接法）
这期我们来继续读一下半稠密直接法求解位姿的程序direct_semidense.cpp。
这个程序与我们上一期读的稀疏直接法direct_sparse.cpp的唯一差别就在于所选取的特征点，我们先来看一下程序的运行结果
稀疏法选取的是FAST特征点，特征点的选取会相对稀疏；在半稠密直接法中我们选点则是选取所有<灰度变化比较明显的像素>，将这些点作为特征点进行光流跟踪与直接法位姿求取。我们先来看一下子函数声明与类、结构体的定义：
struct Measurement
{
...
};
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
...
}
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
...
}
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
...
};可以看出函数的声明与类的定义与稀疏直接法中的定义完全相同，也就是说两个程序在特征点位置与灰度信息的使用（结构体Measurement的定义）、坐标变换方式（project3Dto2D与project2Dto3D的定义）、使用g2o进行图优化求解位姿（poseEstimationDirect的声明与定义）都完全一致，因此在此不做赘述。
下面来看主函数。主函数中，可以发现除了第一帧特征点的选取方式有所改变之外，在读取图像位置信息、其余帧的直接法求解以及相应的函数调用、位姿求解结果的图像输出等其他操作也完全一致。那么我们就只来比对一下稀疏法与直接法在特征点求取上的不同。
稀疏直接法
if ( index ==0 )
{
// 对第一帧提取FAST特征点
vector<cv::KeyPoint> keypoints;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect ( color, keypoints );
for ( auto kp:keypoints )
{
// 去掉邻近边缘处的点
if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
continue;
ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
if ( d==0 )
continue;
Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
measurements.push_back ( Measurement ( p3d, grayscale ) );
}
prev_color = color.clone();
continue;
}首先实例化一个存储KeyPoint类对象的容器keypoints，用来存储提取得到的特征点。虽然后续有特征点的筛选（去掉边缘处的点），但由于筛选后的点会经过其他处理存入其他容器，因此在此没有使用链表。进而实例化一个Ptr结构体detector，调用detector内部的detect()函数进行FAST角点的计算，将当前帧color中的FAST角点存入容器keypoints中。
进而，通过遍历keypoints中每个元素（即刚提取出的每个特征点）的坐标值pt中的横纵像素值（即kp.pt.x与kp.pt.y），判断其像素坐标是否位于图像边缘处，以此来筛选掉边缘处的特征点。同时，判断在该像素位置的深度图中是否能查询到其深度信息，若查询不到该位置点的深度信息依然需要舍去该点。
最后，调用project2Dto3D()函数，利用特征点的像素坐标与深度值将其转化为相机坐标系下的3d坐标并存入三维向量p3d中，同时在灰度图中查找该点处的灰度值并存入浮点变量grayscale中，最后将这两个变量一同构造成一个Measurement结构体变量，压入之前定义好的容器measurements中。
半稠密直接法
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++ )
{
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]
);
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;
}在半稠密直接法中，通过遍历灰度图gray中的各个像素（在此做了一些约束，限制所查询的点的位置不在边缘处10像素范围内），构造了一个二维向量delta用来存储该点处的灰度值梯度（更确切得讲其实是每个像素位置横、纵两个方向相邻像素间灰度值变化值），即：
通过判断梯度向量的二范数（即欧氏距离）是否大于50来判断是否将其作为特征点。这里50这个判别值是因为选取norm()函数作为二范数求取函数，若选取其他函数如squaredNorm()则需要修改阈值。
最后，同样通过判断是否能够查询到深度值来选择保留该特征点与否，并最终将每个特征点的3d坐标与灰度值存入容器measurements。
下面来对比一下稀疏直接法与半稠密直接法的运行结果：
↑稀疏直接法
↑半稠密直接法
可以看出半稠密直接法的特征点个数更多，且就算随机选取五分之一的特征点进行展示，也可以遍布物体的边缘等灰度值突变的位置。
好了，本期的半稠密直接法程序阅读就到此结束。还是一样，由于近期我们开了矩阵分析以及其他多门学位课，小绿发文的周期会比较失调，希望大家能够不离不弃。感谢支持。