77 lines
1.8 KiB
C++
77 lines
1.8 KiB
C++
|
#include "compute3D.h"
|
||
|
|
#include "stdlib.h"
|
||
|
|
|
||
|
|
void compute3D(
|
||
|
|
std::vector<RgnSubPixCalib> &inSubCalib,
|
||
|
|
std::vector<Pnt3D> &outPt3D,
|
||
|
|
double u0,
|
||
|
|
double v0,
|
||
|
|
double F_inv,
|
||
|
|
double plane_a,
|
||
|
|
double plane_b,
|
||
|
|
double plane_c
|
||
|
|
)
|
||
|
|
{
|
||
|
|
RgnSubPixCalib InData;
|
||
|
|
int readPtr = 0;
|
||
|
|
InData = inSubCalib[readPtr++];
|
||
|
|
uchar VSync = InData.Sync &0x02;
|
||
|
|
if(0 == VSync) //wait for syncFirst
|
||
|
|
return;
|
||
|
|
|
||
|
|
ushort WinNum = 0;
|
||
|
|
Pnt3D outData = {0,0,0,0,0};
|
||
|
|
//1st: frameNo
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
outData.x = InData.x;
|
||
|
|
outPt3D.push_back(outData);
|
||
|
|
//2nd: timeStamp
|
||
|
|
InData = inSubCalib[readPtr++];
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
outData.x = InData.x;
|
||
|
|
outPt3D.push_back(outData);
|
||
|
|
//3rd:encInfo
|
||
|
|
InData = inSubCalib[readPtr++];
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
outData.x = InData.x;
|
||
|
|
outPt3D.push_back(outData);
|
||
|
|
//4th: frmW, frmH
|
||
|
|
InData = inSubCalib[readPtr++];
|
||
|
|
//outData.Sync = InData.Sync;
|
||
|
|
//outData.x = InData.x;
|
||
|
|
//outPt3D.write(outData);
|
||
|
|
//5th: frmX, WinNum
|
||
|
|
InData = inSubCalib[readPtr++];
|
||
|
|
outData.Sync = InData.Sync;
|
||
|
|
UintFloat tmp;
|
||
|
|
tmp.fdata = InData.x;
|
||
|
|
uint_32 d32 = (uint_32)tmp.unData;
|
||
|
|
WinNum = (d32 >> 12) & 0x7fff; // .range(23, 12);
|
||
|
|
tmp.unData = (unsigned int)WinNum;
|
||
|
|
outData.x = tmp.fdata;
|
||
|
|
outPt3D.push_back(outData);
|
||
|
|
//6th: frmY
|
||
|
|
InData = inSubCalib[readPtr++];
|
||
|
|
//outData.Sync = InData.Sync;
|
||
|
|
//outData.x = InData.x;
|
||
|
|
//outPt3D.write(outData);
|
||
|
|
|
||
|
|
for(int n = 0; n < WinNum; n ++)
|
||
|
|
{
|
||
|
|
InData = inSubCalib[readPtr++];
|
||
|
|
|
||
|
|
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.push_back(outData);
|
||
|
|
}
|
||
|
|
}
|