324 lines
8.4 KiB
C++
324 lines
8.4 KiB
C++
|
|
#include <fstream>
|
|
#include <cstring>
|
|
#include <stdio.h>
|
|
#include "ap_utils.h"
|
|
#include <iostream>
|
|
#include "compute3D.h"
|
|
#include "../../globals/tbGlobals.h"
|
|
|
|
Luma_frameInfo readParaFile(const char* file)
|
|
{
|
|
std::ifstream inputFile(file);
|
|
Luma_frameInfo frameInfo;
|
|
std::string strLineTxt;
|
|
|
|
int data;
|
|
getline(inputFile, strLineTxt); //XL
|
|
sscanf(strLineTxt.c_str(), "XL:%d", &data);
|
|
frameInfo.L = (uint16_t)data;
|
|
|
|
getline(inputFile, strLineTxt); //XR
|
|
|
|
getline(inputFile, strLineTxt); //TL
|
|
sscanf(strLineTxt.c_str(), "TL:%d", &data);
|
|
frameInfo.T = (uint16_t)data;
|
|
|
|
getline(inputFile, strLineTxt); //TR
|
|
|
|
getline(inputFile, strLineTxt); //W
|
|
sscanf(strLineTxt.c_str(), "W:%d", &data);
|
|
frameInfo.W = (uint16_t)data;
|
|
|
|
getline(inputFile, strLineTxt); //H
|
|
sscanf(strLineTxt.c_str(), "H:%d", &data);
|
|
frameInfo.H = (uint16_t)data;
|
|
|
|
getline(inputFile, strLineTxt); //FrmNO
|
|
sscanf(strLineTxt.c_str(), "FrmNO:%x", &data);
|
|
frameInfo.FrmNo = (uint32_t)data;
|
|
|
|
getline(inputFile, strLineTxt); //TimStp
|
|
sscanf(strLineTxt.c_str(), "TimStp:%x", &data);
|
|
frameInfo.TimeStamp = (uint32_t)data;
|
|
|
|
getline(inputFile, strLineTxt); //EncInfo
|
|
sscanf(strLineTxt.c_str(), "EncInfo:%x", &data);
|
|
frameInfo.encInfo = (uint32_t)data;
|
|
|
|
inputFile.close();
|
|
return frameInfo;
|
|
}
|
|
|
|
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, Sync;
|
|
#if defined(__linux__)
|
|
sscanf(strLineTxt.c_str(), "%f %f %d %x %d %x", &x, &y, &Rid, &Flag, &pkValue, &Sync);
|
|
#else
|
|
sscanf_s(strLineTxt.c_str(), "%f %f %d %x %d %x", &x, &y, &Rid, &Flag, &pkValue, &Sync);
|
|
#endif
|
|
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;
|
|
a_line.Sync = (ap_uint<2>)Sync;
|
|
testData.push_back(a_line);
|
|
}
|
|
inputFile.close();
|
|
return testData;
|
|
}
|
|
|
|
void genCompute3DTestSream(
|
|
std::vector<RgnSubPixCalib>& subpixCalibData,
|
|
hls::stream<RgnSubPixCalib>& conpute3DStream,
|
|
const Luma_frameInfo framwInfo)
|
|
{
|
|
RgnSubPixCalib VSync;
|
|
UintFloat dataBuff;
|
|
//1st:FrmNo
|
|
dataBuff.unData = framwInfo.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 = framwInfo.TimeStamp;
|
|
VSync.Sync = 0b00;
|
|
VSync.x = dataBuff.fdata;
|
|
conpute3DStream.write(VSync);
|
|
//3rd:encInfo
|
|
dataBuff.unData = framwInfo.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 = framwInfo.W;
|
|
data16_2 = framwInfo.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 = framwInfo.L;
|
|
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 = framwInfo.T;
|
|
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];
|
|
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 = data_in.Sync;
|
|
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;
|
|
uint8_t sync = (uint8_t)outData[i].Sync;
|
|
#if defined(__linux__)
|
|
sprintf(data, "%.3f %.3f %.3f %02d %01x",
|
|
outData[i].x, outData[i].y, outData[i].z, value, sync);
|
|
#else
|
|
sprintf_s(data, "%.3f %.3f %.3f %02d %01x",
|
|
outData[i].x, outData[i].y, outData[i].z, value, sync);
|
|
#endif
|
|
sw << data << std::endl;
|
|
}
|
|
sw.close();
|
|
}
|
|
|
|
#define TST_GROUP 3
|
|
|
|
int main()
|
|
{
|
|
#if defined(__linux__)
|
|
const char* SrcDataPath[TST_GROUP] = {
|
|
"/home/zengHQ/CamTestData/testSample/PickRgn_C/PickRgn_Rslt/Txt/",
|
|
"/home/zengHQ/CamTestData/Grp_0/PickRgn_C/PickRgn_Rslt/Txt/",
|
|
"/home/zengHQ/CamTestData/Grp_1/PickRgn_C/PickRgn_Rslt/Txt/"
|
|
};
|
|
const char* InDataPath[TST_GROUP] = {
|
|
"/home/zengHQ/CamTestData/testSample/Calib/",
|
|
"/home/zengHQ/CamTestData/Grp_0/Calib/",
|
|
"/home/zengHQ/CamTestData/Grp_1/Calib/",
|
|
};
|
|
const char* OutDataPath[TST_GROUP] = {
|
|
"/home/zengHQ/CamTestData/testSample/Compute3D/",
|
|
"/home/zengHQ/CamTestData/Grp_0/Compute3D/",
|
|
"/home/zengHQ/CamTestData/Grp_1/Compute3D/",
|
|
};
|
|
#else
|
|
const char* SrcDataPath[TST_GROUP] = {
|
|
"E:/CamTestData/testSample/PickRgn_C/PickRgn_Rslt/Txt/",
|
|
"E:/CamTestData/Grp_0/PickRgn_C/PickRgn_Rslt/Txt/",
|
|
"E:/CamTestData/Grp_1/PickRgn_C/PickRgn_Rslt/Txt/"
|
|
};
|
|
const char* InDataPath[TST_GROUP] = {
|
|
"E:/CamTestData/testSample/Calib/",
|
|
"E:/CamTestData/Grp_0/Calib/",
|
|
"E:/CamTestData/Grp_1/Calib/",
|
|
};
|
|
const char* OutDataPath[TST_GROUP] = {
|
|
"E:/CamTestData/testSample/Compute3D/",
|
|
"E:/CamTestData/Grp_0/Compute3D/",
|
|
"E:/CamTestData/Grp_1/Compute3D/",
|
|
};
|
|
#endif
|
|
int testFileSize[TST_GROUP] = {1, 4, 4};
|
|
|
|
for(int n = 1; n < TST_GROUP; n ++)
|
|
{
|
|
int winSize = 0;
|
|
int fileSize = testFileSize[n];
|
|
for(int fi = 0; fi < fileSize; fi++)
|
|
{
|
|
char fileName[200];
|
|
char paraFileName[200];
|
|
char outFileName[200];
|
|
#if defined(__linux__)
|
|
sprintf(fileName, "%s%d_L_calib.txt", InDataPath[n], fi);
|
|
sprintf(paraFileName, "%s%d_para_verilog.txt", SrcDataPath[n], fi);
|
|
sprintf(outFileName, "%s%d_L_compute3d.txt", OutDataPath[n], fi);
|
|
#else
|
|
sprintf_s(fileName, "%s%d_L_calib.txt", InDataPath[n], fi);
|
|
sprintf_s(paraFileName, "%s%d_para_verilog.txt", SrcDataPath[n], fi);
|
|
sprintf_s(outFileName, "%s%d_L_compute3d.txt", OutDataPath[n], fi);
|
|
#endif
|
|
Luma_frameInfo framwInfo = readParaFile(paraFileName);
|
|
|
|
std::vector<RgnSubPixCalib> testData = readSubpixCalibDataFile(fileName);
|
|
//gen test stream
|
|
hls::stream<RgnSubPixCalib> inStream;
|
|
genCompute3DTestSream(testData, inStream, framwInfo);
|
|
|
|
float u0 = 630;
|
|
float v0 = 960;
|
|
float F_inv = 0.0009933051235;
|
|
float plane_a = -2.0;
|
|
float plane_b = 0.0501805;
|
|
float 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(outFileName, outData);
|
|
}
|
|
}
|
|
printf("done!\n");
|
|
}
|