95 lines
2.2 KiB
C++
95 lines
2.2 KiB
C++
#include "compute3D.h"
|
|
#include "stdlib.h"
|
|
|
|
void compute3D(
|
|
hls::stream<RgnSubPixCalib> &inSubCalib,
|
|
hls::stream<Pnt3D> &outPt3D,
|
|
float u0_f,
|
|
float v0_f,
|
|
float F_inv_f,
|
|
float plane_a_f,
|
|
float plane_b_f,
|
|
float plane_c_f
|
|
)
|
|
{
|
|
#if !(VITIS_HLS)
|
|
#pragma HLS DATA_PACK variable=inSubCalib
|
|
#pragma HLS DATA_PACK variable=outPt3D
|
|
#endif
|
|
#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 = InData.x;
|
|
outPt3D.write(outData);
|
|
//6th: frmY
|
|
InData = inSubCalib.read();
|
|
outData.Sync = InData.Sync;
|
|
outData.x = InData.x;
|
|
outPt3D.write(outData);
|
|
|
|
double u0 = u0_f;
|
|
double v0 =v0_f;
|
|
double F_inv =F_inv_f;
|
|
double plane_a = plane_a_f;
|
|
double plane_b = plane_b_f;
|
|
double plane_c = plane_c_f;
|
|
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 = InData.Sync;
|
|
outPt3D.write(outData);
|
|
if(0b11 == InData.Sync)
|
|
break;
|
|
}
|
|
}
|