opencv单目相机三维点云重建（附代码）
首先，本篇博客编程平台为：vs2013
软件库为：opencv3.4.1 pcl1.8.0
需要配置环境相同才能实现结果
此外，本篇文章参考了以下两位大佬的内容：
https://blog.csdn.net/zhazhiqiang/article/details/42740429
https://blog.csdn.net/shadow_guo/article/details/44193993###
上一篇博客里，无论输入什么图片，生成的都只有一条直线，冥思苦想，想到一个好办法，将求解过程中的每一个数据都输出，查看问题出在哪里，发现Matx33d这个功能使用错误，正确的用法是：
Matx33d K（1，2，3，4，5，6，7，8，9）；并没有我们平时定义赋值时的等号，否则只会将最后一个元素输入矩阵的第一个元素，其余数值为零。
本次拍摄的照片是两个饮料瓶，如图所示：
以下是执行代码：
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include
#include
#include
#include "opencv2/features2d.hpp"
#include
#include
#include
#include
#include
#include
using namespace cv;
using namespace std;
void main()
{ //函数声明：
Mat_ LinearLSTriangulation(
Point3d u,//homogenous image point (u,v,1)
Matx34d P,//camera 1 matrix
Point3d u1,//homogenous image point in 2nd camera
Matx34d P1//camera 2 matrix
);
bool CheckCoherentRotation(cv::Mat_& R);
//输出文件函数：
string getfilename(string filename1,int i,string filename2);
//点云显示框建立
pcl::visualization::PCLVisualizer viewer;
cout << "正在生成点云，请等待..." << endl;
//相机标定部分
ifstream fin; // 标定所用图像文件的路径
ofstream fout; // 保存标定结果的文件
int k = 1;
int ii = 0;
Matx33d K(1262.958489659621, 0, 562.3792964280647,
0, 1266.222824039161, 727.8125879762329,
0, 0, 1);
cout << "K="< f2d = xfeatures2d::SIFT::create();
//Detect the keypoints
vector keypoints_1, keypoints_2;
f2d->detect(img_1, keypoints_1);
f2d->detect(img_2, keypoints_2);
//Calculate descriptors (feature vectors)
Mat descriptors_1, descriptors_2;
f2d->compute(img_1, keypoints_1, descriptors_1);
f2d->compute(img_2, keypoints_2, descriptors_2);
//Matching descriptor vector using BFMatcher
BFMatcher matcher;
vector matches;
matcher.match(descriptors_1, descriptors_2, matches);
//此处应该有掌声，谢谢大佬
int ptCount = (int)matches.size();
cout <<"第"<(i, 0) = pt.x;
p1.at(i, 1) = pt.y;
pt = keypoints_2[matches[i].trainIdx].pt;
p2.at(i, 0) = pt.x;
p2.at(i, 1) = pt.y;
}
Mat F;
F = findFundamentalMat(p1, p2,FM_RANSAC);
cout << "基础矩阵为：" << F << endl;
//绘制匹配出的关键点
Mat img_matches;
drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_matches);
//namedWindow("【match图】",0);
//resizeWindow("【match图】", 1280, 480);
//imshow("【match图】", img_matches);
//重建部分
Mat_ E = Mat(K.t()) * F * Mat(K);
SVD svd(E);
Matx33d W(0, -1, 0,//HZ 9.13
1, 0, 0,
0, 0, 1);
Mat_ R = svd.u * Mat(W) * svd.vt; //HZ 9.19
Mat_ t = svd.u.col(2); //u3
if (!CheckCoherentRotation(R))
{
cout << "resulting rotation is not coherent\n";
}
Matx34d P(1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
Matx34d P1(R(0, 0), R(0, 1), R(0, 2), t(0),
R(1, 0), R(1, 1), R(1, 2), t(1),
R(2, 0), R(2, 1), R(2, 2), t(2));
Mat p_1(ptCount, 2, CV_32F);
Mat p_2(ptCount, 2, CV_32F);
// 把Keypoint转换为Mat
for (int i = 0; i < ptCount; i++)
{
pt = keypoints_1[matches[i].queryIdx].pt;
p_1.at(i, 0) = pt.x;
p_1.at(i, 1) = pt.y;
pt = keypoints_2[matches[i].trainIdx].pt;
p_2.at(i, 0) = pt.x;
p_2.at(i, 1) = pt.y;
}
Mat Kinv;
invert(K, Kinv, DECOMP_LU);
cout << "这个矩阵为："< u_1, u_2;
vector pointcloud;
Mat_ X_1, X;
vector reproj_error;
for (size_t i = 0; i < ptCount; i++)
{
Point2f kp = keypoints_1[i].pt;
Point2f kp1 = keypoints_1[i].pt;
Point3d u_1(kp.x, kp.y, 1);
Point3d u_2(kp1.x, kp1.y, 1);
//cout << "abc"< um_1 = Kinv * Mat_(u_1);
// cout <<"ijk"<< um_1 << endl;
u_1 = Point3d(um_1);
Mat_ um_2 = Kinv * Mat_(u_2);
u_2 = Point3d(um_2);
// cout << "xyz" << u_1 << u_2 << endl;
X_1 = LinearLSTriangulation(u_1, P, u_2, P1);
X.push_back(X_1);
pointcloud.push_back(Point3d(X_1(0), X_1(1), X_1(2)));
//Mat_ xPt_img = K * Mat(P1) * X_1;
//Point2f xPt_img1(xPt_img(0) / xPt_img(2), xPt_img(1) / xPt_img(2));
//reproj_error.push_back(norm(xPt_img1 - kp1));
}
string filename3 = getfilename("pointcloud", k, ".txt");
fout.open(filename3);
//cout <<"点云结果为："<< pointcloud< pointcloud1;
pointcloud1.width = ptCount;
pointcloud1.height = 1;
pointcloud1.is_dense = false;
pointcloud1.points.resize(pointcloud1.width * pointcloud1.height);
for (size_t i = 0; i < ptCount; i++)
{
pointcloud1[i].x = pointcloud[i].x;
pointcloud1[i].y = pointcloud[i].y;
pointcloud1[i].z = pointcloud[i].z;
}
string filename4 = getfilename("pointcloud", k, ".pdb");
pcl::io::savePCDFileASCII(filename4, pointcloud1);
string filename5;
filename5 = getfilename("cloud", k, "");
pcl::PointCloud::Ptr cloud1(new pcl::PointCloud); // 创建点云（指针）
//string filename6 = getfilename("pointcloud", k, "pdb");
pcl::io::loadPCDFile(filename4, *cloud1);
viewer.addPointCloud(cloud1, filename5);
k++;
}
ii++;
fin.close();
fin.open("sift.txt");
for (int j = 0; j < ii; j++)
{
getline(fin, filename1);
}
}
cout << "点云结果如图所示" << endl;
viewer.spin();
system("pause");
}
//函数定义
bool CheckCoherentRotation(cv::Mat_& R) {
if (fabsf(determinant(R)) - 1.0 > 1e-07) {
cerr << "det(R) != +-1.0, this is not a rotation matrix" << endl;
return false;
}
return true;
}
Mat_ LinearLSTriangulation(
Point3d u,//homogenous image point (u,v,1)
Matx34d P,//camera 1 matrix
Point3d u1,//homogenous image point in 2nd camera
Matx34d P1//camera 2 matrix
)
{
//build A matrix
Matx43d A(u.x*P(2, 0) - P(0, 0), u.x*P(2, 1) - P(0, 1), u.x*P(2, 2) - P(0, 2),
u.y*P(2, 0) - P(1, 0), u.y*P(2, 1) - P(1, 1), u.y*P(2, 2) - P(1, 2),
u1.x*P1(2, 0) - P1(0, 0), u1.x*P1(2, 1) - P1(0, 1), u1.x*P1(2, 2) - P1(0, 2),
u1.y*P1(2, 0) - P1(1, 0), u1.y*P1(2, 1) - P1(1, 1), u1.y*P1(2, 2) - P1(1, 2)
);
//build B vector
Matx41d B(-(u.x*P(2, 3) - P(0, 3)),
-(u.y*P(2, 3) - P(1, 3)),
-(u1.x*P1(2, 3) - P1(0, 3)),
-(u1.y*P1(2, 3) - P1(1, 3)));
//solve for X
Mat_ X;
solve(A, B, X, DECOMP_SVD);
return X;
}
string getfilename(string filename1 ,int i,string filename2)
{
//第i个文件名为：
string filename;
char s[10];
_itoa(i, s, 10);
string str = s;
filename = filename1 + str + filename2;
return filename;
} 最终执行结果如图所示：
满意的看官老爷给点个赞，谢谢了。
还在继续学习的路上，希望能够取得长足进步，给大家带来更好的博客。