三维重建之条纹投影结构光（四）——完结（包含重构）

        接上面三文：

三维重建之条纹投影结构光（一）https://blog.csdn.net/beyond951/article/details/123361852?spm=1001.2014.3001.5501三维重建之条纹投影结构光（二）——四步相移+三频外差法https://blog.csdn.net/beyond951/article/details/123769596?spm=1001.2014.3001.5501三维重建之条纹投影结构光（三）——相高模型标定https://blog.csdn.net/beyond951/article/details/124359619        本文贴出完整全部代码，但重构的结果不尽如人意，希望和大家交流，找出隐藏的bug，总体实现思路流程是没问题的。

#include "FringeStructuredLight.h"
#include 
#include   
#include  
FringeStructuredLight Api; 

using namespace std;

int main()
{
	//********声明一些变量存储相位图和高度**********//
	vector heightPhase;
	vector height;
	//********读取文件路径下的图片**********//
	cout << "开始计算各个高度的相位展开图" << endl;
	//参考平面的相位
	string filePath_h0 = "D:\\01-条纹投影结构光\\资料\\h0\\*.bmp";
	vector imgVec_h0 = Api.ReadImg(filePath_h0);
	vector mainPhaseVec_h0 = Api.SolveThePhase(imgVec_h0);
	Mat phase_h0= Api.UnwrappedPhase(mainPhaseVec_h0);

	//二十个标定平面的相位求解
	for (int i = 1; i < 21; i++)
	{
		string filePath = "D:\\01-条纹投影结构光\\资料\\h"+to_string(i)+"\\*.bmp";
		vector imgVec = Api.ReadImg(filePath);
		//********对文件路径下的十二副图计算三组主值相位**********//
		vector mainPhaseVec = Api.SolveThePhase(imgVec);
		//********相位展开**********//
		Mat unwrapPhase = Api.UnwrappedPhase(mainPhaseVec)- phase_h0;
		heightPhase.push_back(unwrapPhase);
	}
	cout << "各个高度的相位图展开完毕" << endl;
	cout << "将各个高度计算赋值" << endl;
	for (int i = 1; i < 21; i++)
	{
		double temp = 0.25*i;
		height.push_back(temp);
	}
	cout << "各个高度赋值完毕" << endl;
	cout << "开始标定" << endl;
	vector abcMat = Api.Calibration(heightPhase, height);
	cout << "标定结束" << endl;
	cout << "开始重构" << endl;
	cout << "开始对求解相位图" << endl;
	string filePath_Part = "D:\\01-条纹投影结构光\\资料\\part\\*.bmp";
	vector imgVecPart = Api.ReadImg(filePath_Part);
	vector mainPhaseVecPart = Api.SolveThePhase(imgVecPart);
	//********相位展开**********//
	Mat unwrapPhasePart = Api.UnwrappedPhase(mainPhaseVecPart) - phase_h0;
	Mat heightPart = Api.Reconstruct(abcMat, unwrapPhasePart);
	//将重构的信息保存为txt
	ofstream fout(".\\part.txt");
	for (int i = 0; i < heightPart.rows; i++)
	{
		float * heightPart_P = heightPart.ptr(i);
		for (int j = 0; j < heightPart.cols; j++)
		{
			fout << i << " " << j << " " << heightPart_P[j] << endl;
		}
	}

	return 0;
}

#include "FringeStructuredLight.h"
const int Resolution = 3145728;
const float PI2 = 6.2831853;
const float m_eps = 0.0000001;



FringeStructuredLight::FringeStructuredLight()
{
}

FringeStructuredLight::~FringeStructuredLight()
{
}

//*****************读取文件夹里面的图片******************//
vector FringeStructuredLight::ReadImg(std::string filepath)
{
	vector tempVec;
	std::vector image_files;
	glob(filepath, image_files);
	//最后一张图不读
	for (int i = 0; i < image_files.size(); i++)
	{
		Mat temp = imread(image_files[i]);
		tempVec.push_back(temp);
	}
	return tempVec;
}
//*****************四步相移法求解图像的相位主值******************//
//本次实验采用四步相移法+三频外差法
//输入输出：输入位十二张图片；输出为三张不同频率的主值图
vector FringeStructuredLight::SolveThePhase(vector srcVec)
{
	vector tempVec;
	if (srcVec.size() % 4 != 0)
	{
		return tempVec;
	}
	else
	{
		for (int i = 0; i < srcVec.size(); i+=4)
		{
			int row = srcVec[i].rows;
			int col = srcVec[i].cols;
			Mat tempPhase = Mat::zeros(srcVec[i].size(), CV_32FC1);
			float a, b;
			for (int k = 0; k < row; k++)
			{
				uchar *r1 = srcVec[i].ptr(k);
				uchar *r2 = srcVec[i + 1].ptr(k);
				uchar *r3 = srcVec[i + 2].ptr(k);
				uchar *r4 = srcVec[i + 3].ptr(k);
				float *ph = tempPhase.ptr(k);
				for (int j = 0; j < col; j++)
				{
					a = r1[j] - r3[j];
					b = r4[j] - r2[j];
					ph[j] = atan2(a, b);
				}
			}
			tempVec.push_back(tempPhase);
		}
		return tempVec;
	}
}
//*****************三频外差法对四步相移法求的相位进行展开******************//
//本次实验采用四步相移法+三频外差法
//输入输出：输入位三张主值相位图片；输出为相位的展开图
Mat FringeStructuredLight::UnwrappedPhase(vector srcVec)
{
	Mat unwrapPhase;
	double wrapPhase12, wrapPhase23, wrapPhase123;
	double unPH12, unPH23, unPH123, unPH1, unPH2, unPH3;
	int f1 = 73, f2 = 64, f3 = 56, f12 = 9, f23 = 8, f123 = 1;
	unwrapPhase = Mat::zeros(srcVec[0].size(), CV_32FC1);
	for (int i = 0; i < srcVec[0].rows; i++)
	{
		float * un1 = srcVec[0].ptr(i);
		float * un2 = srcVec[1].ptr(i);
		float * un3 = srcVec[2].ptr(i);
		float * ph = unwrapPhase.ptr(i);
		for (int j = 0; j < srcVec[0].cols; j++)
		{
			if ((un1[j] - un2[j] > -PI2) && (un1[j] - un2[j] <0))
				wrapPhase12 = un1[j] - un2[j] + PI2;
			else
				wrapPhase12 = un1[j] - un2[j];
			if ((un2[j] - un3[j] > -PI2) && (un2[j] - un3[j] < 0))
				wrapPhase23 = un2[j] - un3[j] + PI2;
			else
				wrapPhase23 = un2[j] - un3[j];
			if ((wrapPhase12 - wrapPhase23 > -PI2) && (wrapPhase12 - wrapPhase23 < 0))
				wrapPhase123 = wrapPhase12 - wrapPhase23 + PI2;
			else
				wrapPhase123 = wrapPhase12 - wrapPhase23;
			unPH12 = wrapPhase12 + PI2 * round((f12 / f123 * wrapPhase123 - wrapPhase12) / PI2);
			unPH23 = wrapPhase23 + PI2 * round((f23 / f123 * wrapPhase123 - wrapPhase23) / PI2);
			unPH1 = un1[j] + PI2 * round((f1 / f12 * unPH12 - un1[j]) / PI2);
			unPH2 = un2[j] + PI2 * round((f2 / f12 * unPH12 - un2[j]) / PI2);
			unPH3 = un3[j] + PI2 * round((f3 / f23 * unPH23 - un3[j]) / PI2);
			ph[j] = f1 * ((f1 * unPH1 + f2 * unPH2 + f3 * unPH3) / (f1 * f1 + f2 * f2 + f3 * f3));
		}
	}
	return unwrapPhase;
}
//*****************根据展开的相位和高度关系进行标定******************//
//输入为20副对应高度和相位图
//输出为一个三通道的Mat矩阵，分别对应标定参数a、b、c
vector FringeStructuredLight::Calibration(vector heightPhase, vectorheight)
{
	vector resTemp;//结果输出矩阵
	//A=heigh*heightPhase^2,B=heigh*heightPhase,C=heigh,D=-heightPhase^2
	//声明向量存储20组不同的A、B、C、D的值
	vector A_Vector, B_Vector, C_Vector, D_Vector;
	for (int i = 0; i < heightPhase.size(); i++)
	{
		Mat tempA = height[i] * (heightPhase[i].mul(heightPhase[i]));
		A_Vector.push_back(tempA);
		Mat tempB = height[i] * heightPhase[i];
		B_Vector.push_back(tempB);
		Mat tempC = height[i] * (Mat::ones(heightPhase[i].rows, heightPhase[i].cols, CV_32FC1));
		C_Vector.push_back(tempC);
		D_Vector.push_back(-1 * (heightPhase[i].mul(heightPhase[i])));
	}
	//计算M、N、L和H矩阵
	Mat M1 = Mat::zeros(A_Vector[0].rows, A_Vector[0].cols, CV_32FC1);
	Mat N1 = M1.clone(), L1 = M1.clone(), H1 = M1.clone();
	Mat M2 = M1.clone(), N2 = M1.clone(), L2 = M1.clone(), H2 = M1.clone();
	Mat M3 = M1.clone(), N3 = M1.clone(), L3 = M1.clone(), H3 = M1.clone();

	for (int i = 0; i < A_Vector.size(); i++)
	{
		
		M1 += A_Vector[i].mul(A_Vector[i]);
		N1 += A_Vector[i].mul(B_Vector[i]);
		L1 += A_Vector[i].mul(C_Vector[i]);
		H1 += A_Vector[i].mul(D_Vector[i]);

		M2 += B_Vector[i].mul(A_Vector[i]);
		N2 += B_Vector[i].mul(B_Vector[i]);
		L2 += B_Vector[i].mul(C_Vector[i]);
		H2 += B_Vector[i].mul(D_Vector[i]);

		M3 += C_Vector[i].mul(A_Vector[i]);
		N3 += C_Vector[i].mul(B_Vector[i]);
		L3 += C_Vector[i].mul(C_Vector[i]);
		H3 += C_Vector[i].mul(D_Vector[i]);
	}
	//开始遍历像素点的相位和对应的高度，求解参数a、b、c
	Mat a = Mat::zeros(M1.rows, M1.cols, CV_32FC1);
	Mat b = Mat::zeros(M1.rows, M1.cols, CV_32FC1);
	Mat c = Mat::zeros(M1.rows, M1.cols, CV_32FC1);

	for (int i = 0; i (i);
		float * N1_P = N1.ptr(i);
		float * L1_P = L1.ptr(i);
		float * H1_P = H1.ptr(i);

		float * M2_P = M2.ptr(i);
		float * N2_P = N2.ptr(i);
		float * L2_P = L2.ptr(i);
		float * H2_P = H2.ptr(i);

		float * M3_P = M3.ptr(i);
		float * N3_P = N3.ptr(i);
		float * L3_P = L3.ptr(i);
		float * H3_P = H3.ptr(i);

		float * a_P = a.ptr(i);
		float * b_P = b.ptr(i);
		float * c_P = c.ptr(i);

		for (int j = 0; j < M1.cols; j++)
		{
			Mat P = (Mat_(3, 3) << M1_P[j], N1_P[j], L1_P[j], M2_P[j], N2_P[j], L2_P[j], M3_P[j], N3_P[j], L3_P[j]);
			Mat Q = (Mat_(3, 1) << H1_P[j], H2_P[j], H3_P[j]);
			Mat X = P.inv()*Q;
			a_P[j] = X.at(0, 0);
			b_P[j] = X.at(1, 0);
			c_P[j] = X.at(2, 0);
		}
	}
	resTemp.push_back(a);
	resTemp.push_back(b);
	resTemp.push_back(c);
	return resTemp;
}
//*****************根据标定的abc参数进行三维重建******************//
//输入为模组的相位图和标定参数的abc矩阵
//输出为模组对应的点云
Mat FringeStructuredLight::Reconstruct(vectorabc_Vec, Mat heightPhase)
{
	Mat resTemp = Mat::zeros(heightPhase.rows, heightPhase.cols, CV_32FC1);
	for (int i = 0; i < abc_Vec[0].rows; i++)
	{
		float * a_P = abc_Vec[0].ptr(i);
		float * b_P = abc_Vec[1].ptr(i);
		float * c_P = abc_Vec[2].ptr(i);
		float * phase_P = heightPhase.ptr(i);
		float * res_P = resTemp.ptr(i);
		for (int j = 0; j < heightPhase.cols; j++)
		{
			res_P[j] = phase_P[j] * phase_P[j] / (a_P[j] * phase_P[j] * phase_P[j] + b_P[j] * phase_P[j] + c_P[j]);
		}
	}
	return resTemp;
}