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");
|
|||
|
|
}
|