从运动中恢复结构以便能更好的通过摄像机移动来提取图像几何结构。在书中为了使用单目相机，一个离散且稀疏的视频帧集合，而不是连续的视频流。这在后面两两循环组合配对提供了方便性。
主要内容：
1：从两幅图像估计摄像机的运动姿态。
2：重构场景
3：从视图中重构
4：重构细化
5：可视化三维点云
I：假定使用一个标定过的摄像机——一个先前标定过的摄像机。前面的博客也提到如何进行相机标定。因此，我们假定摄像机内参数存在，并且具体化到K矩阵中，K矩阵为摄像机标定过程的一个结果输出。
在程序中初始化相机标定内参数：
//load calibration matrix
cv::FileStorage fs;
if(fs.open(imgs_path_+ "\\out_camera_data.yml",cv::FileStorage::READ)) //打开校正参数文件
{
fs["camera_matrix"]>>cam_matrix;
fs["distortion_coefficients"]>>distortion_coeff;
} else {
//若没有标定文件，就组合一个标定内参数
cv::Size imgs_size = imgs_[0].size();
double max_w_h = MAX(imgs_size.height,imgs_size.width);
cam_matrix = (cv::Mat_<double>(3,3) << max_w_h , 0 , imgs_size.width/2.0,
0, max_w_h, imgs_size.height/2.0,
0, 0, 1);
distortion_coeff = cv::Mat_<double>::zeros(1,4);
}
K = cam_matrix;
cv::invert(K, Kinv); //对内参数进行取反
distortion_coeff.convertTo(distcoeff_32f,CV_32FC1);
K.convertTo(K_32f,CV_32FC1);若有标定文件，就从文件中导入，若没有标定文件，就组合一个相机内参K，根据图像大小就可以组合，畸变参数设为0，对相机内参取反得Kinv，转换成32FC1精度。
II：获取图像
书中在获取图像时，采用给定目录，逐个读取目录中的图像，保存在一个std::vector& imgs中。我们也可以挨个读取，或者截取视频帧。
我觉得这是一个常规代码片，放在下面供以后随便调用。
//给定目录的路径，读取目录下图像文件，保存图片名，设置图像缩放比率
void open_imgs_dir(char* dir_name, std::vector<cv::Mat>& images, std::vector<std::string>& images_names, double downscale_factor) {
if (dir_name == NULL) {
return;
}
string dir_name_ = string(dir_name);
vector<string> files_;
#ifndef WIN32
//open a directory the POSIX way
//linux或者macos下面读取图片，需要包含#include <dirent.h>
DIR *dp;
struct dirent *ep;
dp = opendir (dir_name);
if (dp != NULL)
{
while (ep = readdir (dp)) {
if (ep->d_name[0] != '.')
files_.push_back(ep->d_name);
}
(void) closedir (dp);
}
else {
cerr << ("Couldn't open the directory");
return;
}
#else
//open a directory the WIN32 way
HANDLE hFind = INVALID_HANDLE_VALUE;
WIN32_FIND_DATA fdata;
if(dir_name_[dir_name_.size()-1] == '\\' || dir_name_[dir_name_.size()-1] == '/') {
dir_name_ = dir_name_.substr(0,dir_name_.size()-1);
}
hFind = FindFirstFile(string(dir_name_).append("\\*").c_str(), &fdata);
if (hFind != INVALID_HANDLE_VALUE)
{
do
{
if (strcmp(fdata.cFileName, ".") != 0 &&
strcmp(fdata.cFileName, "..") != 0)
{
if (fdata.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)
{
continue; // a diretory
}
else
{
files_.push_back(fdata.cFileName);
}
}
}
while (FindNextFile(hFind, &fdata) != 0);
} else {
cerr << "can't open directory\n";
return;
}
if (GetLastError() != ERROR_NO_MORE_FILES)
{
FindClose(hFind);
cerr << "some other error with opening directory: " << GetLastError() << endl;
return;
}
FindClose(hFind);
hFind = INVALID_HANDLE_VALUE;
#endif
for (unsigned int i=0; i<files_.size(); i++) {
if (files_[i][0] == '.' || !(hasEndingLower(files_[i],"jpg")||hasEndingLower(files_[i],"png"))) {
continue;
}
cv::Mat m_ = cv::imread(string(dir_name_).append("/").append(files_[i]));
if(downscale_factor != 1.0)
cv::resize(m_,m_,Size(),downscale_factor,downscale_factor);//是否进行缩放
images_names.push_back(files_[i]);//存储图片名称
images.push_back(m_);//保存图片成Mat
}
}这段代码片调用了两个函数进行图片名称拼接。
bool hasEndingLower (string const &fullString_, string const &_ending)
{
string fullstring = fullString_, ending = _ending;
transform(fullString_.begin(),fullString_.end(),fullstring.begin(),::tolower); // 放在最后
return hasEnding(fullstring,ending);
}
bool hasEnding (std::string const &fullString, std::string const &ending)
{
if (fullString.length() >= ending.length()) {
return (0 == fullString.compare (fullString.length() - ending.length(), ending.length(), ending));
} else {
return false;
}
}
接下来就是进行了一步转化，先确保图像是8UC3的，然后再灰度化，就是预处理步骤。
//确保图像是CV_8UC3
for (unsigned int i=0; i<imgs_.size(); i++) {
imgs_orig.push_back(cv::Mat_<cv::Vec3b>());//cv::Vec3b是8UC3的代替，typedef Vec<uchar, 3> Vec3b;
if (!imgs_[i].empty()) {
if (imgs_[i].type() == CV_8UC1) {
cvtColor(imgs_[i], imgs_orig[i], CV_GRAY2BGR);
} else if (imgs_[i].type() == CV_32FC3 || imgs_[i].type() == CV_64FC3) {
imgs_[i].convertTo(imgs_orig[i],CV_8UC3,255.0);
} else {
imgs_[i].copyTo(imgs_orig[i]);
}
}
imgs.push_back(cv::Mat());
cvtColor(imgs_orig[i],imgs[i], CV_BGR2GRAY);
//定义vector<std::vector<cv::KeyPoint> > imgpts，imgpts_good;用于保存图像特征点。
imgpts.push_back(std::vector<cv::KeyPoint>());//
imgpts_good.push_back(std::vector<cv::KeyPoint>());
std::cout << ".";
}III：特征提取
有了图像后，对图像进行特征匹配，计算两幅图像的基础矩阵，本质矩阵。这也是SFM的第一个精华部分。
基础矩阵（用F表示）和本征矩阵（用E表示）。本征矩阵是假设使用的标定的相机，它们非常相似。OpenCV函数仅允许我们通过findFundamentalMat函数找到基础矩阵。然而，我们非常简单地使用标定矩阵（calibration matrix）K从本征矩阵中获得基础矩阵，如下：
Mat_<double> E = K.t() * F * K; //according to HZ (9.12)本征矩阵，是一个3×3大小的矩阵，使用x’Ex=0在图像中的一点和另外一个图像中的一点之间施加了一个约束，这里x是图像一中的的一点，x’是图像二中与之相对应的一点。这非常有用，因为我们将要看到。我们使用的另一个重要的事实是本征矩阵是我们用来为我们的图像恢复两个相机的所有需要，尽管缺少尺度因子，但是我们以后会得到。因此，如果我们获得了本征矩阵，我们知道每一个相机在空间中的位置，并且知道它们的观察方向，如果我们有足够这样的约束等式，那么我们可以简单地计算出这个矩阵。简单的因为每一个等式可以用来解决矩阵的一小部分。事实上，OpenCV允许我们仅使用7个点对来计算它，但是我们希望获得更多的点对来得到一个鲁棒性的解。
在计算机视觉中，特征提取和描述子匹配是一个基础的过程，并且用在许多方法中来执行各种各样的操作。例如，检测图像中一个目标的位置和方向，或者通过给出一个查询图像在大数据图像中找到相似的图像。从本质上讲，提取意味着在图像中选择点，使得获得好的特征，并且为它们计算一个描述子。一个描述子是含有多个数据的向量，用来描述在一个图像中围绕着特征点的周围环境。不同的方法有不同的长度和数据类型来表示描述子矢量。匹配是使用它的描述子从另外一个图像中找到一组与之对应的特征。OpenCV提供了非常简单和有效的方法支持特征提取和匹配。关于特征匹配的更多信息可以在Chapter 3少量（无）标记增强现实中找到。
本书采用三种特征提取方法：一般的富特征提取，基于GPU的特征提取，使用光流法进行特征提取。
1：一般的富特征提取
detector = FeatureDetector::create("PyramidFAST");
extractor = DescriptorExtractor::create("ORB");
std::cout << " -------------------- extract feature points for all images -------------------\n";
detector->detect(imgs, imgpts);//对所有图像进行特征点检测
extractor->compute(imgs, imgpts, descriptors);//计算所有特征点的描述子
std::cout << " ------------------------------------- done -----------------------------------\n";2：使用GPU的富特征提取
extractor = new SURF();//这个类可以直接提取图像的特征点和计算描述子。
std::cout << " -------------------- extract feature points for all images (GPU) -------------------\n";
imgpts.resize(imgs_.size());
descriptors.resize(imgs_.size());
CV_PROFILE("extract",
for(int img_i=0;img_i<imgs_.size();img_i++) {
GpuMat _m; _m.upload(imgs_[img_i]);
(*extractor)(_m,GpuMat(),imgpts[img_i],descriptors[img_i]);//这一行就是直接提取每幅图像的特征点和描述子
cout << ".";
}
)3：光流法：
光流是匹配来至一幅图像选择的点到另外一幅图像选择点的过程，假定这两个图像是一个视频序列的一部分并且它们彼此非常相近。大多数的光流方法比较一个小的区域，称为搜索窗口或者块，这些块围绕着图像A中的每一点和同样区域的图像B中的每一点。遵循计算机视觉中一个非常普通的规则，称为亮度恒定约束（brightness constancy constraint）（和其他名字），图像中的这些小块从一个图像到另外一个图像不会有太大的变化，因此，他们的幅值差接近于0。除了匹配块，更新的光流方法使用一些额外的方法来获得更好的结果。其中一个方法就是使用图像金字塔，它是图像越来越小的尺寸（大小）版本，这考虑到了工作的从粗糙到精致——计算机视觉中一个非常有用的技巧。另外一个方法是定义一个流场上的全局约束，假定这些点相互靠近，向同一方向一起运动。
为了和富特征的特征提取方法进行兼容，这里先用Fast特征提取算法只计算特征点。之后用光流法进行特征匹配。
//detect keypoints for all images
FastFeatureDetector ffd;
// DenseFeatureDetector ffd;
ffd.detect(imgs, imgpts);IV：特征匹配
特征提取完毕后，就要进行特征匹配。书中使用openmp进行并行for运算，需要安装openmp。这里使用循环组合的方法每两幅图像都要进行匹配。增加精度，但速度超慢。
int loop1_top = imgs.size() - 1, loop2_top = imgs.size();
int frame_num_i = 0;
#pragma omp parallel for //并行计算
for (frame_num_i = 0; frame_num_i < loop1_top; frame_num_i++) {
for (int frame_num_j = frame_num_i + 1; frame_num_j < loop2_top; frame_num_j++)
{
std::cout << "------------ Match " << imgs_names[frame_num_i] << ","<<imgs_names[frame_num_j]<<" ------------\n";//打印当前是哪两帧进行匹配
std::vector<cv::DMatch> matches_tmp;
feature_matcher->MatchFeatures(frame_num_i,frame_num_j,&matches_tmp);
//std::map<std::pair<int,int> ,std::vector<cv::DMatch> > matches_matrix;这个变量保存的是那两帧进行匹配，及匹配的结果。 matches_matrix[std::make_pair(frame_num_i,frame_num_j)] = matches_tmp;
std::vector<cv::DMatch> matches_tmp_flip = FlipMatches(matches_tmp);//对两幅图像进行调换位置，完成交叉匹配检查过滤
matches_matrix[std::make_pair(frame_num_j,frame_num_i)] = matches_tmp_flip;
}
}接下来就分析下这个里面最关键的函数feature_matcher->MatchFeatures。这个feature_matcher是前面特征提取所介绍的三种方法。每一种方法都可以进行匹配。
1：一般的富特征匹配。
采用暴力BF匹配法加上HAMMING距进行匹配，得到的匹配组很多，当然错误的也很多。
//输入当前匹配的帧以及得到的匹配
void RichFeatureMatcher::MatchFeatures(int idx_i, int idx_j, vector<DMatch>* matches) {
#ifdef __SFM__DEBUG__
const Mat& img_1 = imgs[idx_i];
const Mat& img_2 = imgs[idx_j];
#endif
//获取当前匹配帧的关键点和描述子
const vector<KeyPoint>& imgpts1 = imgpts[idx_i];
const vector<KeyPoint>& imgpts2 = imgpts[idx_j];
const Mat& descriptors_1 = descriptors[idx_i];
const Mat& descriptors_2 = descriptors[idx_j];
std::vector< DMatch > good_matches_,very_good_matches_;
std::vector<KeyPoint> keypoints_1, keypoints_2;
//打印关键点的数量
stringstream ss; ss << "imgpts1 has " << imgpts1.size() << " points (descriptors " << descriptors_1.rows << ")" << endl;
cout << ss.str();
stringstream ss1; ss1 << "imgpts2 has " << imgpts2.size() << " points (descriptors " << descriptors_2.rows << ")" << endl;
cout << ss1.str();
keypoints_1 = imgpts1;
keypoints_2 = imgpts2;
if(descriptors_1.empty()) {
CV_Error(0,"descriptors_1 is empty");
}
if(descriptors_2.empty()) {
CV_Error(0,"descriptors_2 is empty");
}
//使用BF暴力匹配法进行HAMMING匹配，允许交叉检查
BFMatcher matcher(NORM_HAMMING,true); //allow cross-check. use Hamming distance for binary descriptor (ORB)
std::vector< DMatch > matches_;
if (matches == NULL) {
matches = &matches_;
}
if (matches->size() == 0) {
matcher.match( descriptors_1, descriptors_2, *matches );
}
assert(matches->size() > 0);
// double max_dist = 0; double min_dist = 1000.0;
// //-- Quick calculation of max and min distances between keypoints
// for(unsigned int i = 0; i < matches->size(); i++ )
// {
// double dist = (*matches)[i].distance;
// if (dist>1000.0) { dist = 1000.0; }
// if( dist < min_dist ) min_dist = dist;
// if( dist > max_dist ) max_dist = dist;
// }
//
//#ifdef __SFM__DEBUG__
// printf("-- Max dist : %f \n", max_dist );
// printf("-- Min dist : %f \n", min_dist );
//#endif
vector<KeyPoint> imgpts1_good,imgpts2_good;
// if (min_dist <= 0) {
// min_dist = 10.0;
// }
//去除每一个重复匹配的训练点，即一个训练点有多个查询点 Eliminate any re-matching of training points (multiple queries to one training)
// double cutoff = 4.0*min_dist;
std::set<int> existing_trainIdx;
for(unsigned int i = 0; i < matches->size(); i++ )
{
//归一化匹配：有时图像数下标就是训练数的下标"normalize" matching: somtimes imgIdx is the one holding the trainIdx
if ((*matches)[i].trainIdx <= 0) {
(*matches)[i].trainIdx = (*matches)[i].imgIdx;
}
//这里set::find的意思是在set中查找键值，若找到则返回键值迭代器的位置，若找不到就返回set::end，这是为了
//防止有一个训练值对应多个查询值。每个训练值检测过后，就存入set中，下次这个训练值再出现在匹配对中时，就判断为多对一去除。
//另外这里的if中也要判断训练值要在0和关键点数量之间。
if( existing_trainIdx.find((*matches)[i].trainIdx) == existing_trainIdx.end() &&
(*matches)[i].trainIdx >= 0 && (*matches)[i].trainIdx < (int)(keypoints_2.size()) /*&&
(*matches)[i].distance > 0.0 && (*matches)[i].distance < cutoff*/ )
{
good_matches_.push_back( (*matches)[i]);//符合条件的匹配组
imgpts1_good.push_back(keypoints_1[(*matches)[i].queryIdx]);//符合条件的匹配组的查询关键点
imgpts2_good.push_back(keypoints_2[(*matches)[i].trainIdx]);//符合条件的匹配组的训练关键点
existing_trainIdx.insert((*matches)[i].trainIdx);
}
}
//这里第一步的匹配就完成了，这时会有很多错误的匹配。
#ifdef __SFM__DEBUG__
cout << "keypoints_1.size() " << keypoints_1.size() << " imgpts1_good.size() " << imgpts1_good.size() << endl;
cout << "keypoints_2.size() " << keypoints_2.size() << " imgpts2_good.size() " << imgpts2_good.size() << endl;
{
//-- Draw only "good" matches
Mat img_matches;
drawMatches( img_1, keypoints_1, img_2, keypoints_2,
good_matches_, img_matches, Scalar::all(-1), Scalar::all(-1),
vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
//-- Show detected matches
stringstream ss; ss << "Feature Matches " << idx_i << "-" << idx_j;
imshow(ss.str() , img_matches );
waitKey(500);
destroyWindow(ss.str());
}
#endif
//下面将用计算基础矩阵的方式再次优化匹配组。
vector<uchar> status;
vector<KeyPoint> imgpts2_very_good,imgpts1_very_good;
assert(imgpts1_good.size() > 0);
assert(imgpts2_good.size() > 0);
assert(good_matches_.size() > 0);
assert(imgpts1_good.size() == imgpts2_good.size() && imgpts1_good.size() == good_matches_.size());
//Select features that make epipolar sense
//计算匹配组的基础的矩阵，再次优化匹配组，去除错误匹配。
GetFundamentalMat(keypoints_1,keypoints_2,imgpts1_very_good,imgpts2_very_good,good_matches_);
//Draw matches
#ifdef __SFM__DEBUG__
{
//-- Draw only "good" matches
Mat img_matches;
drawMatches( img_1, keypoints_1, img_2, keypoints_2,
good_matches_, img_matches, Scalar::all(-1), Scalar::all(-1),
vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
//-- Show detected matches
imshow( "Good Matches", img_matches );
waitKey(100);
destroyWindow("Good Matches");
}
#endif
}
2：使用GPU加速的特征匹配
使用knn比率测试进行匹配，即最优匹配的距离/次优大于0.7，去除错误匹配。这时还会有错误匹配。
//同样使用暴力匹配，只不过有gpu加速。matching descriptor vectors using Brute Force matcher
BruteForceMatcher_GPU<L2<float> > matcher;
std::vector< DMatch > matches_;
if (matches == NULL) {
matches = &matches_;
}
if (matches->size() == 0) {
cout << "match " << descriptors_1.rows << " vs. " << descriptors_2.rows << " ...";
if(use_ratio_test) {
vector<vector<DMatch> > knn_matches;
GpuMat trainIdx,distance,allDist;
CV_PROFILE("match",
matcher.knnMatchSingle(descriptors_1,descriptors_2,trainIdx,distance,allDist,2); //使用knn比率测试进行匹配
matcher.knnMatchDownload(trainIdx,distance,knn_matches);
)
(*matches).clear();
//ratio test
for(int i=0;i<knn_matches.size();i++) {
if(knn_matches[i][0].distance / knn_matches[i][1].distance < 0.7) {//knn比率判别，去除匹配不好的匹配组
(*matches).push_back(knn_matches[i][0]);
}
}
cout << "kept " << (*matches).size() << " features after ratio test"<<endl;
} else {
CV_PROFILE("match",matcher.match( descriptors_1, descriptors_2, *matches );)
}
}3：光流法进行匹配。大致就是每个左图点在两幅图中是如何移动的，在右图中找每一个光流点，在小区域进行匹配。
Vector<KeyPoint>left_keypoints,right_keypoints;
// Detect keypoints in the left and right images
FastFeatureDetectorffd;
ffd.detect(img1, left_keypoints);
ffd.detect(img2, right_keypoints);
vector<Point2f>left_points;
KeyPointsToPoints(left_keypoints,left_points);//关键点转化成Point2f点
vector<Point2f>right_points(left_points.size());
// making sure images are grayscale
Mat prevgray,gray;
if (img1.channels() == 3) {
cvtColor(img1,prevgray,CV_RGB2GRAY);
cvtColor(img2,gray,CV_RGB2GRAY);
} else {
prevgray = img1;
gray = img2;
}
// Calculate the optical flow field:
// how each left_point moved across the 2 images
vector<uchar>vstatus; vector<float>verror;
calcOpticalFlowPyrLK(prevgray, gray, left_points, right_points,
vstatus, verror);
// First, filter out the points with high error
vector<Point2f>right_points_to_find;
vector<int>right_points_to_find_back_index;
for (unsigned inti=0; i<vstatus.size(); i++) {
if (vstatus[i] &&verror[i] < 12.0) {
// Keep the original index of the point in the
// optical flow array, for future use
right_points_to_find_back_index.push_back(i);
// Keep the feature point itself
right_points_to_find.push_back(j_pts[i]);
} else {
vstatus[i] = 0; // a bad flow
}
}
// for each right_point see which detected feature it belongs to
Mat right_points_to_find_flat = Mat(right_points_to_find).
reshape(1,to_find.size()); //flatten array
vector<Point2f>right_features; // detected features
KeyPointsToPoints(right_keypoints,right_features);
Mat right_features_flat = Mat(right_features).reshape(1,right_
features.size());
// Look around each OF point in the right image
// for any features that were detected in its area
// and make a match.
BFMatchermatcher(CV_L2);
vector<vector<DMatch>>nearest_neighbors;
matcher.radiusMatch(
right_points_to_find_flat,
right_features_flat,
nearest_neighbors,
2.0f);
// Check that the found neighbors are unique (throw away neighbors
// that are too close together, as they may be confusing)
std::set<int>found_in_right_points; // for duplicate prevention
for(inti=0;i<nearest_neighbors.size();i++) {
DMatch _m;
if(nearest_neighbors[i].size()==1) {
_m = nearest_neighbors[i][0]; // only one neighbor
} else if(nearest_neighbors[i].size()>1) {
// 2 neighbors – check how close they are
double ratio = nearest_neighbors[i][0].distance /
nearest_neighbors[i][1].distance;
if(ratio < 0.7) { // not too close
// take the closest (first) one
_m = nearest_neighbors[i][0];
} else { // too close – we cannot tell which is better
continue; // did not pass ratio test – throw away
}
} else {
continue; // no neighbors... :(
}
// prevent duplicates
if (found_in_right_points.find(_m.trainIdx) == found_in_right_points.
end()) {
// The found neighbor was not yet used:
// We should match it with the original indexing
// ofthe left point
_m.queryIdx = right_points_to_find_back_index[_m.queryIdx];
matches->push_back(_m); // add this match
found_in_right_points.insert(_m.trainIdx);
}
}
cout<<"pruned "<< matches->size() <<" / "<<nearest_neighbors.size()
<<" matches"<<endl;
一个特征从左手边图像的一个位置移动到右手边图像的另外一个位置。但是我们有一组在右手边图像中检测到的新的特征，在光流中从这个图像到左手边图像的特征不一定是对齐的。我们必须使它们对齐。为了找到这些丢失的特征，我们使用一个k邻近（KNN）半径搜索,这给出了我们两个特征，即感兴趣的点落入了2个像素半径范围内。
KNN比率测试实现，在SfM中这是一种常见的减少错误的方法。实质上，当我们对左手边图像上的一个特征和右手边图像上的一个特征进行匹配时，它作为一个滤波器，用来移除混淆的匹配。如果右手边图像中两个特征太靠近，或者它们之间这个比例（the rate）太大（接近于1.0），我们认为它们混淆了并且不使用它们。我们也安装一个双重防御滤波器来进一步修剪匹配。
使用光流法代替丰富特征的优点是这个过程通常更快并且可以适应更多的点，使重构更加稠密。在许多光流方法中也有一个块整体运动的统一模型，在这个模型中，丰富的特征匹配通常不考虑。使用光流要注意的是对于从同一个硬件获取的连续图像，它处理的很快，然而丰富的特征通常不可知。它们之间的差异源于这样的一个事实：光流法通常使用非常基础的特征，像围绕着一个关键点的图像块，然而，高阶丰富特征（例如，SURF）考虑每一个特征点的较高层次的信息。使用光流或者丰富的特征是设计师根据应用程序的输入所做的决定。
接下来将要求解摄像机矩阵，下一篇再分析。