前两张图采用本质矩阵计算RT的方法,然后三角测量计算三维坐标。具体原理可以百度。
后面的增量图采用PnP的方法。通过计算图2图3匹配点和图1图2匹配点的公共部分,而图1和图2已经重建完成了,这些公共部分都是有点云坐标的,则直接PnP求解RT。
#pragma once
#include <opencv2xfeatures2dnonfree.hpp>
#include <opencv2features2dfeatures2d.hpp>
#include <opencv2highguihighgui.hpp>
#include <opencv2calib3dcalib3d.hpp>
#include <fstream>
//#include <opencv2>
#include <iostream>
#include <vector>
using namespace std;
using namespace cv;
class ImageBag
{
public:
ImageBag(Mat, Mat);
~ImageBag();
void SetKeyPoints(vector<KeyPoint>, Mat);
void SetStructIdx(vector<int>);
//公用参数
Mat Image;
Mat K;
vector<KeyPoint> Key_points;
Mat Descriptor;
Mat R;
Mat T;
//特征点所对应的空间点在点云中的索引
vector<int> Correspond_struct_idx;
private:
};
ImageBag::ImageBag(Mat image, Mat k)
{
Image = image;
K = k;
}
ImageBag::~ImageBag()
{
if (!Image.empty())
{
Image.release();
}
if (!K.empty())
{
K.release();
}
}
void ImageBag::SetStructIdx(vector<int> index)
{
Correspond_struct_idx = index;
}
void ImageBag::SetKeyPoints(vector<KeyPoint> keyPoints, Mat descriptor)
{
Key_points = keyPoints;
Descriptor = descriptor;
//根据keyPoints来初始化Correspond_struct_idx
for (int i = 0; i < keyPoints.size(); i++)
{
Correspond_struct_idx.push_back(-1);
}
}
以上是每一张图的实体类,包含了每一个图的一些信息。后面代码写起来会比较清晰。
#pragma once
#include <opencv2xfeatures2dnonfree.hpp>
#include <opencv2features2dfeatures2d.hpp>
#include <opencv2highguihighgui.hpp>
#include <opencv2calib3dcalib3d.hpp>
#include <fstream>
#include "ImageBag.h"
//#include <opencv2>
#include <iostream>
using namespace cv;
using namespace std;
static class _3DBuilder
{
public:
_3DBuilder();
~_3DBuilder();
static void ExtractFeatures(vector<ImageBag>&);
static void MatchFeatures(Mat, Mat, vector<DMatch>&);
static void GetMatchedPoints(vector<KeyPoint>, vector<KeyPoint>, vector<DMatch>, vector<Point2f>&, vector<Point2f>&);
static bool FindTransform(Mat, vector<Point2f>, vector<Point2f>, Mat&, Mat&, Mat&);
static void MaskoutPoints(vector<Point2f>&, Mat&);
static void Reconstruct(Mat&, Mat&, Mat&, Mat&, Mat&, vector<Point2f>&, vector<Point2f>&, vector<Point3f>&);
private:
};
_3DBuilder::_3DBuilder()
{
}
_3DBuilder::~_3DBuilder()
{
}
#pragma region 方法
/