IP_release_v13

Calib/sourceCode/Calib.cpp

@@ -52,36 +52,50 @@ void calib(
 	tmp.unData = (unsigned int)data_32;
 	outData.x = tmp.fdata;
 	outSubpixCalib.write(outData);
+
 	//5th: frmX, WinNum
 	InData = inCalibData.read();
 	outData.Sync = InData.Sync;
 	dataBuff.dData = InData.calibK[0];
 	data_32 = dataBuff.nData[0];
-	tmp.unData = (unsigned int)data_32;
+	ap_uint<32> d32_t5h = data_32;
-	outData.x = tmp.fdata;
+	WinNum(11,0) = d32_t5h.range(23,12);
-	ap_uint<32> d32_tmp = data_32;
+	//outSubpixCalib.write(outData);
-	WinNum(11,0) = d32_tmp.range(23,12);
+
-	outSubpixCalib.write(outData);
 	//6th: frmY
+	ap_uint<12> validDataSize = 0;
+	RgnSubPixCalib outData_1 = {0,0,0,0,0,0,0};
 	InData = inCalibData.read();
-	outData.Sync = InData.Sync;
+	outData_1.Sync = InData.Sync;
 	dataBuff.dData = InData.calibK[0];
 	data_32 = dataBuff.nData[0];
+	ap_uint<32> d32_tmp = data_32;
+	validDataSize(11,0) = d32_tmp.range(23,12);
+	d32_tmp(31,12) = 0;
 	tmp.unData = (unsigned int)data_32;
+	outData_1.x = tmp.fdata;
+
+	//output validDataSize
+	d32_t5h(23,12) = validDataSize.range(11,0);
+	tmp.unData = (unsigned int)d32_t5h;
 	outData.x = tmp.fdata;
 	outSubpixCalib.write(outData);
+	outSubpixCalib.write(outData_1);
 
 	RgnSubPixCalib calibPnt;
 	double rawx,rawy;
 	double x2, x3,x4,x5,x6,x7;
 	double y2, y3,y4,y5,y6,y7;
-	double kx, ky;
+	//double kx, ky;
+	double kx_1, kx_2, ky_1, ky_2;
+	Byte8 dataConver_x, dataConver_y;
 	double xmul0=0, xmul1=0, xmul2=0, xmul3=0, xmul4=0, xmul5=0, xmul6=0, xmul7=0;
 	double ymul0=0, ymul1=0, ymul2=0, ymul3=0, ymul4=0, ymul5=0, ymul6=0, ymul7=0;
+	bool vldFlag = 0;
 	for(int n = 0; n < WinNum; n ++)
 	{
-#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
+#pragma HLS LOOP_TRIPCOUNT min=4096 max=4096
-		for(int j = 0; j <= 8; j ++)
+		for(int j = 0; j <= 4; j ++)
 		{
 #pragma HLS PIPELINE II=1
 			InData = inCalibData.read();
@@ -89,7 +103,7 @@ void calib(
 			{
 				calibPnt.x = (float)(xmul0 + xmul1 + xmul2 + xmul3 + xmul4 + xmul5+ xmul6 + xmul7);
 				calibPnt.y = (float)(ymul0 + ymul1 + ymul2 + ymul3 + ymul4 + ymul5+ ymul6 + ymul7);
-				if(n > 0)
+				if( (n > 0) && (vldFlag == 1))
 					outSubpixCalib.write(calibPnt);
 
 				dataBuff.dData = InData.calibK[0];
@@ -105,77 +119,115 @@ void calib(
 				calibPnt.value = apData32.range(7,0);
 				calibPnt.Sync = 0;
 				calibPnt.rsv = 0;
-			}
+				if(rawx < 0)
-			else if(1 == j)//k0
+					vldFlag = 0;
-			{
+				else
-				kx = InData.calibK[0];
+					vldFlag = 1;
-				ky = InData.calibK[1];
+
-				xmul0 = kx;
-				ymul0 = ky;
-			}
-			else if(2 == j)//k1 * x
-			{
-				kx = InData.calibK[0];
-				ky = InData.calibK[1];
-				xmul1 = kx * rawx;
-				ymul1 = ky * rawy;
 				x2 = rawx * rawx;
 				y2 = rawy * rawy;
 			}
-			else if(3 == j) //k2 * x^2
+			else if(1 == j)//k0, k1*x
 			{
-				kx = InData.calibK[0];
+				//kx = InData.calibK[0];
-				ky = InData.calibK[1];
+				//ky = InData.calibK[1];
-				xmul2 = kx * x2;
+				dataConver_x.dData = InData.calibK[0];
-				ymul2 = ky * y2;
+				kx_1 = (double)dataConver_x.fData[0];
+				kx_2 = (double)dataConver_x.fData[1];
+				dataConver_y.dData = InData.calibK[0];
+				ky_1 = (double)dataConver_y.fData[0];
+				ky_2 = (double)dataConver_y.fData[1];
+				xmul0 = kx_1;
+				ymul0 = ky_1;
+				xmul1 = kx_2 * rawx;
+				ymul1 = ky_2 * rawy;
 				x3 = x2 * rawx;
 				y3 = y2 * rawy;
 			}
-			else if(4 == j)//k3 * x^3
+			else if(2 == j)//k2 * x^2, k3 * x^3
 			{
-				kx = InData.calibK[0];
+				dataConver_x.dData = InData.calibK[0];
-				ky = InData.calibK[1];
+				kx_1 = (double)dataConver_x.fData[0];
-				xmul3 = kx * x3;
+				kx_2 = (double)dataConver_x.fData[1];
-				ymul3 = ky * y3;
+				dataConver_y.dData = InData.calibK[0];
+				ky_1 = (double)dataConver_y.fData[0];
+				ky_2 = (double)dataConver_y.fData[1];
+
+				xmul2 = kx_1 * x2;
+				ymul2 = ky_1 * y2;
+				xmul3 = kx_2 * x3;
+				ymul3 = ky_2 * y3;
+
 				x4 = x3 * rawx;
 				y4 = y3 * rawy;
+				x5 = x3 * x2;
+				y5 = y3 * x2;
 			}
-			else if(5 == j) //k4 * x^4
+			else if(3 == j) //k4 * x^4, k5 * x^5
+			{
+				dataConver_x.dData = InData.calibK[0];
+				kx_1 = (double)dataConver_x.fData[0];
+				kx_2 = (double)dataConver_x.fData[1];
+				dataConver_y.dData = InData.calibK[0];
+				ky_1 = (double)dataConver_y.fData[0];
+				ky_2 = (double)dataConver_y.fData[1];
+
+				xmul4 = kx_1 * x4;
+				ymul4 = ky_1 * y4;
+				xmul5 = kx_2 * x5;
+				ymul5 = ky_2 * y5;
+
+				x6 = x5 * rawx;
+				y6 = y5 * rawy;
+				x7 = x5 * x2;
+				y7 = y5 * x2;
+			}
+			else if(4 == j)//k6 * x^6, k7 * x^7
+			{
+				dataConver_x.dData = InData.calibK[0];
+				kx_1 = (double)dataConver_x.fData[0];
+				kx_2 = (double)dataConver_x.fData[1];
+				dataConver_y.dData = InData.calibK[0];
+				ky_1 = (double)dataConver_y.fData[0];
+				ky_2 = (double)dataConver_y.fData[1];
+
+				xmul6 = kx_1 * x6;
+				ymul6 = ky_1 * y6;
+				xmul7 = kx_2 * x7;
+				ymul7 = ky_2 * y7;
+			}
+#if 0
+			else if(5 == j) //
 			{
 				kx = InData.calibK[0];
 				ky = InData.calibK[1];
-				xmul4 = kx * x4;
+
-				ymul4 = ky * y4;
 				x5 = x4 * rawx;
 				y5 = y4 * rawy;
 			}
-			else if(6 == j) //k5 * x^5
+			else if(6 == j) //
 			{
 				kx = InData.calibK[0];
 				ky = InData.calibK[1];
-				xmul5 = kx * x5;
+
-				ymul5 = ky * y5;
+
-				x6 = x5 * rawx;
-				y6 = y5 * rawy;
 			}
-			else if(7 == j)//k6 * x^6
+			else if(7 == j)//
 			{
 				kx = InData.calibK[0];
 				ky = InData.calibK[1];
-				xmul6 = kx * x6;
+
-				ymul6 = ky * y6;
+
-				x7 = x6 * rawx;
-				y7 = y6 * rawy;
 			}
-			else if(8 == j)//k7 * x^7
+			else if(8 == j)//
 			{
 				kx = InData.calibK[0];
 				ky = InData.calibK[1];
-				xmul7 = kx * x7;
+
-				ymul7 = ky * y7;
+			}
+#endif
 		}
 	}
-	}
+	if( (WinNum > 0)&& (vldFlag == 1))
-	if(WinNum > 0)
 		outSubpixCalib.write(calibPnt);
 }

Calib/sourceCode/calib_tb.cpp

@@ -0,0 +1,277 @@
+
+#include <fstream>
+#include <cstring>
+#include <stdio.h>
+#include "ap_utils.h"
+#include <iostream>
+#include "calib.h"
+#include "..\..\globals\tbGlobals.h"
+
+std::vector<RgnSubPix> readSubpixDataFile(
+		const char* file,
+		uint16_t* validSize)
+{
+	std::ifstream inputFile(file);
+
+	std::string strLineTxt;
+	int size = 0;
+	std::vector<RgnSubPix> testData;
+	RgnSubPix a_line;
+	uint16_t validNum = 0;
+	while(getline(inputFile, strLineTxt))
+	{
+		if (strLineTxt.empty())
+			continue;
+
+		float x;
+		int y, Rid, Flag, pkValue;
+		sscanf_s(strLineTxt.c_str(), "%f %d %d %x %d", &x, &y, &Rid, &Flag, &pkValue);
+
+		a_line.x = (float)x;
+		a_line.y = (ap_uint<12>)y;
+		a_line.rid = (ap_uint<11>)Rid;
+		a_line.flag = (ap_uint<4>)Flag;
+		a_line.value = (ap_uint<8>)pkValue;
+		testData.push_back(a_line);
+		if(x >= 0)
+			validNum ++;
+	}
+	*validSize = validNum;
+	inputFile.close();
+	return testData;
+}
+
+void genCalibTestSream(
+		std::vector<RgnSubPix>& subpixData,
+		hls::stream<CalibData>& calibStream,
+		const uint32_t FrmNo,
+		const uint32_t timeStamp,
+		const uint32_t encInfo,
+		const uint16_t frameROI_w,
+		const uint16_t frameROI_h,
+		const uint16_t frameROI_x,
+		const uint16_t frameROI_y,
+		const uint16_t validDataSize)
+{
+	CalibData VSync;
+	Byte8 dataBuff;
+	//1st:FrmNo
+	dataBuff.nData[0] = FrmNo;
+	dataBuff.nData[1] = 0;
+	VSync.Sync = 0b10;
+	VSync.calibK[0] = dataBuff.dData;
+	VSync.calibK[1] = 0;
+	calibStream.write(VSync);
+
+	//2nd:FrmNo
+	dataBuff.nData[0] = timeStamp;
+	VSync.Sync = 0b00;
+	VSync.calibK[0] = dataBuff.dData;
+	VSync.calibK[1] = 0;
+	calibStream.write(VSync);
+	//3rd:encInfo
+	dataBuff.nData[0] = encInfo;
+	VSync.Sync = 0b00;
+	VSync.calibK[0] = dataBuff.dData;
+	VSync.calibK[1] = 0;
+	calibStream.write(VSync);
+	//4th:frmW(12bit), frmH(12bit)
+	ap_uint<16> data16_1, data16_2;
+	data16_1 = frameROI_w;
+	data16_2 = frameROI_h;
+	ap_uint<32> tmpData;
+	tmpData(11,0) = data16_1.range(11,0);
+	tmpData(23,12) = data16_2.range(11,0);
+	tmpData(31,24) = 0;
+	dataBuff.nData[0] = (uint32_t)tmpData;
+	VSync.calibK[0] = dataBuff.dData;
+	VSync.calibK[1] = 0;
+	VSync.Sync = 0b00;
+	calibStream.write(VSync);
+	//5th: frmX, WinNum
+	int winNum = (int)subpixData.size();
+	data16_1 = frameROI_x;
+	data16_2 = (uint16_t)winNum;
+	tmpData(11,0) = data16_1.range(11,0);
+	tmpData(23,12) = data16_2.range(11,0);
+	tmpData(31,24) = 0;
+	dataBuff.nData[0] = (uint32_t)tmpData;
+	VSync.calibK[0] = dataBuff.dData;
+	VSync.calibK[1] = 0;
+	VSync.Sync = 0b00;
+	calibStream.write(VSync);
+	//6th: frmY
+	data16_1 = frameROI_y;
+	data16_2 = validDataSize;
+	tmpData(11,0) = data16_1.range(11,0);
+	tmpData(23,12) = data16_2.range(11,0);
+	tmpData(31,24) = 0;
+	dataBuff.nData[0] = (uint32_t)tmpData;
+	VSync.calibK[0] = dataBuff.dData;
+	VSync.calibK[1] = 0;
+	VSync.Sync = 0b00;
+	calibStream.write(VSync);
+	//output data
+	for(int i = 0; i < winNum; i ++)
+	{
+		RgnSubPix a_line = subpixData[i];
+		CalibData conData;
+
+		dataBuff.fData[0] = a_line.x;
+		ap_uint<32> apData32;
+		apData32(11, 0) = a_line.y.range(11,0);
+		apData32(22, 12) = a_line.rid.range(10,0);
+		apData32(26,23) = a_line.flag(3,0);
+		apData32(31,27) = 0;
+		dataBuff.nData[1] = (uint32_t)apData32;
+		conData.calibK[0] = dataBuff.dData;
+		apData32(7,0) = a_line.value.range(7,0);
+		apData32(31,8) = 0;
+		dataBuff.nData[0] = (uint32_t)apData32;
+		dataBuff.nData[1] = 0;
+		conData.calibK[1] = dataBuff.dData;
+		conData.Sync = 0b01;
+		calibStream.write(conData);
+
+		for(int m = 0; m < 4; m ++)  //write k
+		{
+			conData.Sync = 0b00;
+			if(m == 0)
+			{
+				Byte8 dataConv;
+				dataConv.fData[0] = 0.0;
+				dataConv.fData[1] = 1.0;
+				conData.calibK[0] = dataConv.dData;
+				conData.calibK[1] = dataConv.dData;
+			}
+			else
+			{
+				Byte8 dataConv;
+				dataConv.fData[0] = 0.0;
+				dataConv.fData[1] = 0.0;
+				conData.calibK[0] = dataConv.dData;
+				conData.calibK[1] = dataConv.dData;
+			}
+			calibStream.write(conData);
+		}
+	}
+	return;
+}
+
+void convertSubpixCalibStreamToArray(
+		hls::stream<RgnSubPixCalib>& streamData,
+		std::vector<Luma_rgnSubpixCalib>& arrayData,
+		uint32_t* FrmNo,
+		uint32_t* timeStamp,
+		uint32_t* encInfo,
+		uint16_t* frameROI_w,
+		uint16_t* frameROI_h,
+		uint16_t* frameROI_x,
+		uint16_t* frameROI_y)
+{
+	//1st:FrmNo
+	RgnSubPixCalib VSync = streamData.read();
+	UintFloat tmp;
+	tmp.fdata = VSync.x;
+	*FrmNo = (uint32_t)tmp.unData;
+	//2nd:timeStamp
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*timeStamp = (uint32_t)tmp.unData;
+	//3rd:encInfo
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*encInfo = (uint32_t)tmp.unData;
+	//4th:frmW(12bit), frmH(12bit)
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*frameROI_w = tmp.unData & 0xfff;
+	*frameROI_h = (tmp.unData >> 12) & 0xfff;
+	//5th: frmX, WinNum
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*frameROI_x = tmp.unData & 0xfff;
+	uint16_t winNum = (tmp.unData >> 12) & 0xfff;
+	//6th: frmY
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*frameROI_y = tmp.unData & 0xfff;
+	//output data
+	for(int i = 0; i < winNum; i ++)
+	{
+		RgnSubPixCalib data_in = streamData.read();
+		Luma_rgnSubpixCalib a_pnt;
+		a_pnt.x = data_in.x;
+		a_pnt.y = data_in.y;
+		a_pnt.Rid = (uint16_t)data_in.rid;
+		a_pnt.Flag = (uint8_t)data_in.flag;
+		a_pnt.pkValue = (uint8_t)data_in.value;
+		arrayData.push_back(a_pnt);
+	}
+	return;
+}
+
+void writeSubpixCalibData(
+		const char* fileName,
+		std::vector<Luma_rgnSubpixCalib>& outData)
+{
+    std::ofstream sw(fileName);
+    int i_max = outData.size();
+    for (int i = 0; i < i_max; i++)
+    {
+        char data[250];
+        sprintf_s(data, "%.2f %.2f %04d %01x %02d",
+        		outData[i].x, outData[i].y, outData[i].Rid, outData[i].Flag, outData[i].pkValue);
+        sw << data << std::endl;
+    }
+    sw.close();
+}
+
+#define TST_GROUP	1
+
+int main()
+{
+	const char* InDataPath[TST_GROUP] = {
+			"E:\\CamTestData\\Subpix\\testSample\\testData.txt"
+	};
+	const char* OutDataPath[TST_GROUP] = {
+			"E:\\CamTestData\\Calib\\testSample\\testData.txt"
+	};
+
+	for(int n = 0; n < TST_GROUP; n ++)
+	{
+		int winSize = 0;
+		uint16_t vldSize = 0;
+		std::vector<RgnSubPix> testData = readSubpixDataFile(InDataPath[n], &vldSize);
+		//gen test stream
+		hls::stream<CalibData> inStream;
+		uint32_t FrmNo = 0x1;
+		uint32_t timeStamp = 0x2;
+		uint32_t encInfo = 0x3;
+		uint16_t frameROI_w = 0x100;
+		uint16_t frameROI_h = 2048;
+		uint16_t frameROI_x = 0;
+		uint16_t frameROI_y = 0;
+		genCalibTestSream(testData, inStream, FrmNo, timeStamp, encInfo, frameROI_w, frameROI_h, frameROI_x, frameROI_y, vldSize);
+
+		hls::stream<RgnSubPixCalib> outStream;
+		calib(inStream,outStream);
+
+		std::vector<Luma_rgnSubpixCalib> outData;
+		uint32_t outFrmNo, outTimeStamp, outEncInfo;
+		uint16_t outROI_w,outROI_h, outROI_x,outROI_y;
+		convertSubpixCalibStreamToArray(
+				outStream,
+				outData,
+				&outFrmNo,
+				&outTimeStamp,
+				&outEncInfo,
+				&outROI_w,
+				&outROI_h,
+				&outROI_x,
+				&outROI_y);
+
+		writeSubpixCalibData(OutDataPath[n], outData);
+	}
+
+}

compute3D/sourceCode/compute3D.cpp

@@ -42,9 +42,9 @@ void compute3D(
 	outPt3D.write(outData);
 	//4th: frmW, frmH
 	InData = inSubCalib.read();
-	//outData.Sync = InData.Sync;
+	outData.Sync = InData.Sync;
-	//outData.x = InData.x;
+	outData.x = InData.x;
-	//outPt3D.write(outData);
+	outPt3D.write(outData);
 	//5th: frmX, WinNum
 	InData = inSubCalib.read();
 	outData.Sync = InData.Sync;
@@ -53,13 +53,13 @@ void compute3D(
 	ap_uint<32> d32 = (ap_uint<32>)tmp.unData;
 	WinNum(11,0) = d32.range(23,12);
 	tmp.unData = (unsigned int)WinNum;
-	outData.x = tmp.fdata;
+	outData.x = InData.x;
 	outPt3D.write(outData);
 	//6th: frmY
 	InData = inSubCalib.read();
-	//outData.Sync = InData.Sync;
+	outData.Sync = InData.Sync;
-	//outData.x = InData.x;
+	outData.x = InData.x;
-	//outPt3D.write(outData);
+	outPt3D.write(outData);
 
 	for(int n = 0; n < WinNum; n ++)
 	{

compute3D/sourceCode/compute3D_tb.cpp

@@ -0,0 +1,240 @@
+
+#include <fstream>
+#include <cstring>
+#include <stdio.h>
+#include "ap_utils.h"
+#include <iostream>
+#include "compute3D.h"
+#include "..\..\globals\tbGlobals.h"
+
+std::vector<RgnSubPixCalib> readSubpixCalibDataFile(
+		const char* file)
+{
+	std::ifstream inputFile(file);
+	std::string strLineTxt;
+	int size = 0;
+	std::vector<RgnSubPixCalib> testData;
+	RgnSubPixCalib a_line;
+	while(getline(inputFile, strLineTxt))
+	{
+		if (strLineTxt.empty())
+			continue;
+
+		float x,y;
+		int Rid, Flag, pkValue;
+		sscanf_s(strLineTxt.c_str(), "%f %f %d %x %d", &x, &y, &Rid, &Flag, &pkValue);
+
+		a_line.x = (float)x;
+		a_line.y = (float)y;
+		a_line.rid = (ap_uint<11>)Rid;
+		a_line.flag = (ap_uint<4>)Flag;
+		a_line.value = (ap_uint<8>)pkValue;
+		testData.push_back(a_line);
+	}
+	inputFile.close();
+	return testData;
+}
+
+void genCompute3DTestSream(
+		std::vector<RgnSubPixCalib>& subpixCalibData,
+		hls::stream<RgnSubPixCalib>& conpute3DStream,
+		const uint32_t FrmNo,
+		const uint32_t timeStamp,
+		const uint32_t encInfo,
+		const uint16_t frameROI_w,
+		const uint16_t frameROI_h,
+		const uint16_t frameROI_x,
+		const uint16_t frameROI_y)
+{
+	RgnSubPixCalib VSync;
+	UintFloat dataBuff;
+	//1st:FrmNo
+	dataBuff.unData = FrmNo;
+	VSync.Sync = 0b10;
+	VSync.x = dataBuff.fdata;
+	VSync.y = 0;
+	VSync.rid = 0;
+	VSync.flag = 0;
+	VSync.value = 0;
+	VSync.rsv = 0;
+	conpute3DStream.write(VSync);
+
+	//2nd:FrmNo
+	dataBuff.unData = timeStamp;
+	VSync.Sync = 0b00;
+	VSync.x = dataBuff.fdata;
+	conpute3DStream.write(VSync);
+	//3rd:encInfo
+	dataBuff.unData = encInfo;
+	VSync.Sync = 0b00;
+	VSync.x = dataBuff.fdata;
+	conpute3DStream.write(VSync);
+	//4th:frmW(12bit), frmH(12bit)
+	ap_uint<16> data16_1, data16_2;
+	data16_1 = frameROI_w;
+	data16_2 = frameROI_h;
+	ap_uint<32> tmpData;
+	tmpData(11,0) = data16_1.range(11,0);
+	tmpData(23,12) = data16_2.range(11,0);
+	tmpData(31,24) = 0;
+	dataBuff.unData = (uint32_t)tmpData;
+	VSync.x = dataBuff.fdata;
+	VSync.Sync = 0b00;
+	conpute3DStream.write(VSync);
+	//5th: frmX, WinNum
+	int winNum = (int)subpixCalibData.size();
+	data16_1 = frameROI_x;
+	data16_2 = (uint16_t)winNum;
+	tmpData(11,0) = data16_1.range(11,0);
+	tmpData(23,12) = data16_2.range(11,0);
+	tmpData(31,24) = 0;
+	dataBuff.unData = (uint32_t)tmpData;
+	VSync.x = dataBuff.fdata;
+	VSync.Sync = 0b00;
+	conpute3DStream.write(VSync);
+	//6th: frmY
+	data16_1 = frameROI_y;
+	data16_2 = 0;
+	tmpData(11,0) = data16_1.range(11,0);
+	tmpData(23,12) = data16_2.range(11,0);
+	tmpData(31,24) = 0;
+	dataBuff.unData = (uint32_t)tmpData;
+	VSync.x = dataBuff.fdata;
+	VSync.Sync = 0b00;
+	conpute3DStream.write(VSync);
+	//output data
+	for(int i = 0; i < winNum; i ++)
+	{
+		RgnSubPixCalib a_line = subpixCalibData[i];
+		if(i == 0)
+			a_line.Sync = 0b01;
+		else
+			a_line.Sync = 0b00;
+		a_line.rsv = 0;
+		conpute3DStream.write(a_line);
+	}
+	return;
+}
+
+void convertSubpixCalibStreamToArray(
+		hls::stream<Pnt3D>& streamData,
+		std::vector<Pnt3D>& arrayData,
+		uint32_t* FrmNo,
+		uint32_t* timeStamp,
+		uint32_t* encInfo,
+		uint16_t* frameROI_w,
+		uint16_t* frameROI_h,
+		uint16_t* frameROI_x,
+		uint16_t* frameROI_y)
+{
+	//1st:FrmNo
+	Pnt3D VSync = streamData.read();
+	UintFloat tmp;
+	tmp.fdata = VSync.x;
+	*FrmNo = (uint32_t)tmp.unData;
+	//2nd:timeStamp
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*timeStamp = (uint32_t)tmp.unData;
+	//3rd:encInfo
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*encInfo = (uint32_t)tmp.unData;
+	//4th:frmW(12bit), frmH(12bit)
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*frameROI_w = tmp.unData & 0xfff;
+	*frameROI_h = (tmp.unData >> 12) & 0xfff;
+	//5th: frmX, WinNum
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*frameROI_x = tmp.unData & 0xfff;
+	uint16_t winNum = (tmp.unData >> 12) & 0xfff;
+	//6th: frmY
+	VSync = streamData.read();
+	tmp.fdata = VSync.x;
+	*frameROI_y = tmp.unData & 0xfff;
+	//output data
+	for(int i = 0; i < winNum; i ++)
+	{
+		Pnt3D data_in = streamData.read();
+		Pnt3D a_pnt;
+		a_pnt.x = data_in.x;
+		a_pnt.y = data_in.y;
+		a_pnt.z = data_in.z;
+		a_pnt.value = (ap_uint<8>)data_in.value;
+		a_pnt.Sync = 0;
+		arrayData.push_back(a_pnt);
+	}
+	return;
+}
+
+void writeCompute3DData(
+		const char* fileName,
+		std::vector<Pnt3D>& outData)
+{
+    std::ofstream sw(fileName);
+    int i_max = outData.size();
+    for (int i = 0; i < i_max; i++)
+    {
+        char data[250];
+        uint8_t value = (uint8_t)outData[i].value;
+        sprintf_s(data, "%.3f %.3f %.3f %02d",
+        		outData[i].x, outData[i].y, outData[i].z, value);
+        sw << data << std::endl;
+    }
+    sw.close();
+}
+
+#define TST_GROUP	1
+
+int main()
+{
+	const char* InDataPath[TST_GROUP] = {
+			"E:\\CamTestData\\Calib\\testSample\\testData.txt"
+	};
+	const char* OutDataPath[TST_GROUP] = {
+			"E:\\CamTestData\\Compute3D\\testSample\\testData.txt"
+	};
+
+	for(int n = 0; n < TST_GROUP; n ++)
+	{
+		std::vector<RgnSubPixCalib> testData = readSubpixCalibDataFile(InDataPath[n]);
+		//gen test stream
+		hls::stream<RgnSubPixCalib> inStream;
+		uint32_t FrmNo = 0x1;
+		uint32_t timeStamp = 0x2;
+		uint32_t encInfo = 0x3;
+		uint16_t frameROI_w = 0x100;
+		uint16_t frameROI_h = 2048;
+		uint16_t frameROI_x = 0;
+		uint16_t frameROI_y = 0;
+		genCompute3DTestSream(testData, inStream, FrmNo, timeStamp, encInfo, frameROI_w, frameROI_h, frameROI_x, frameROI_y);
+
+		double u0 = 620.1;
+		double v0 = 1023.51;
+		double F_inv = 0.0009933051235;
+		double plane_a = -2.67733;
+		double plane_b = 0.0501805;
+		double plane_c = 631.944;
+		hls::stream<Pnt3D> outStream;
+		compute3D(inStream, outStream, u0, v0, F_inv, plane_a, plane_b, plane_c );
+
+		//output
+		std::vector<Pnt3D> outData;
+		uint32_t outFrmNo, outTimeStamp, outEncInfo;
+		uint16_t outROI_w,outROI_h, outROI_x,outROI_y;
+		convertSubpixCalibStreamToArray(
+				outStream,
+				outData,
+				&outFrmNo,
+				&outTimeStamp,
+				&outEncInfo,
+				&outROI_w,
+				&outROI_h,
+				&outROI_x,
+				&outROI_y);
+		writeCompute3DData(OutDataPath[n], outData);
+	}
+
+}

globals/algoGlobals.h

@@ -95,7 +95,7 @@ union Byte8
 	//SubpixInfo subpix;
 	unsigned int nData[2];
 	float fData[2];
-	double dData;;
+	double dData;
 };
 struct CalibData
 {

globals/tbGlobals.h

@@ -41,6 +41,16 @@ typedef struct
 	uint8_t pkValue;
 }Luma_rgnSubpix;
 
+
+typedef struct
+{
+	float x;
+	float y; //y坐标
+	uint16_t Rid;  //region的ID
+	uint8_t Flag;  //bit0是overlap标志。bit1是反光信号标
+	uint8_t pkValue;
+}Luma_rgnSubpixCalib;
+
 #pragma pack(push, 1)
 typedef struct {
     uint16_t file_type;      // 文件类型，'BM'

peakCentering/sourceCode/peakCentering_tb.cpp

@@ -300,6 +300,8 @@ void writepeakCenteringData(
     sw.close();
 }
 
+
+
 #define TST_GROUP	1
 
 int main()

subPix/sourceCode/subPix.cpp

@@ -3,7 +3,7 @@
 
 void subpix(
 	hls::stream<RgnPixConvolve> 	&InConvolvePnt,
-	hls::stream<RgnSubPix> 			&OutSubpixPnt
+	hls::stream<RgnSubPix> 			&OutSubpixPnt,
 )
 {
 #pragma HLS DATA_PACK variable=InConvolvePnt
@@ -12,6 +12,8 @@ void subpix(
 #pragma HLS INTERFACE axis register both port=InConvolvePnt
 
 	ap_uint<12> WinNum = 0;
+	ap_uint<12> frmH = 0;
+	ap_uint<12> frmY = 0;
 	RgnPixConvolve 	InData;
 	InData = InConvolvePnt.read();
 	ap_uint<1> VSync = InData.Sync.range(1,1);
@@ -19,6 +21,7 @@ void subpix(
 		return;
 
 	RgnSubPix outData = {0,0,0,0,0,0};
+	RgnSubPix nullOut = {-1.0f,0,0,0,0,0};
 	//1st: frameNo
 	outData.Sync = InData.Sync;
 	outData.rsv = 0;
@@ -54,53 +57,90 @@ void subpix(
 	data_32(11,0) = InData.LazerWinX.range(11,0);
 	data_32(23,12) = InData.LazerWinY.range(11,0);
 	data_32(31,24) = 0;
+	frmH(11,0) = InData.LazerWinY.range(11,0);
 	tmp.unData = (unsigned int)data_32;
 	outData.x = tmp.fdata;
 	OutSubpixPnt.write(outData);
 	//5th: frmX, WinNum
+	int nLines = (int)frmH;
+	int nLoops = nLines * 2;
+	if(nLoops >= 4096)
+		nLoops = 4095;
+	ap_uint<12> outSize = (ap_uint<12>)nLoops;
 	InData = InConvolvePnt.read();
 	outData.Sync = InData.Sync;
 	data_32(11,0) = InData.LazerWinX.range(11,0);
-	data_32(23,12) = InData.LazerWinY.range(11,0);
+	data_32(23,12) = outSize.range(11,0);	//InData.LazerWinY.range(11,0);
 	data_32(31,24) = 0;
 	tmp.unData = (unsigned int)data_32;
 	outData.x = tmp.fdata;
 	WinNum(11,0) = InData.LazerWinY.range(11,0);
 	OutSubpixPnt.write(outData);
-	//6th: frmY
+	//6th: frmY, validDataSize
 	InData = InConvolvePnt.read();
 	outData.Sync = InData.Sync;
 	data_32(11,0) = InData.LazerWinX.range(11,0);
-	data_32(23,12) = InData.LazerWinY.range(11,0);
+	data_32(23,12) = WinNum.range(11,0);  //validDataSize
 	data_32(31,24) = 0;
+	frmY(11,0) = InData.LazerWinX.range(11,0);
 	tmp.unData = (unsigned int)data_32;
 	outData.x = tmp.fdata;
 	OutSubpixPnt.write(outData);
 
-	for(int n = 0; n < WinNum; n ++)
+	int nStart = (int)frmY;
-	{
+	ap_uint<12> lastLine = frmY + frmH - 1;
-#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
+	ap_uint<16> readNum = 0;
-#pragma HLS PIPELINE II=1
+	ap_uint<1> readFlag = 1;
 
 	InData = InConvolvePnt.read();
+	readNum ++;
+	RgnPixConvolve currData;
+	for(int n = 0; n < nLoops; n ++)
+	{
+#pragma HLS LOOP_TRIPCOUNT min=4096 max=4096
+#pragma HLS PIPELINE II=1
 
-		float fx;
+		int lineIndx = n / 2 + nStart;
-		if(InData.nD2 == 0)
+
-			fx = 0;
+		ap_uint<1> vldFlag = 0;
-		else
+		if( InData.LazerWinY <= lineIndx)
-			fx = (float)InData.nD1/(float)InData.nD2;
+		{
+			if(readNum <= WinNum)
+			{
+				currData = InData;
+				vldFlag = 1;
+				if(readNum < WinNum)
+				{
+					InData = InConvolvePnt.read();
+				}
+				readNum ++;
+			}
+		}
 
 		RgnSubPix a_pnt;
+		if(vldFlag == 0)
+		{
+			a_pnt = nullOut;
+		}
+		else
+		{
+			float fx;
+			if(currData.nD2 == 0)
+				fx = 0;
+			else
+				fx = (float)currData.nD1/(float)currData.nD2;
+
+			a_pnt.flag = currData.LazerWinFlag;
+			a_pnt.y = currData.LazerWinY;
+			a_pnt.rid = currData.LazerWinRid;
+			a_pnt.value = currData.value;
+			a_pnt.x = (float)currData.LazerWinX + 8.0f + fx + 0.5; //adjust point to pixel center
+			a_pnt.rsv = 0;
+		}
 		if(n == 0)
 			a_pnt.Sync = 0b01;
 		else
 			a_pnt.Sync = 0b00;
-		a_pnt.flag = InData.LazerWinFlag;
-		a_pnt.y = InData.LazerWinY;
-		a_pnt.rid = InData.LazerWinRid;
-		a_pnt.value = InData.value;
-		a_pnt.x = (float)InData.LazerWinX + 8.0f + fx + 0.5; //adjust point to pixel center
-		a_pnt.rsv = 0;
 		OutSubpixPnt.write(a_pnt);
 	}
 }