IP_release_v13
This commit is contained in:
parent
87d6debb12
commit
ac2e58a7af
@ -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;
|
||||
outData.x = tmp.fdata;
|
||||
ap_uint<32> d32_tmp = data_32;
|
||||
WinNum(11,0) = d32_tmp.range(23,12);
|
||||
outSubpixCalib.write(outData);
|
||||
ap_uint<32> d32_t5h = data_32;
|
||||
WinNum(11,0) = d32_t5h.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
|
||||
for(int j = 0; j <= 8; j ++)
|
||||
#pragma HLS LOOP_TRIPCOUNT min=4096 max=4096
|
||||
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;
|
||||
}
|
||||
else if(1 == j)//k0
|
||||
{
|
||||
kx = InData.calibK[0];
|
||||
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;
|
||||
if(rawx < 0)
|
||||
vldFlag = 0;
|
||||
else
|
||||
vldFlag = 1;
|
||||
|
||||
x2 = rawx * rawx;
|
||||
y2 = rawy * rawy;
|
||||
}
|
||||
else if(3 == j) //k2 * x^2
|
||||
else if(1 == j)//k0, k1*x
|
||||
{
|
||||
kx = InData.calibK[0];
|
||||
ky = InData.calibK[1];
|
||||
xmul2 = kx * x2;
|
||||
ymul2 = ky * y2;
|
||||
//kx = InData.calibK[0];
|
||||
//ky = InData.calibK[1];
|
||||
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];
|
||||
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];
|
||||
ky = InData.calibK[1];
|
||||
xmul3 = kx * x3;
|
||||
ymul3 = ky * y3;
|
||||
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];
|
||||
|
||||
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)
|
||||
if( (WinNum > 0)&& (vldFlag == 1))
|
||||
outSubpixCalib.write(calibPnt);
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
@ -42,9 +42,9 @@ void compute3D(
|
||||
outPt3D.write(outData);
|
||||
//4th: frmW, frmH
|
||||
InData = inSubCalib.read();
|
||||
//outData.Sync = InData.Sync;
|
||||
//outData.x = InData.x;
|
||||
//outPt3D.write(outData);
|
||||
outData.Sync = InData.Sync;
|
||||
outData.x = InData.x;
|
||||
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.x = InData.x;
|
||||
//outPt3D.write(outData);
|
||||
outData.Sync = InData.Sync;
|
||||
outData.x = InData.x;
|
||||
outPt3D.write(outData);
|
||||
|
||||
for(int n = 0; n < WinNum; n ++)
|
||||
{
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
@ -95,7 +95,7 @@ union Byte8
|
||||
//SubpixInfo subpix;
|
||||
unsigned int nData[2];
|
||||
float fData[2];
|
||||
double dData;;
|
||||
double dData;
|
||||
};
|
||||
struct CalibData
|
||||
{
|
||||
|
||||
@ -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'
|
||||
|
||||
@ -300,6 +300,8 @@ void writepeakCenteringData(
|
||||
sw.close();
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define TST_GROUP 1
|
||||
|
||||
int main()
|
||||
|
||||
@ -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 ++)
|
||||
{
|
||||
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
||||
#pragma HLS PIPELINE II=1
|
||||
int nStart = (int)frmY;
|
||||
ap_uint<12> lastLine = frmY + frmH - 1;
|
||||
ap_uint<16> readNum = 0;
|
||||
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;
|
||||
if(InData.nD2 == 0)
|
||||
fx = 0;
|
||||
else
|
||||
fx = (float)InData.nD1/(float)InData.nD2;
|
||||
int lineIndx = n / 2 + nStart;
|
||||
|
||||
ap_uint<1> vldFlag = 0;
|
||||
if( InData.LazerWinY <= lineIndx)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user