178 lines
4.4 KiB
C++
178 lines
4.4 KiB
C++
#include "calib.h"
|
|
#include "stdlib.h"
|
|
|
|
void calib(
|
|
std::vector<CalibData> &inCalibData,
|
|
std::vector<RgnSubPixCalib> &outSubpixCalib
|
|
)
|
|
{
|
|
CalibData InData;
|
|
int readPtr = 0;
|
|
InData = inCalibData[readPtr ++];
|
|
uchar VSync = InData.Sync & 0x02; //ap_uint<1> VSync = InData.Sync.range(1,1);
|
|
if(0b0 == VSync) //wait for syncFirst
|
|
return;
|
|
|
|
Byte8 dataBuff;
|
|
dataBuff.dData = InData.calibK[0];
|
|
ushort WinNum = 0; // ap_uint<12> WinNum = 0;
|
|
RgnSubPixCalib outData = {0,0,0,0,0,0,0};
|
|
//1st: frameNo
|
|
outData.Sync = InData.Sync;
|
|
unsigned int data_32 = dataBuff.nData[0];
|
|
UintFloat tmp;
|
|
tmp.unData = (unsigned int)data_32;
|
|
outData.x = tmp.fdata;
|
|
outSubpixCalib.push_back(outData);
|
|
//2nd: timeStamp
|
|
InData = inCalibData[readPtr++];
|
|
outData.Sync = InData.Sync;
|
|
dataBuff.dData = InData.calibK[0];
|
|
data_32 = dataBuff.nData[0];
|
|
tmp.unData = (unsigned int)data_32;
|
|
outData.x = tmp.fdata;
|
|
outSubpixCalib.push_back(outData);
|
|
//3rd:encInfo
|
|
InData = inCalibData[readPtr++];
|
|
outData.Sync = InData.Sync;
|
|
dataBuff.dData = InData.calibK[0];
|
|
data_32 = dataBuff.nData[0];
|
|
tmp.unData = (unsigned int)data_32;
|
|
outData.x = tmp.fdata;
|
|
outSubpixCalib.push_back(outData);
|
|
//4th: frmW, frmH
|
|
InData = inCalibData[readPtr++];
|
|
outData.Sync = InData.Sync;
|
|
dataBuff.dData = InData.calibK[0];
|
|
data_32 = dataBuff.nData[0];
|
|
tmp.unData = (unsigned int)data_32;
|
|
outData.x = tmp.fdata;
|
|
outSubpixCalib.push_back(outData);
|
|
//5th: frmX, WinNum
|
|
InData = inCalibData[readPtr++];
|
|
outData.Sync = InData.Sync;
|
|
dataBuff.dData = InData.calibK[0];
|
|
data_32 = dataBuff.nData[0];
|
|
tmp.unData = (unsigned int)data_32;
|
|
outData.x = tmp.fdata;
|
|
uint_32 d32_tmp = data_32;
|
|
WinNum = (d32_tmp >> 12) & 0x7ff;
|
|
outSubpixCalib.push_back(outData);
|
|
//6th: frmY
|
|
InData = inCalibData[readPtr++];
|
|
outData.Sync = InData.Sync;
|
|
dataBuff.dData = InData.calibK[0];
|
|
data_32 = dataBuff.nData[0];
|
|
tmp.unData = (unsigned int)data_32;
|
|
outData.x = tmp.fdata;
|
|
outSubpixCalib.push_back(outData);
|
|
|
|
RgnSubPixCalib calibPnt;
|
|
double rawx,rawy;
|
|
double x2, x3,x4,x5,x6,x7;
|
|
double y2, y3,y4,y5,y6,y7;
|
|
double kx, ky;
|
|
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;
|
|
for(int n = 0; n < WinNum; n ++)
|
|
{
|
|
for(int j = 0; j <= 8; j ++)
|
|
{
|
|
InData = inCalibData[readPtr++];
|
|
if( 0 == j)//read subpix[readPtr++]
|
|
{
|
|
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)
|
|
outSubpixCalib.push_back(calibPnt);
|
|
|
|
dataBuff.dData = InData.calibK[0];
|
|
rawx = (double)dataBuff.fData[0];
|
|
uint_32 apData32 = (uint_32)dataBuff.nData[1];
|
|
ushort dy_12;
|
|
dy_12= apData32 & 0xfff;
|
|
rawy = (double)dy_12;
|
|
apData32 >>= 12;
|
|
calibPnt.rid = apData32 & 0x7ff;
|
|
apData32 >>= 11;
|
|
calibPnt.flag = apData32 & 0x0f;
|
|
dataBuff.dData = InData.calibK[1];
|
|
apData32 = (uint_32)dataBuff.nData[0];
|
|
calibPnt.value = apData32 && 0xff;
|
|
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;
|
|
x2 = rawx * rawx;
|
|
y2 = rawy * rawy;
|
|
}
|
|
else if(3 == j) //k2 * x^2
|
|
{
|
|
kx = InData.calibK[0];
|
|
ky = InData.calibK[1];
|
|
xmul2 = kx * x2;
|
|
ymul2 = ky * y2;
|
|
x3 = x2 * rawx;
|
|
y3 = y2 * rawy;
|
|
}
|
|
else if(4 == j)//k3 * x^3
|
|
{
|
|
kx = InData.calibK[0];
|
|
ky = InData.calibK[1];
|
|
xmul3 = kx * x3;
|
|
ymul3 = ky * y3;
|
|
x4 = x3 * rawx;
|
|
y4 = y3 * rawy;
|
|
}
|
|
else if(5 == j) //k4 * x^4
|
|
{
|
|
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
|
|
{
|
|
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
|
|
{
|
|
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
|
|
{
|
|
kx = InData.calibK[0];
|
|
ky = InData.calibK[1];
|
|
xmul7 = kx * x7;
|
|
ymul7 = ky * y7;
|
|
}
|
|
}
|
|
}
|
|
if(WinNum > 0)
|
|
outSubpixCalib.push_back(calibPnt);
|
|
}
|