373 lines
10 KiB
C++
373 lines
10 KiB
C++
///流程
|
||
/// CG-Frontend -> pkCentering -> Convolve -> Subpix -> Calib -> Compute3D
|
||
#include <vector>
|
||
#include "algoGlobals.h"
|
||
#include "PickLazerWin.h"
|
||
#include "peakCentering.h"
|
||
#include "convolve.h"
|
||
#include "subPix.h"
|
||
#include "calib.h"
|
||
#include "compute3D.h"
|
||
#include "camAlgoSW_Export.h"
|
||
|
||
typedef struct
|
||
{
|
||
uint16_t WinRdx;//窗口在图像内的起始坐标
|
||
uint16_t y; //y坐标
|
||
uint16_t Rid; //region的ID
|
||
uint8_t Flag; //bit0是overlap标志。bit1是反光信号标志
|
||
uint8_t PeakRltvRdx; //peak在窗口内的的相对坐标，低4位是起始坐标，高4位是结束坐标
|
||
uint8_t data[RGN_DATA_WIN_SIZE];
|
||
//double offset;
|
||
}Luma_rgnData;
|
||
|
||
#define SV_WIN_SIZE 16
|
||
#define ALIGN_RDX ((LAZER_WIN_SIZE-SV_WIN_SIZE)/2)
|
||
std::vector<Luma_rgnData> SvPickRgnRslt(
|
||
const unsigned char* SrcPixel,
|
||
int FrmH,
|
||
int FrmW,
|
||
PickLaserResult& PickLaserResult_inst
|
||
)
|
||
{
|
||
int LazerWinNum = 0;
|
||
int winW = 0x800 | SV_WIN_SIZE;
|
||
std::vector<Luma_rgnData> testData;
|
||
for (unsigned short y = 0; y < FrmH; y++)
|
||
{
|
||
int WinNumPerLine = 0;
|
||
for (int PickId = 0; PickId < PICK_RGN_MAX; PickId++)
|
||
{
|
||
if (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_PickMsk[y][PickId] > 0)
|
||
{
|
||
unsigned short WinRdx = PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_WinRdx[y][PickId] + ALIGN_RDX;//PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerRdx[y] - ((MEANSTG_WIN_SIZE/2) - (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerW[y]/2));
|
||
unsigned short WinRltvLazerRdxStr = PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerRdx[y][PickId] - WinRdx;
|
||
unsigned short WinRltvLazerRdxEnd = PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerRdx[y][PickId] + PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LazerW[y][PickId] - 1 - WinRdx;
|
||
|
||
unsigned char JointWinRltvRdx = (unsigned char)(((WinRltvLazerRdxEnd & 0xf) << 4) | (WinRltvLazerRdxStr & 0xf));
|
||
unsigned short RealRflctMsk = (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_RflctMsk[y][PickId] & (~PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LiteRflct[y][PickId]));
|
||
unsigned short Flag = ((RealRflctMsk << 1) | PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_OverlapMsk[y][PickId]);
|
||
// snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x", WinRdx, y, PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_Rid[y][PickId], Flag);
|
||
|
||
Luma_rgnData a_line;
|
||
a_line.WinRdx = (uint16_t)WinRdx;
|
||
a_line.y = (uint16_t)y;
|
||
a_line.Rid = (uint16_t)PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_Rid[y][PickId];
|
||
a_line.Flag = (uint8_t)Flag;
|
||
a_line.PeakRltvRdx = (uint8_t)JointWinRltvRdx;
|
||
for (int n = 0; n < RGN_DATA_WIN_SIZE; n++)
|
||
a_line.data[n] = 0;
|
||
//snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", WinRdx, y, PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_Rid[y][PickId], Flag, JointWinRltvRdx);
|
||
//TXTFile << TXTData << std::endl;
|
||
for (short WinPixCnt = 0; WinPixCnt < SV_WIN_SIZE; WinPixCnt++)//MEANSTG_WIN_SIZE; WinPixCnt++)
|
||
{
|
||
int PixIdx = y * FrmW + WinRdx + WinPixCnt;
|
||
unsigned char PixVlu = 0;//SrcPixel[PixIdx];
|
||
if (PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_RflctMsk[y][PickId] == 1 && PickLaserResult_inst.PickRgn_FrmMaxDeltaStg_LiteRflct[y][PickId] == 0)
|
||
{
|
||
if ((WinPixCnt >= (SV_WIN_SIZE / 2 - 1)) && (WinPixCnt <= (SV_WIN_SIZE / 2)))
|
||
{
|
||
PixVlu = 255;
|
||
}
|
||
else
|
||
{
|
||
PixVlu = 0;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
PixVlu = SrcPixel[PixIdx];
|
||
}
|
||
a_line.data[WinPixCnt] = (uint8_t)PixVlu;
|
||
//snprintf(TXTData, sizeof(TXTData), "%02x", PixVlu);
|
||
//TXTFile << TXTData << std::endl;
|
||
}
|
||
testData.push_back(a_line);
|
||
WinNumPerLine++;
|
||
}
|
||
}
|
||
|
||
if (LazerWinNum > (MAX_WIN_NUM - WinNumPerLine))
|
||
{
|
||
break;
|
||
}
|
||
|
||
LazerWinNum += WinNumPerLine;
|
||
}
|
||
return testData;
|
||
}
|
||
|
||
void genTestSream(
|
||
std::vector<Luma_rgnData>& srcData,
|
||
std::vector<RgnPix>& inStream,
|
||
const FramePara inFramePara)
|
||
{
|
||
RgnPix VSync = { 0,0,0,0,0,0,0 };
|
||
uint_32 data_32;
|
||
//1st:FrmNo
|
||
data_32 = inFramePara.FrmNo;
|
||
VSync.Sync = 0b10;
|
||
VSync.LazerWinX = data_32 & 0xfff; //range(11, 0);
|
||
data_32 = data_32 >> 12;
|
||
VSync.LazerWinY = data_32 & 0xfff;
|
||
data_32 = data_32 >> 12;
|
||
VSync.LazerWinRid = data_32 & 0xff;
|
||
inStream.push_back(VSync);
|
||
//2nd:FrmNo
|
||
data_32 = inFramePara.timeStamp;
|
||
VSync.Sync = 0b00;
|
||
VSync.LazerWinX = data_32 & 0xfff;
|
||
data_32 = data_32 >> 12;
|
||
VSync.LazerWinY = data_32 & 0xfff;
|
||
data_32 = data_32 >> 12;
|
||
VSync.LazerWinRid = data_32 & 0xff;
|
||
inStream.push_back(VSync);
|
||
//3rd:encInfo
|
||
data_32 = inFramePara.encInfo;
|
||
VSync.Sync = 0b00;
|
||
VSync.LazerWinX = data_32 & 0xfff;
|
||
data_32 = data_32 >> 12;
|
||
VSync.LazerWinY = data_32 & 0xfff;
|
||
data_32 = data_32 >> 12;
|
||
VSync.LazerWinRid = data_32 & 0xff;
|
||
inStream.push_back(VSync);
|
||
//4th:frmW(12bit), frmH(12bit)
|
||
ushort data16_1, data16_2;
|
||
data16_1 = inFramePara.frameROI_w;
|
||
data16_2 = inFramePara.frameROI_h;
|
||
VSync.Sync = 0b00;
|
||
VSync.LazerWinX = data16_1;
|
||
VSync.LazerWinY = data16_2;
|
||
inStream.push_back(VSync);
|
||
//5th: frmX, WinNum
|
||
int winNum = (int)srcData.size();
|
||
data16_1 = inFramePara.frameROI_x;
|
||
data16_2 = (uint16_t)winNum;
|
||
VSync.Sync = 0b00;
|
||
VSync.LazerWinX = data16_1;
|
||
VSync.LazerWinY = data16_2;
|
||
inStream.push_back(VSync);
|
||
//6th: frmY
|
||
data16_1 = inFramePara.frameROI_y;
|
||
VSync.Sync = 0b00;
|
||
VSync.LazerWinX = data16_1;
|
||
VSync.LazerWinY = 0;
|
||
inStream.push_back(VSync);
|
||
//output data
|
||
for (int i = 0; i < winNum; i++)
|
||
{
|
||
Luma_rgnData* a_line = &srcData[i];
|
||
RgnPix Hsync;
|
||
Hsync.LazerWinX = a_line->WinRdx;
|
||
Hsync.LazerWinY = a_line->y;
|
||
Hsync.LazerWinRid = a_line->Rid;
|
||
Hsync.LazerWinFlag = a_line->Flag;
|
||
Hsync.LazerWinRsv = 0;
|
||
Hsync.RltvRdx = a_line->PeakRltvRdx;
|
||
Hsync.Sync = 0b01;
|
||
inStream.push_back(Hsync);
|
||
for (int j = 0; j < RGN_DATA_WIN_SIZE / 2; j++)
|
||
{
|
||
RgnPix a_data = { 0,0,0,0,0,0,0 };
|
||
uchar data8_1, data8_2;
|
||
data8_1 = a_line->data[j * 2];
|
||
data8_2 = a_line->data[j * 2 + 1];
|
||
a_data.LazerWinX = data8_1;
|
||
a_data.LazerWinY = data8_2;
|
||
a_data.Sync = 0b00;
|
||
inStream.push_back(a_data);
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
std::vector<CalibData> _combineSubpixCalibData(
|
||
std::vector<RgnSubPix>& subpixPnt,
|
||
LineCalibK lineCalibKx[MAX_HEIGHT],
|
||
LineCalibK lineCalibKy[MAX_HEIGHT])
|
||
{
|
||
std::vector<CalibData> outStream;
|
||
//1st: timeStamp
|
||
int readPtr = 0;
|
||
CalibData OutData;
|
||
RgnSubPix subpix = subpixPnt[readPtr++];
|
||
OutData.Sync = subpix.Sync;
|
||
Byte8 dataBuff;
|
||
UintFloat tmpData;
|
||
tmpData.fdata = subpix.x;
|
||
dataBuff.nData[0] = tmpData.unData;
|
||
OutData.calibK[0] = dataBuff.dData;
|
||
OutData.calibK[1] = 0;
|
||
outStream.push_back(OutData);
|
||
//2nd: timeStamp
|
||
subpix = subpixPnt[readPtr++];
|
||
tmpData.fdata = subpix.x;
|
||
dataBuff.nData[0] = tmpData.unData;
|
||
OutData.Sync = subpix.Sync;
|
||
OutData.calibK[0] = dataBuff.dData;
|
||
outStream.push_back(OutData);
|
||
//3rd:encInfo
|
||
subpix = subpixPnt[readPtr++];
|
||
OutData.Sync = subpix.Sync;
|
||
tmpData.fdata = subpix.x;
|
||
dataBuff.nData[0] = tmpData.unData;
|
||
OutData.calibK[0] = dataBuff.dData;
|
||
outStream.push_back(OutData);
|
||
//4th: frmW, frmH
|
||
subpix = subpixPnt[readPtr++];
|
||
OutData.Sync = subpix.Sync;
|
||
tmpData.fdata = subpix.x;
|
||
dataBuff.nData[0] = tmpData.unData;
|
||
OutData.calibK[0] = dataBuff.dData;
|
||
outStream.push_back(OutData);
|
||
//5th: frmX, WinNum
|
||
subpix = subpixPnt[readPtr++];
|
||
OutData.Sync = subpix.Sync;
|
||
tmpData.fdata = subpix.x;
|
||
dataBuff.nData[0] = tmpData.unData;
|
||
int WinNum = (tmpData.unData >> 12) & 0x7ff;
|
||
OutData.calibK[0] = dataBuff.dData;
|
||
outStream.push_back(OutData);
|
||
//6th: frmY
|
||
subpix = subpixPnt[readPtr++];
|
||
OutData.Sync = subpix.Sync;
|
||
tmpData.fdata = subpix.x;
|
||
dataBuff.nData[0] = tmpData.unData;
|
||
OutData.calibK[0] = dataBuff.dData;
|
||
outStream.push_back(OutData);
|
||
|
||
for (int n = 0; n < WinNum; n++)
|
||
{
|
||
subpix = subpixPnt[readPtr++];
|
||
uint_32 y = subpix.y;
|
||
for (int j = 0; j <= 8; j++)
|
||
{
|
||
if (0 == j)//read subpix
|
||
{
|
||
OutData.Sync = 0x01;
|
||
dataBuff.dData = 0;
|
||
dataBuff.fData[0] = subpix.x;
|
||
int tmpData = subpix.flag & 0x0f;
|
||
tmpData <<= 11;
|
||
tmpData += subpix.rid & 0x7ff;
|
||
tmpData <<= 12;
|
||
tmpData += subpix.y & 0xfff;
|
||
dataBuff.nData[1] = (uint_32)tmpData;
|
||
OutData.calibK[0] = dataBuff.dData;
|
||
dataBuff.dData = 0;
|
||
dataBuff.nData[0] = (uint_32)subpix.value;
|
||
OutData.calibK[1] = dataBuff.dData;
|
||
outStream.push_back(OutData);
|
||
}
|
||
else
|
||
{
|
||
OutData.Sync = 0x0;
|
||
OutData.calibK[0] = lineCalibKx[y].calibK[j - 1];
|
||
OutData.calibK[1] = lineCalibKy[y].calibK[j - 1];
|
||
outStream.push_back(OutData);
|
||
}
|
||
}
|
||
}
|
||
return outStream;
|
||
}
|
||
|
||
void camAlgoSW(
|
||
int inFrmH,
|
||
int inFrmW,
|
||
unsigned char* inData,
|
||
ProcObj& frontendPara,
|
||
FramePara& inFramePara,
|
||
CamPara& calibPara,
|
||
LineCalibK* lineCalibKx,
|
||
LineCalibK* lineCalibKy,
|
||
std::vector<Pnt3D>& resul3DPnts)
|
||
{
|
||
PickLaserResult pFrontendResult;
|
||
PickLazerWin(
|
||
1,
|
||
inFrmW,
|
||
inFrmH,
|
||
inData,
|
||
|
||
frontendPara.MinStgTh,
|
||
frontendPara.LazerWMin,
|
||
frontendPara.LazerWMax,
|
||
frontendPara.RflctPixTh,
|
||
frontendPara.RflctOutEna,
|
||
frontendPara.OverlapPixTh,
|
||
frontendPara.EnhanStep,
|
||
frontendPara.PickEna,
|
||
frontendPara.RgnFltrTh,
|
||
frontendPara.RmvWkEndEna,
|
||
frontendPara.RmvWkEndTh,
|
||
frontendPara.RmvWkEndMultCoe,
|
||
frontendPara.RmvWkEndMinLen,
|
||
frontendPara.PickBFEna,
|
||
frontendPara.PickBkGrnd,
|
||
frontendPara.PickRLenTh,
|
||
|
||
frontendPara.EnergyPickEna,
|
||
frontendPara.PickEnergyType,
|
||
frontendPara.RgnEnergyPLen,
|
||
frontendPara.RgnMeanETh,
|
||
&pFrontendResult
|
||
);
|
||
|
||
//将前端数据转成输入数据格式
|
||
|
||
std::vector<Luma_rgnData> frontendData = SvPickRgnRslt(
|
||
inData,
|
||
inFrmH,
|
||
inFrmW,
|
||
pFrontendResult
|
||
);
|
||
std::vector<RgnPix> InRgnPnt;
|
||
genTestSream(
|
||
frontendData,
|
||
InRgnPnt,
|
||
inFramePara);
|
||
//pkCentering
|
||
std::vector<RgnPix> OutCenteringPnt;
|
||
peakCentering(
|
||
InRgnPnt,
|
||
OutCenteringPnt
|
||
);
|
||
|
||
//Convolve
|
||
std::vector<RgnPixConvolve> OutConvolvePnt;
|
||
convolve(
|
||
OutCenteringPnt,
|
||
OutConvolvePnt);
|
||
|
||
// Subpix
|
||
std::vector<RgnSubPix> OutSubpixPnt;
|
||
subpix(
|
||
OutConvolvePnt,
|
||
OutSubpixPnt);
|
||
|
||
//将subpix转成RgnSubPixCalib格式。在硬件实现时这一步骤由硬件完成
|
||
//数据格式：每行由行同步和后续的8拍构成。行同步传送x和y数据，后8拍依次传送KX0~KX7, 和KY0 ~ KY7
|
||
std::vector<CalibData> inCalibData = _combineSubpixCalibData(
|
||
OutSubpixPnt,
|
||
lineCalibKx,
|
||
lineCalibKy);
|
||
//Calib
|
||
std::vector<RgnSubPixCalib> outSubpixCalib;
|
||
calib(
|
||
inCalibData,
|
||
outSubpixCalib
|
||
);
|
||
|
||
// Compute3D
|
||
compute3D(
|
||
outSubpixCalib,
|
||
resul3DPnts,
|
||
calibPara.u0,
|
||
calibPara.v0,
|
||
calibPara.F_inv,
|
||
calibPara.plane_a,
|
||
calibPara.plane_b,
|
||
calibPara.plane_c
|
||
);
|
||
return;
|
||
} |