85 lines
2.0 KiB
C++
85 lines
2.0 KiB
C++
|
#include "compute3D.h"
|
||
|
|
#include "stdlib.h"
|
||
|
|
|
||
|
|
void compute3D(
|
||
|
|
hls::stream<RgnSubPixCalib> &inSubCalib,
|
||
|
|
hls::stream<Pnt3D> &outPt3D,
|
||
|
|
double u0,
|
||
|
|
double v0,
|
||
|
|
double F_inv,
|
||
|
|
double plane_a,
|
||
|
|
double plane_b,
|
||
|
|
double plane_c
|
||
|
|
)
|
||
|
|
{
|
||
|
|
#pragma HLS DATA_PACK variable=inSubCalib
|
||
|
|
#pragma HLS DATA_PACK variable=outPt3D
|
||
|
|
#pragma HLS INTERFACE axis register both port=inSubCalib
|
||
|
|
#pragma HLS INTERFACE axis register both port=outPt3D
|
||
|
|
|
||
|
|
|
||
|
|
RgnSubPixCalib InData;
|
||
|
|
InData = inSubCalib.read();
|
||
|
|
ap_uint<1> VSync = InData.Sync.range(1,1);
|
||
|
|
if(0b0 == VSync) //wait for syncFirst
|
||
|
|
return;
|
||
|
|
|
||
|
|
ap_uint<12> WinNum = 0;
|
||
|
|
Pnt3D outData = {0,0,0,0,0};
|
||
|
|
//1st: frameNo
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
outData.x = InData.x;
|
||
|
|
outPt3D.write(outData);
|
||
|
|
//2nd: timeStamp
|
||
|
|
InData = inSubCalib.read();
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
outData.x = InData.x;
|
||
|
|
outPt3D.write(outData);
|
||
|
|
//3rd:encInfo
|
||
|
|
InData = inSubCalib.read();
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
outData.x = InData.x;
|
||
|
|
outPt3D.write(outData);
|
||
|
|
//4th: frmW, frmH
|
||
|
|
InData = inSubCalib.read();
|
||
|
|
//outData.Sync = InData.Sync;
|
||
|
|
//outData.x = InData.x;
|
||
|
|
//outPt3D.write(outData);
|
||
|
|
//5th: frmX, WinNum
|
||
|
|
InData = inSubCalib.read();
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
UintFloat tmp;
|
||
|
|
tmp.fdata = InData.x;
|
||
|
|
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;
|
||
|
|
outPt3D.write(outData);
|
||
|
|
//6th: frmY
|
||
|
|
InData = inSubCalib.read();
|
||
|
|
//outData.Sync = InData.Sync;
|
||
|
|
//outData.x = InData.x;
|
||
|
|
//outPt3D.write(outData);
|
||
|
|
|
||
|
|
for(int n = 0; n < WinNum; n ++)
|
||
|
|
{
|
||
|
|
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
||
|
|
#pragma HLS PIPELINE II=1
|
||
|
|
|
||
|
|
InData = inSubCalib.read();
|
||
|
|
|
||
|
|
double u = (double)InData.x;
|
||
|
|
double v = (double)InData.y;
|
||
|
|
double z_inv = u * plane_a + v * plane_b + plane_c;
|
||
|
|
double z = 1/ z_inv;
|
||
|
|
double x = (u - u0) * z * F_inv;
|
||
|
|
double y = (v - v0) * z * F_inv;
|
||
|
|
outData.x = (float)x;
|
||
|
|
outData.y = (float)y;
|
||
|
|
outData.z = (float)z;
|
||
|
|
outData.value = InData.value;
|
||
|
|
outData.Sync = 0;
|
||
|
|
outPt3D.write(outData);
|
||
|
|
}
|
||
|
|
}
|