jerryzeng/camAlgo
2
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
camAlgo/camAlgoSW/sourceCode/camAlgoSW.cpp

373 lines
10 KiB
C++
Raw Normal View History

初版提交
2025-08-16 15:25:29 +08:00
///<2F><><EFBFBD><EFBFBD>
/// 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;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
uint16_t y; //y<><79><EFBFBD><EFBFBD>
uint16_t Rid; //region<6F><6E>ID
uint8_t Flag; //bit0<74><30>overlap<61><70>־<EFBFBD><D6BE>bit1<74>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>źű<C5BA>־
uint8_t PeakRltvRdx; //peak<61>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD>ڵĵ<DAB5><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>꣬<EFBFBD><EAA3AC>4λ<34><CEBB><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD>꣬<EFBFBD><EAA3AC>4λ<34>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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
);
//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ
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);
//<2F><>subpixת<78><D7AA>RgnSubPixCalib<69><62>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>Ӳ<EFBFBD><D3B2>ʵ<EFBFBD><CAB5>ʱ<EFBFBD><CAB1>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD><CABD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD>8<EFBFBD>Ĺ<EFBFBD><C4B9>ɡ<EFBFBD><C9A1><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x<EFBFBD><78>y<EFBFBD><79><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>8<EFBFBD><38><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD>KX0~KX7, <20><>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;
}
Reference in New Issue Copy Permalink