285 lines
8.8 KiB
C++
285 lines
8.8 KiB
C++
|
||
#include <fstream>
|
||
#include <cstring>
|
||
#include <stdio.h>
|
||
//#include "ap_utils.h"
|
||
#include <iostream>
|
||
#include "lrMatch.h"
|
||
#include "../../globals/tbGlobals.h"
|
||
|
||
void readSubpixCalibDataFile(
|
||
const char* file, std::vector<InterpolateCalibSubpix>& testData)
|
||
{
|
||
std::ifstream inputFile(file);
|
||
std::string strLineTxt;
|
||
int size = 0;
|
||
InterpolateCalibSubpix a_line;
|
||
while(getline(inputFile, strLineTxt))
|
||
{
|
||
if (strLineTxt.empty())
|
||
continue;
|
||
|
||
float x;
|
||
int y, Rid, Flag, pkValue, Sync;
|
||
#if defined(__linux__)
|
||
sscanf(strLineTxt.c_str(), "%f %d %d %x %d %x", &x, &y, &Rid, &Flag, &pkValue, &Sync);
|
||
#else
|
||
sscanf_s(strLineTxt.c_str(), "%f %d %d %x %d %x", &x, &y, &Rid, &Flag, &pkValue, &Sync);
|
||
#endif
|
||
a_line.x = (float)x;
|
||
a_line.y = (ap_uint<CAM4K_Y_WIDTH>)y;
|
||
a_line.rid = (ap_uint<11>)Rid;
|
||
a_line.value = (ap_uint<8>)pkValue;
|
||
a_line.flag = (ap_uint<2>)Flag;
|
||
a_line.Sync = (ap_uint<2>)Sync;
|
||
testData.push_back(a_line);
|
||
}
|
||
inputFile.close();
|
||
return;
|
||
}
|
||
|
||
void genLRMatchTestSream(
|
||
std::vector<InterpolateCalibSubpix>& inCalibData,
|
||
hls::stream<InterpolateCalibSubpix>& inCalibStream,
|
||
int LRflag, //0-left image; 1-right image
|
||
const Luma_frameInfo frameInfo)
|
||
{
|
||
InterpolateCalibSubpix inData = {0.0f, 0, 0, 0, 0, 0};
|
||
UintFloat tmp;
|
||
//1st:FrmNo
|
||
inData.Sync = 0b10;
|
||
tmp.unData = frameInfo.FrmNo;
|
||
inData.x = tmp.fdata;
|
||
inCalibStream.write(inData);
|
||
//2nd: timeStamp
|
||
inData.Sync = 0b00;
|
||
tmp.unData = frameInfo.TimeStamp;
|
||
inData.x = tmp.fdata;
|
||
inCalibStream.write(inData);
|
||
//3rd:encInfo
|
||
tmp.unData = frameInfo.encInfo;
|
||
inData.x = tmp.fdata;
|
||
inCalibStream.write(inData);
|
||
//4th: frmW, frmH
|
||
tmp.unData = (frameInfo.H << 16)| frameInfo.W;
|
||
inData.x = tmp.fdata;
|
||
inCalibStream.write(inData);
|
||
//5th: frmX, WinNum
|
||
uint16_t total_point_num = (uint16_t)inCalibData.size();
|
||
tmp.unData = total_point_num << 16;
|
||
if(LRflag == 0)
|
||
tmp.unData = tmp.unData |frameInfo.left_L;
|
||
else
|
||
tmp.unData = tmp.unData |frameInfo.right_L;
|
||
inData.x = tmp.fdata;
|
||
inCalibStream.write(inData);
|
||
//6th: frmY
|
||
if(LRflag == 0)
|
||
tmp.unData = frameInfo.left_T;
|
||
else
|
||
tmp.unData = frameInfo.right_T;
|
||
inData.x = tmp.fdata;
|
||
if(total_point_num == 0)
|
||
inData.Sync = 0b11;
|
||
else
|
||
inData.Sync = 0b00;
|
||
inCalibStream.write(inData);
|
||
//output data
|
||
if(total_point_num > 0)
|
||
{
|
||
for(int i = 0; i < total_point_num; i ++)
|
||
{
|
||
if(i == total_point_num-1)
|
||
int kkk = 1; //debug point
|
||
|
||
inData = inCalibData[i];
|
||
if(i == total_point_num-1)
|
||
inData.Sync = 0b11;
|
||
else
|
||
inData.Sync = 0b00;
|
||
inCalibStream.write(inData);
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void convertSubpixCalibStreamToArray(
|
||
hls::stream<SLRMatchOutput>& streamData,
|
||
std::vector<SLRMatchOutput>& arrayData,
|
||
uint32_t* FrmNo,
|
||
uint32_t* timeStamp,
|
||
uint32_t* encInfo,
|
||
uint16_t* minY,
|
||
uint16_t* maxY)
|
||
{
|
||
SLRMatchOutput VSync = streamData.read();
|
||
UintFloat tmp;
|
||
tmp.fdata = VSync.left_pt1.x;
|
||
*FrmNo = tmp.unData;
|
||
tmp.fdata = VSync.left_pt2.x;
|
||
*timeStamp = tmp.unData;
|
||
tmp.fdata = VSync.right_pt1.x;
|
||
*encInfo = tmp.unData;
|
||
tmp.fdata = VSync.right_pt2.x;
|
||
*minY = tmp.unData & 0xffff;
|
||
*maxY = tmp.unData >> 16;
|
||
|
||
uint16_t ptNum = *maxY - *minY + 1;
|
||
//output data
|
||
for(int i = 0; i < ptNum; i ++)
|
||
{
|
||
SLRMatchOutput data_in = streamData.read();
|
||
arrayData.push_back(data_in);
|
||
if(data_in.syncWord == 0b11)
|
||
break;
|
||
}
|
||
return;
|
||
}
|
||
|
||
void writeLRMatchData(
|
||
const char* fileName,
|
||
std::vector<SLRMatchOutput>& outData)
|
||
{
|
||
std::ofstream sw(fileName);
|
||
|
||
int i_max = outData.size();
|
||
for (int i = 0; i < i_max; i++)
|
||
{
|
||
char data[250];
|
||
|
||
SLRMatchOutput a_matchPt = outData[i];
|
||
|
||
int matchNum = a_matchPt.matchInfo.matchNum;
|
||
int match1_seg_id = a_matchPt.matchInfo.match1.match_seg_id;
|
||
int match1_flag = a_matchPt.matchInfo.match1.match_flag; //1: L-R match; 2:left single; 3: right single
|
||
int match1_L_idx = a_matchPt.matchInfo.match1.pt_idx_L;
|
||
int match1_R_idx = a_matchPt.matchInfo.match1.pt_idx_R;
|
||
int match1_ridL = a_matchPt.matchInfo.match1.rid_L;
|
||
int match1_ridR = a_matchPt.matchInfo.match1.rid_R;
|
||
|
||
int match2_seg_id = a_matchPt.matchInfo.match2.match_seg_id;
|
||
int match2_flag = a_matchPt.matchInfo.match2.match_flag;
|
||
int match2_L_idx = a_matchPt.matchInfo.match2.pt_idx_L;
|
||
int match2_R_idx = a_matchPt.matchInfo.match2.pt_idx_R;
|
||
int match2_ridL = a_matchPt.matchInfo.match2.rid_L;
|
||
int match2_ridR = a_matchPt.matchInfo.match2.rid_R;
|
||
int syncWord = a_matchPt.syncWord;
|
||
#if defined(__linux__)
|
||
sprintf(data, "%01d %04d %01x %01x %01x %04d %04d %04d %01x %01x %01x %04d %04d %01x",
|
||
matchNum, match1_seg_id, match1_flag, match1_L_idx, match1_R_idx,match1_ridL, match1_ridR,
|
||
match2_seg_id, match2_flag, match2_L_idx, match2_R_idx, match2_ridL, match2_ridR, syncWord);
|
||
#else
|
||
sprintf_s(data, "%01d %04d %01x %01x %01x %04d %04d %04d %01x %01x %01x %04d %04d %01x",
|
||
matchNum, match1_seg_id, match1_flag, match1_L_idx, match1_R_idx,match1_ridL, match1_ridR,
|
||
match2_seg_id, match2_flag, match2_L_idx, match2_R_idx, match2_ridL, match2_ridR, syncWord);
|
||
#endif
|
||
sw << data << std::endl;
|
||
|
||
int num = (int)a_matchPt.left_ptNum;
|
||
|
||
int y1 = a_matchPt.left_pt1.y;
|
||
int rid1 = a_matchPt.left_pt1.rid;
|
||
int value1 = a_matchPt.left_pt1.value;
|
||
int flag1 = a_matchPt.left_pt1.flag;
|
||
int matchFlag1 = a_matchPt.left_pt1.matchFlag;
|
||
|
||
int y2 = a_matchPt.left_pt2.y;
|
||
int rid2 = a_matchPt.left_pt2.rid;
|
||
int value2 = a_matchPt.left_pt2.value;
|
||
int flag2 = a_matchPt.left_pt2.flag;
|
||
int matchFlag2 = a_matchPt.left_pt2.matchFlag;
|
||
|
||
#if defined(__linux__)
|
||
sprintf(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
|
||
num, a_matchPt.left_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.left_pt2.x, y2, rid2, value2, flag2, matchFlag2);
|
||
#else
|
||
sprintf_s(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
|
||
num, a_matchPt.left_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.left_pt2.x, y2, rid2, value2, flag2, matchFlag2);
|
||
#endif
|
||
sw << data << std::endl;
|
||
|
||
num = (int)a_matchPt.right_ptNum;
|
||
|
||
y1 = a_matchPt.right_pt1.y;
|
||
rid1 = a_matchPt.right_pt1.rid;
|
||
value1 = a_matchPt.right_pt1.value;
|
||
flag1 = a_matchPt.right_pt1.flag;
|
||
matchFlag1 = a_matchPt.right_pt1.matchFlag;
|
||
|
||
y2 = a_matchPt.right_pt2.y;
|
||
rid2 = a_matchPt.right_pt2.rid;
|
||
value2 = a_matchPt.right_pt2.value;
|
||
flag2 = a_matchPt.right_pt2.flag;
|
||
matchFlag2 = a_matchPt.right_pt2.matchFlag;
|
||
|
||
#if defined(__linux__)
|
||
sprintf(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
|
||
num, a_matchPt.right_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.right_pt2.x, y2, rid2, value2, flag2, matchFlag2);
|
||
#else
|
||
sprintf_s(data, "%d %.3f %04d %04d %03d %01x %01x %.3f %04d %04d %03d %01x %01x",
|
||
num, a_matchPt.right_pt1.x, y1, rid1, value1, flag1, matchFlag1, a_matchPt.right_pt2.x, y2, rid2, value2, flag2, matchFlag2);
|
||
#endif
|
||
sw << data << std::endl;
|
||
}
|
||
sw.close();
|
||
}
|
||
|
||
int main()
|
||
{
|
||
setvbuf(stdout, NULL, _IONBF, 0); // _IONBF = <20><EFBFBD><DEBB>壬<EFBFBD><E5A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
TbModule currModule = TB_5_lrMatch;
|
||
|
||
for(int n = 0; n < tb_group_count; n ++)
|
||
{
|
||
int fileStart = tb_groups[n].start_id;
|
||
int fileEnd = tb_groups[n].end_id;
|
||
for(int fi = fileStart; fi <= fileEnd; fi++)
|
||
{
|
||
TbPaths test_path_L;
|
||
int rcode_L = tb_get_paths(currModule, n, fi, 0, &test_path_L);
|
||
TbPaths test_path_R;
|
||
int rcode_R = tb_get_paths(currModule, n, fi, 1, &test_path_R);
|
||
|
||
Luma_frameInfo frameInfo = readParaFile(test_path_L.paras, fi);
|
||
|
||
std::vector<InterpolateCalibSubpix> testData_L;
|
||
readSubpixCalibDataFile(test_path_L.input, testData_L);
|
||
std::vector<InterpolateCalibSubpix> testData_R;
|
||
readSubpixCalibDataFile(test_path_R.input, testData_R);
|
||
|
||
hls::stream<InterpolateCalibSubpix> inStream_L;
|
||
genLRMatchTestSream(testData_L, inStream_L, 0, frameInfo);
|
||
hls::stream<InterpolateCalibSubpix> inStream_R;
|
||
genLRMatchTestSream(testData_R, inStream_R, 1, frameInfo);
|
||
|
||
hls::stream<SLRMatchOutput> matchPoints;
|
||
float uL_offset = 710;
|
||
uint16_t useLaserPlane = 0; //0- disable; 1- enable
|
||
float plane_a_f = 0.0f, plane_b_f=0.0f, plane_c_f= 0.0f, plane_norm=0.0f; //d = a*UL + b*V + c
|
||
float maxPtPlaneDist = 3.0; //point to plane distance threshod=ld
|
||
lrMatch(inStream_L, inStream_R, matchPoints,
|
||
uL_offset,
|
||
useLaserPlane, plane_a_f, plane_b_f, plane_c_f, plane_norm, //d = a*UL + b*V + c
|
||
maxPtPlaneDist //point to plane distance threshod=ld
|
||
);
|
||
|
||
//output
|
||
std::vector<SLRMatchOutput> outData;
|
||
uint32_t outFrmNo, outTimeStamp, outEncInfo;
|
||
uint16_t minY, maxY;
|
||
convertSubpixCalibStreamToArray(
|
||
matchPoints,
|
||
outData,
|
||
&outFrmNo,
|
||
&outTimeStamp,
|
||
&outEncInfo,
|
||
&minY,
|
||
&maxY);
|
||
writeLRMatchData(test_path_L.output, outData);
|
||
|
||
printf("%s ... done\n", test_path_L.input);
|
||
}
|
||
}
|
||
printf("done!\n");
|
||
}
|