opencv实现三维点云重建

最近几天真的好气，搞了半天结果却是毛线。

opencv版本为3.4.1，pcl版本为1.8，开发环境为vs2013，不多说，贴代码（能跑通，但是结果只有一条极线，好气！！)

先打个草稿，只是为了安慰自己，这几天还是干了点东西的，好一点的结果出来后，还会来完善这篇博客，等我好消息。

#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/core.hpp"
#include "opencv2/features2d.hpp"
#include   
#include
#include
#include  
#include


using namespace cv;
using namespace std;


void main()


{


ifstream fin("calibdata.txt"); // 标定所用图像文件的路径 
ofstream fout;  // 保存标定结果的文件 
fout.open("caliberation_result.txt");
//读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化   
cout << "开始提取角点………………" << endl;
int image_count = 0;  // 图像数量 
Size image_size;  // 图像的尺寸 
Size board_size = Size(7, 7);    // 标定板上每行、列的角点数 
vector image_points_buf;  //缓存每幅图像上检测到的角点 
vector> image_points_seq; // 保存检测到的所有角点 
string filename;
int count = -1;//用于存储角点个数。  
while (getline(fin, filename))
{
image_count++;
// 用于观察检验输出  
cout << "image_count = " << image_count << endl;
//输出检验
cout << "-->count = " << count;
Mat imageInput = imread(filename);
if (image_count == 1)  //读入第一张图片时获取图像宽高信息  
{
image_size.width = imageInput.cols;
image_size.height = imageInput.rows;
cout << "image_size.width = " << image_size.width << endl;
cout << "image_size.height = " << image_size.height << endl;
}


// 提取角点 
if (findChessboardCorners(imageInput, board_size, image_points_buf) == 0)
{
cout << "can not find chessboard corners!\n"; //找不到角点  
exit(1);
}
else
{
Mat view_gray;
cvtColor(imageInput, view_gray, CV_RGB2GRAY);
// 亚像素精确化 
find4QuadCornerSubpix(view_gray, image_points_buf, Size(7, 7)); //对粗提取的角点进行精确化  
image_points_seq.push_back(image_points_buf);  //保存亚像素角点  
// 在图像上显示角点位置
drawChessboardCorners(view_gray, board_size, image_points_buf, true); //用于在图片中标记角点  
namedWindow("Camera Calibration", 0);
resizeWindow("Camera Calibration", 640, 480);
imshow("Camera Calibration", view_gray);//显示图片  
waitKey(500);//暂停0.5S         
}
}
int total = image_points_seq.size();
cout << "total = " << total << endl;
int CornerNum = board_size.width*board_size.height;  //每张图片上总的角点数  
for (int ii = 0; ii {
if (ii%CornerNum == 0)// 24 是每幅图片的角点个数。此判断语句是为了输出 图片号，便于控制台观看   
{
int i = -1;
i = ii / CornerNum;
int j = i + 1;
cout << "--> 第 " << j << "图片的数据 --> : " << endl;
}
if (0 == ii % 3)  // 此判断语句，格式化输出，便于控制台查看  
{
cout << endl;
}
else
{
cout.width(10);
}
//输出所有的角点  
cout << " -->" << image_points_seq[ii][0].x;
cout << " -->" << image_points_seq[ii][0].y;
}
cout << "角点提取完成！\n";


//以下是摄像机标定  
cout << "开始标定………………";
//棋盘三维信息
Size square_size = Size(20, 20);  // 实际测量得到的标定板上每个棋盘格的大小 
vector> object_points; // 保存标定板上角点的三维坐标 
//外参数
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); // 摄像机内参数矩阵 
vector point_counts;  // 每幅图像中角点的数量  
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); // 摄像机的5个畸变系数：k1,k2,p1,p2,k3 
vector tvecsMat;  // 每幅图像的旋转向量 
vector rvecsMat; //每幅图像的平移向量 
// 初始化标定板上角点的三维坐标 
//int i, j, t;
for (int t = 0; t {
vector tempPointSet;
for (int i = 0; i {
for (int j = 0; j {
Point3f realPoint;
//假设标定板放在世界坐标系中z=0的平面上 
realPoint.x = i*square_size.width;
realPoint.y = j*square_size.height;
realPoint.z = 0;
tempPointSet.push_back(realPoint);
}
}
object_points.push_back(tempPointSet);
}
//初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 
for (int i = 0; i {
point_counts.push_back(board_size.width*board_size.height);
}
//开始标定 
calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);
cout << "标定完成！\n";
//对标定结果进行评价  
cout << "开始评价标定结果………………\n";
double total_err = 0.0; //图像的平均误差的总和 
double err = 0.0; //每幅图像的平均误差 
vector image_points2; //保存重新计算得到的投影点 
cout << "\t每幅图像的标定误差：\n";
fout << "每幅图像的标定误差：\n";
for (int i = 0; i {
vector tempPointSet = object_points[i];
//通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 
projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
//计算新的投影点和旧的投影点之间的误差
vector tempImagePoint = image_points_seq[i];
Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
for (size_t j = 0; j < tempImagePoint.size(); j++)
{
image_points2Mat.at(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
tempImagePointMat.at(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
}
err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
total_err += err /= point_counts[i];
std::cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
}
std::cout << "总体平均误差：" << total_err / image_count << "像素" << endl;
fout << "总体平均误差：" << total_err / image_count << "像素" << endl << endl;
std::cout << "评价完成！" << endl;
//保存定标结果      
std::cout << "开始保存定标结果………………" << endl;
Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); //保存每幅图像的旋转矩阵
fout << "相机内参数矩阵：" << endl;
fout << cameraMatrix << endl << endl;
fout << "畸变系数：\n";
fout << distCoeffs << endl << endl << endl;
for (int i = 0; i {
fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
fout << tvecsMat[i] << endl;
//旋转向量转换为相对应的旋转矩阵 
Rodrigues(tvecsMat[i], rotation_matrix);
fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
fout << rotation_matrix << endl;
fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
fout << rvecsMat[i] << endl << endl;
}
Mat K = cameraMatrix;
fout << endl; 
fout.close();
//Create SIFT class pointer
Ptr f2d = xfeatures2d::SIFT::create();
//读入图片
Mat img_1 = imread("bear1.jpg");
Mat img_2 = imread("bear2.jpg");
//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();

Mat p1(ptCount, 2, CV_32F);
Mat p2(ptCount, 2, CV_32F);
// 把Keypoint转换为Mat


Point2f pt;
for (int i = 0; i {
pt = keypoints_1[matches[i].queryIdx].pt;
p1.at(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);

fout << "基础矩阵为：" << F << endl;
Mat_ E = K.t() * F * 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 
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));

//cout <

int points_size = (int)keypoints_1.size();
vector points_1, points_2;
int ptCount_1 = ptCount;
points_1 = keypoints_1;
points_2 = keypoints_2;




Mat p_1(ptCount_1, 2, CV_32F);
Mat p_2(ptCount_1, 2, CV_32F);
// 把Keypoint转换为Mat
for (int i = 0; i {
pt = points_1[matches[i].queryIdx].pt;
p_1.at(i, 0) = pt.x;
p_1.at(i, 1) = pt.y;


pt = points_2[matches[i].trainIdx].pt;
p_2.at(i, 0) = pt.x;
p_2.at(i, 1) = pt.y;
}




Mat Kinv;
Kinv = invert(K, Kinv);
MatSize length = Kinv.size;


vector u_1, u_2;
for (int i = 0; i < ptCount_1; i++)
{
u_1.push_back(Point3f(p_1.at(i, 0), p_1.at(i, 1), 1));
u_2.push_back(Point3f(p_2.at(i, 0), p_2.at(i, 1), 1));


}
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  
);
fout.open("pointcloud.txt");
vector pointcloud;
Mat_ X_1,X;
for (int i = 0; i < ptCount_1; i++)
{
X_1 = LinearLSTriangulation(u_1[i], P, u_2[i], P1);
X.push_back(X_1);
pointcloud.push_back (Point3d(X_1(0), X_1(1), X_1(2)));

}
cout <<"点云结果为："<< pointcloud< fout << "点云结果为："< fout << pointcloud;
fout.close();


pcl::PointCloud pointcloud1;
pointcloud1.width = ptCount_1;
pointcloud1.height = 1;
pointcloud1.height = 1;
pointcloud1.height = 1;
pointcloud1.is_dense = false;
pointcloud1.points.resize(pointcloud1.width * pointcloud1.height);
for (size_t i = 0; i < ptCount_1; i++)
{
pointcloud1[i].x = pointcloud[i].x;
pointcloud1[i].y = pointcloud[i].y;
pointcloud1[i].z = pointcloud[i].z;


}
pcl::io::savePCDFileASCII("pointcloud.pcd", pointcloud1);





pcl::PointCloud::Ptr cloud1(new pcl::PointCloud); // 创建点云（指针）  
pcl::io::loadPCDFile("pointcloud.pcd", *cloud1);
pcl::visualization::PCLVisualizer viewer;
cout << "点云结果如图所示" << endl;
viewer.addPointCloud(cloud1, "cloud1");
viewer.spin();


system("pause");






}
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;
}


int numofPoints(char* fname) {
int n = 0;
int c = 0;
FILE *fp;
fp = fopen(fname, "r");
do{
c = fgetc(fp);
if (c == '\n'){
++n;
}
} while (c != EOF);
fclose(fp);
return n;
}