949 lines
27 KiB
C++
949 lines
27 KiB
C++
#include "lrMatch.h"
|
||
#include "stdlib.h"
|
||
#include <cstdint>
|
||
#include <cmath>
|
||
#include <cfloat>
|
||
|
||
void read_left(
|
||
hls::stream<InterpolateCalibSubpix> &inSubpix_L,
|
||
SRowData left_buf[ROW_COUNT],
|
||
ap_uint<42> leftState[ROW_COUNT],
|
||
uint16_t* roiL_L,
|
||
uint16_t* roiL_T,
|
||
uint16_t* roiL_W,
|
||
uint16_t* roiL_H,
|
||
uint16_t* left_min_y,
|
||
uint16_t* left_max_y,
|
||
uint32_t* roiL_FrmNo,
|
||
uint32_t* roiL_TimeStamp,
|
||
uint32_t* roiL_encInfo,
|
||
ap_uint<40>* roiL_frameTag)
|
||
{
|
||
#pragma HLS INLINE off
|
||
|
||
InterpolateCalibSubpix InData;
|
||
ap_uint<2> VSync;
|
||
while(1) //wait for vSync signal
|
||
{
|
||
InData = inSubpix_L.read();
|
||
VSync = InData.Sync;
|
||
if(0b10 == VSync) //wait for syncFirst
|
||
break;
|
||
}
|
||
|
||
//1st: frameNo
|
||
UintFloat tmp;
|
||
tmp.fdata = InData.x;
|
||
*roiL_FrmNo = tmp.unData;
|
||
//2nd: timeStamp
|
||
InData = inSubpix_L.read();
|
||
tmp.fdata = InData.x;
|
||
*roiL_TimeStamp = tmp.unData;
|
||
//3rd:encInfo
|
||
InData = inSubpix_L.read();
|
||
tmp.fdata = InData.x;
|
||
*roiL_encInfo = tmp.unData;
|
||
//4th: frmW, frmH
|
||
InData = inSubpix_L.read();
|
||
tmp.fdata = InData.x;
|
||
*roiL_W = tmp.unData & 0xffff;
|
||
*roiL_H = tmp.unData >> 16;
|
||
//5th: frmX, WinNum
|
||
InData = inSubpix_L.read();
|
||
tmp.fdata = InData.x;
|
||
*roiL_L = tmp.unData & 0xffff;
|
||
uint16_t winNum = tmp.unData >> 16;
|
||
//6th: frmY
|
||
InData = inSubpix_L.read();
|
||
tmp.fdata = InData.x;
|
||
*roiL_T = tmp.unData & 0xffff;
|
||
|
||
static ap_uint<40> leftFrameGeneration = 0;
|
||
#pragma HLS RESET variable=leftFrameGeneration off
|
||
|
||
const ap_uint<40> max_generation = 0xffffffffffULL;
|
||
if (leftFrameGeneration == max_generation)
|
||
{
|
||
LEFT_GENERATION_WRAP_CLEAR:
|
||
for (unsigned row = 0; row < ROW_COUNT; ++row)
|
||
{
|
||
#pragma HLS PIPELINE II=1
|
||
leftState[row] = 0;
|
||
}
|
||
|
||
// ÇåÁãÍê³ÉºóÔÙ·¢²¼ generation 1¡£
|
||
leftFrameGeneration = 1;
|
||
}
|
||
else
|
||
{
|
||
leftFrameGeneration = leftFrameGeneration + 1;
|
||
}
|
||
|
||
const ap_uint<40> currFrameTag = leftFrameGeneration;
|
||
|
||
uint16_t min_y = 0xffff;
|
||
uint16_t max_y = 0;
|
||
|
||
uint16_t previousY = 0xffff;
|
||
for(uint16_t n = 0; n < winNum; n ++)
|
||
{
|
||
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
||
#pragma HLS PIPELINE II=1
|
||
|
||
InData = inSubpix_L.read();
|
||
BuffCalibSubpix pkPt;
|
||
pkPt.x = InData.x;
|
||
pkPt.y = InData.y;
|
||
pkPt.rid = InData.rid;
|
||
pkPt.value = InData.value;
|
||
pkPt.flag = InData.flag;
|
||
pkPt.matchFlag = 0;
|
||
uint16_t y = (uint16_t)InData.y;
|
||
|
||
if (y < ROW_COUNT)
|
||
{
|
||
const bool firstPoint = (y != previousY);
|
||
const bool secondPoint = (y == previousY) ;
|
||
|
||
if (firstPoint)
|
||
left_buf[y].pt1 = pkPt;
|
||
else if (secondPoint)
|
||
left_buf[y].pt2 = pkPt;
|
||
|
||
// ºÏ·¨×´Ì¬Ö»ÓÐ0¡¢1¡¢2£ºµÚÒ»´ÎдºóΪ1£¬ÆäÓà±¥ºÍΪ2¡£
|
||
ap_uint<42> newState = 0;
|
||
newState.range(41, 2) = currFrameTag;
|
||
newState.range(1, 0) = firstPoint ? ap_uint<2>(1) : ap_uint<2>(2);
|
||
leftState[y] = newState;
|
||
if (min_y > y) min_y = y;
|
||
if (max_y < y) max_y = y;
|
||
|
||
previousY = y;
|
||
}
|
||
|
||
if(0b11 == InData.Sync)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
*left_min_y = min_y;
|
||
*left_max_y = max_y;
|
||
*roiL_frameTag = currFrameTag;
|
||
return;
|
||
}
|
||
|
||
void read_right(
|
||
hls::stream<InterpolateCalibSubpix> &inSubpix_R,
|
||
SRowData right_buf[ROW_COUNT],
|
||
ap_uint<42> rightState[ROW_COUNT],
|
||
uint16_t* roiR_L,
|
||
uint16_t* roiR_T,
|
||
uint16_t* roiR_W,
|
||
uint16_t* roiR_H,
|
||
uint16_t* right_min_y,
|
||
uint16_t* right_max_y,
|
||
uint32_t* roiR_FrmNo,
|
||
uint32_t* roiR_TimeStamp,
|
||
uint32_t* roiR_encInfo,
|
||
ap_uint<40>* roiR_frameTag)
|
||
{
|
||
#pragma HLS INLINE off
|
||
InterpolateCalibSubpix InData;
|
||
ap_uint<2> VSync;
|
||
while(1) //wait for vSync signal
|
||
{
|
||
InData = inSubpix_R.read();
|
||
VSync = InData.Sync;
|
||
if(0b10 == VSync) //wait for syncFirst
|
||
break;
|
||
}
|
||
|
||
//1st: frameNo
|
||
UintFloat tmp;
|
||
tmp.fdata = InData.x;
|
||
*roiR_FrmNo = tmp.unData;
|
||
//2nd: timeStamp
|
||
InData = inSubpix_R.read();
|
||
tmp.fdata = InData.x;
|
||
*roiR_TimeStamp = tmp.unData;
|
||
//3rd:encInfo
|
||
InData = inSubpix_R.read();
|
||
tmp.fdata = InData.x;
|
||
*roiR_encInfo = tmp.unData;
|
||
//4th: frmW, frmH
|
||
InData = inSubpix_R.read();
|
||
tmp.fdata = InData.x;
|
||
*roiR_W = tmp.unData & 0xffff;
|
||
*roiR_H = tmp.unData >> 16;
|
||
//5th: frmX, WinNum
|
||
InData = inSubpix_R.read();
|
||
tmp.fdata = InData.x;
|
||
*roiR_L = tmp.unData & 0xffff;
|
||
uint16_t winNum = tmp.unData >> 16;
|
||
//6th: frmY
|
||
InData = inSubpix_R.read();
|
||
tmp.fdata = InData.x;
|
||
*roiR_T = tmp.unData & 0xffff;
|
||
|
||
// Óë rightState Ò»Æð±£Áô£¬ÔËÐÐʱ¸´Î»²»ÇåÁã¡£
|
||
static ap_uint<40> rightFrameGeneration = 0;
|
||
#pragma HLS RESET variable=rightFrameGeneration off
|
||
|
||
const ap_uint<40> max_generation = 0xffffffffffULL;
|
||
if (rightFrameGeneration == max_generation)
|
||
{
|
||
RIGHT_GENERATION_WRAP_CLEAR:
|
||
for (unsigned row = 0; row < ROW_COUNT; ++row)
|
||
{
|
||
#pragma HLS PIPELINE II=1
|
||
rightState[row] = 0;
|
||
}
|
||
// ÇåÁãÍê³ÉºóÔÙ·¢²¼ generation 1¡£
|
||
rightFrameGeneration = 1;
|
||
}
|
||
else
|
||
{
|
||
rightFrameGeneration = rightFrameGeneration + 1;
|
||
}
|
||
|
||
const ap_uint<40> currFrameTag = rightFrameGeneration;
|
||
|
||
uint16_t min_y = 0xffff;
|
||
uint16_t max_y = 0;
|
||
|
||
uint16_t previousY = 0xffff;
|
||
for(uint16_t n = 0; n < winNum; n ++)
|
||
{
|
||
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
||
#pragma HLS PIPELINE II=1
|
||
|
||
InData = inSubpix_R.read();
|
||
BuffCalibSubpix pkPt;
|
||
pkPt.x = InData.x;
|
||
pkPt.y = InData.y;
|
||
pkPt.rid = InData.rid;
|
||
pkPt.value = InData.value;
|
||
pkPt.flag = InData.flag;
|
||
pkPt.matchFlag = 0;
|
||
|
||
uint16_t y = (uint16_t)InData.y;
|
||
if (y < ROW_COUNT)
|
||
{
|
||
const bool firstPoint = (y != previousY);
|
||
const bool secondPoint = (y == previousY) ;
|
||
|
||
if (firstPoint)
|
||
right_buf[y].pt1 = pkPt;
|
||
else if (secondPoint)
|
||
right_buf[y].pt2 = pkPt;
|
||
|
||
// ºÏ·¨×´Ì¬Ö»ÓÐ0¡¢1¡¢2£ºµÚÒ»´ÎдºóΪ1£¬ÆäÓà±¥ºÍΪ2¡£
|
||
ap_uint<42> newState = 0;
|
||
newState.range(41, 2) = currFrameTag;
|
||
newState.range(1, 0) =
|
||
firstPoint ? ap_uint<2>(1) : ap_uint<2>(2);
|
||
|
||
rightState[y] = newState;
|
||
|
||
if (min_y > y) min_y = y;
|
||
if (max_y < y) max_y = y;
|
||
|
||
previousY = y;
|
||
}
|
||
|
||
if(0b11 == InData.Sync)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
*right_min_y = min_y;
|
||
*right_max_y = max_y;
|
||
*roiR_frameTag = currFrameTag;
|
||
return;
|
||
}
|
||
|
||
void receive_lr(
|
||
hls::stream<InterpolateCalibSubpix> &inSubpix_L,
|
||
SRowData left_buf[ROW_COUNT],
|
||
ap_uint<42> leftState[ROW_COUNT],
|
||
uint16_t* roiL_L,
|
||
uint16_t* roiL_T,
|
||
uint16_t* roiL_W,
|
||
uint16_t* roiL_H,
|
||
uint16_t* left_min_y,
|
||
uint16_t* left_max_y,
|
||
uint32_t* roiL_FrmNo,
|
||
uint32_t* roiL_TimeStamp,
|
||
uint32_t* roiL_encInfo,
|
||
ap_uint<40>* roiL_frameTag,
|
||
|
||
hls::stream<InterpolateCalibSubpix> &inSubpix_R,
|
||
SRowData right_buf[ROW_COUNT],
|
||
ap_uint<42> rightState[ROW_COUNT],
|
||
uint16_t* roiR_L,
|
||
uint16_t* roiR_T,
|
||
uint16_t* roiR_W,
|
||
uint16_t* roiR_H,
|
||
uint16_t* right_min_y,
|
||
uint16_t* right_max_y,
|
||
uint32_t* roiR_FrmNo,
|
||
uint32_t* roiR_TimeStamp,
|
||
uint32_t* roiR_encInfo,
|
||
ap_uint<40>* roiR_frameTag)
|
||
{
|
||
#pragma HLS INLINE off
|
||
#pragma HLS DATAFLOW
|
||
read_left(inSubpix_L, left_buf, leftState,
|
||
roiL_L, roiL_T, roiL_W, roiL_H, left_min_y, left_max_y,
|
||
roiL_FrmNo, roiL_TimeStamp, roiL_encInfo, roiL_frameTag);
|
||
read_right(inSubpix_R, right_buf, rightState,
|
||
roiR_L, roiR_T, roiR_W, roiR_H, right_min_y, right_max_y,
|
||
roiR_FrmNo, roiR_TimeStamp, roiR_encInfo, roiR_frameTag);
|
||
}
|
||
|
||
float _computePtPlaneDist(
|
||
float xL, float xR, float y,float uL_offset,
|
||
float plane_a_f, float plane_b_f, float plane_c_f, //d = a*UL + b*V + c
|
||
float plane_norm
|
||
)
|
||
{
|
||
float dist = FLT_MAX;
|
||
float disparity = xL + uL_offset - xR;
|
||
if(disparity <= 10)
|
||
{
|
||
return dist;
|
||
}
|
||
dist = xL * plane_a_f + y * plane_b_f - disparity + plane_c_f;
|
||
if(dist < 0)
|
||
dist = -dist;
|
||
dist = dist * plane_norm;
|
||
return dist;
|
||
}
|
||
|
||
void lrMatch(
|
||
hls::stream<InterpolateCalibSubpix> &inSubpix_L,
|
||
hls::stream<InterpolateCalibSubpix> &inSubpix_R,
|
||
hls::stream<SLRMatchOutput> &outMatchPt,
|
||
float uL_offset,
|
||
uint16_t useLaserPlane, //0- disable; 1- enable
|
||
float plane_a_f, //d = a*UL + b*V + c
|
||
float plane_b_f,
|
||
float plane_c_f,
|
||
float plane_norm,
|
||
float maxPtPlaneDist //point to plane distance threshod=ld
|
||
)
|
||
{
|
||
#if !(VITIS_HLS)
|
||
#pragma HLS DATA_PACK variable=inSubpix_L
|
||
#pragma HLS DATA_PACK variable=inSubpix_R
|
||
#pragma HLS DATA_PACK variable=outMatchPt
|
||
#else
|
||
#pragma HLS aggregate variable=inSubpix_L compact=bit
|
||
#pragma HLS aggregate variable=inSubpix_R compact=bit
|
||
#pragma HLS aggregate variable=outMatchPt compact=bit
|
||
#endif
|
||
#pragma HLS INTERFACE axis register both port=inSubpix_L
|
||
#pragma HLS INTERFACE axis register both port=inSubpix_R
|
||
#pragma HLS INTERFACE axis register both port=outMatchPt
|
||
|
||
#pragma HLS INTERFACE mode=ap_none port=uL_offset register
|
||
#pragma HLS INTERFACE mode=ap_none port=useLaserPlane register
|
||
#pragma HLS INTERFACE mode=ap_none port=plane_a_f register
|
||
#pragma HLS INTERFACE mode=ap_none port=plane_b_f register
|
||
#pragma HLS INTERFACE mode=ap_none port=plane_c_f register
|
||
#pragma HLS INTERFACE mode=ap_none port=plane_norm register
|
||
#pragma HLS INTERFACE mode=ap_none port=maxPtPlaneDist register
|
||
|
||
// ¸ß40λ£ºÖ¡±ê¼Ç£»µÍ2λ£º±¾ÐеãÊý
|
||
// [41:2]£º40λÄÚ²¿Ö¡±ê¼Ç£»[1:0]£º±¾ÐеãÊý¡£
|
||
// Éϵç³õʼ»¯Îª0£¬ÆÕͨ֡²»ÇåÁã¡£
|
||
static ap_uint<42> leftState[ROW_COUNT] = {};
|
||
static ap_uint<42> rightState[ROW_COUNT] = {};
|
||
#pragma HLS BIND_STORAGE variable=leftState type=ram_2p impl=bram
|
||
#pragma HLS BIND_STORAGE variable=rightState type=ram_2p impl=bram
|
||
#pragma HLS RESET variable=leftState off
|
||
#pragma HLS RESET variable=rightState off
|
||
|
||
static SRowData left_buf[ROW_COUNT];
|
||
static SRowData right_buf[ROW_COUNT];
|
||
#pragma HLS BIND_STORAGE variable=left_buf type=ram_2p impl=bram
|
||
#pragma HLS BIND_STORAGE variable=right_buf type=ram_2p impl=bram
|
||
#pragma HLS RESET variable=left_buf off
|
||
#pragma HLS RESET variable=right_buf off
|
||
|
||
static SLineMatchInfo matchInfo_buf[ROW_COUNT];
|
||
#pragma HLS BIND_STORAGE variable=matchInfo_buf type=ram_2p impl=bram
|
||
static SMatchSegInfo matchSegInfo_buf[ROW_COUNT];
|
||
#pragma HLS BIND_STORAGE variable=matchSegInfo_buf type=ram_2p impl=bram
|
||
|
||
//row data initialization
|
||
SRowData nullRowData;
|
||
nullRowData.ptNum = 0;
|
||
nullRowData.pt1.x = 0.0f;
|
||
nullRowData.pt1.y = 0;; //support up to 4k camera
|
||
nullRowData.pt1.rid = 0;
|
||
nullRowData.pt1.value = 0;
|
||
nullRowData.pt1.flag = 0; //bit0:OverLap | bit1:Rflct
|
||
nullRowData.pt1.matchFlag = 0;
|
||
|
||
nullRowData.pt2.x = 0.0f;
|
||
nullRowData.pt2.y = 0;; //support up to 4k camera
|
||
nullRowData.pt2.rid = 0;
|
||
nullRowData.pt2.value = 0;
|
||
nullRowData.pt2.flag = 0; //bit0:OverLap | bit1:Rflct
|
||
nullRowData.pt2.matchFlag = 0;
|
||
#if 0
|
||
for(uint16_t i= 0; i < ROW_COUNT; i ++)
|
||
{
|
||
#pragma HLS PIPELINE II=1
|
||
left_buf[i] = nullRowData;
|
||
right_buf[i] = nullRowData;
|
||
matchSegInfo_buf[i].seg_id = 0;
|
||
matchSegInfo_buf[i].seg_ptSize = 0;
|
||
}
|
||
#endif
|
||
|
||
//setp 1: read steam into buff, target: II = 1
|
||
uint16_t roiL_L, roiL_T, roiL_W, roiL_H, minY_left, maxY_left;
|
||
uint32_t roiL_FrmNo, roiL_TimeStamp, roiL_encInfo;
|
||
ap_uint<40> roiL_frameTag;
|
||
uint16_t roiR_L, roiR_T, roiR_W, roiR_H, minY_right, maxY_right;
|
||
uint32_t roiR_FrmNo, roiR_TimeStamp, roiR_encInfo;
|
||
ap_uint<40> roiR_frameTag;
|
||
receive_lr(
|
||
inSubpix_L, left_buf, leftState,
|
||
&roiL_L, &roiL_T, &roiL_W, &roiL_H, &minY_left, &maxY_left,
|
||
&roiL_FrmNo, &roiL_TimeStamp, &roiL_encInfo, &roiL_frameTag,
|
||
inSubpix_R, right_buf, rightState,
|
||
&roiR_L, &roiR_T, &roiR_W, &roiR_H, &minY_right, &maxY_right,
|
||
&roiR_FrmNo, &roiR_TimeStamp, &roiR_encInfo, &roiR_frameTag);
|
||
|
||
bool left_empty = (minY_left == 0xffff);
|
||
bool right_empty = (minY_right == 0xffff);
|
||
|
||
uint16_t minY, maxY;
|
||
if( left_empty || right_empty)
|
||
{
|
||
minY = 0;
|
||
maxY = 0;
|
||
}
|
||
else
|
||
{
|
||
minY = minY_left > minY_right ? minY_right : minY_left;
|
||
maxY = maxY_left < maxY_right ? maxY_right : maxY_left;
|
||
}
|
||
|
||
//step 2: LR match, target II = 1
|
||
uint16_t idxL = 0;
|
||
uint16_t idxR = 0;
|
||
uint16_t linePtNum_L = 0;
|
||
uint16_t linePtNum_R = 0;
|
||
InterpolateCalibSubpix L_pt1, L_pt2;
|
||
InterpolateCalibSubpix R_pt1, R_pt2;
|
||
for(uint16_t i = minY; i <= maxY; i ++)
|
||
{
|
||
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
||
#pragma HLS PIPELINE II=1
|
||
|
||
SRowData line_L = left_buf[i];
|
||
SRowData line_R = right_buf[i];
|
||
float y = (float)line_L.pt1.y;
|
||
|
||
const ap_uint<42> stateL = leftState[i];
|
||
const ap_uint<40> tagL = stateL.range(41, 2);
|
||
const ap_uint<2> numL = stateL.range(1, 0);
|
||
const ap_uint<42> stateR = rightState[i];
|
||
const ap_uint<40> tagR = stateR.range(41, 2);
|
||
const ap_uint<2> numR = stateR.range(1, 0);
|
||
const bool validL = (tagL == roiL_frameTag);
|
||
const bool validR = (tagR == roiR_frameTag);
|
||
|
||
SLineMatchInfo a_lineMatch;
|
||
a_lineMatch.matchNum = 0;
|
||
a_lineMatch.match1.match_seg_id = 0;
|
||
a_lineMatch.match1.match_flag = 0;
|
||
a_lineMatch.match1.pt_idx_L = 0;
|
||
a_lineMatch.match1.pt_idx_R = 0;
|
||
a_lineMatch.match1.rid_L = 0;
|
||
a_lineMatch.match1.rid_R = 0;
|
||
|
||
a_lineMatch.match2.match_seg_id = 0;
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
//compute distance to laser plane
|
||
float dist_11 = _computePtPlaneDist(line_L.pt1.x, line_R.pt1.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
|
||
float dist_12 = _computePtPlaneDist(line_L.pt1.x, line_R.pt2.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
|
||
float dist_21 = _computePtPlaneDist(line_L.pt2.x, line_R.pt1.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
|
||
float dist_22 = _computePtPlaneDist(line_L.pt2.x, line_R.pt2.x, y, uL_offset, plane_a_f, plane_b_f, plane_c_f, plane_norm);
|
||
if( validL && validR && (numL == 1)&& (numR == 1))
|
||
{
|
||
if( (useLaserPlane == 0) || ((useLaserPlane == 1) && (dist_11 < maxPtPlaneDist)))
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 1;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt1.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
}
|
||
}
|
||
else if( validL && validR && (numL == 1)&& (numR == 2))
|
||
{
|
||
if(useLaserPlane == 1)
|
||
{
|
||
if( (dist_11 < maxPtPlaneDist) && (dist_11 < dist_12))
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 1;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt1.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
}
|
||
else if( (dist_12 < maxPtPlaneDist) && (dist_12 < dist_11))
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 2;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt2.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt2.matchFlag = 1;
|
||
}
|
||
}
|
||
}
|
||
else if( validL && validR && (numL == 2)&& (numR == 1))
|
||
{
|
||
if(useLaserPlane == 1)
|
||
{
|
||
if( (dist_11 < maxPtPlaneDist) && (dist_11 < dist_21))
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 1;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt1.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
}
|
||
else if( (dist_21 < maxPtPlaneDist) && (dist_21 < dist_11))
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 2;
|
||
a_lineMatch.match1.pt_idx_R = 1;
|
||
a_lineMatch.match1.rid_L = line_L.pt2.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt1.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt2.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
}
|
||
}
|
||
}
|
||
else if( validL && validR && (numL == 2)&& (numR == 2))
|
||
{
|
||
if(useLaserPlane == 1)
|
||
{
|
||
if( (dist_11 < maxPtPlaneDist) && (dist_11 < dist_12))
|
||
{
|
||
if( dist_22 < maxPtPlaneDist)
|
||
{
|
||
a_lineMatch.matchNum = 2;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 1;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt1.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 1;
|
||
a_lineMatch.match2.pt_idx_L = 2;
|
||
a_lineMatch.match2.pt_idx_R = 2;
|
||
a_lineMatch.match2.rid_L = line_L.pt2.rid;
|
||
a_lineMatch.match2.rid_R = line_R.pt2.rid;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
left_buf[i].pt2.matchFlag = 1;
|
||
right_buf[i].pt2.matchFlag = 1;
|
||
}
|
||
else
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 1;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt1.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
}
|
||
}
|
||
else if( (dist_12 < maxPtPlaneDist) && (dist_12 < dist_11))
|
||
{
|
||
if(dist_21 < maxPtPlaneDist)
|
||
{
|
||
a_lineMatch.matchNum = 2;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 2;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt2.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 1;
|
||
a_lineMatch.match2.pt_idx_L = 2;
|
||
a_lineMatch.match2.pt_idx_R = 1;
|
||
a_lineMatch.match2.rid_L = line_L.pt2.rid;
|
||
a_lineMatch.match2.rid_R = line_R.pt1.rid;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt2.matchFlag = 1;
|
||
left_buf[i].pt2.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
}
|
||
else
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 1;
|
||
a_lineMatch.match1.pt_idx_R = 2;
|
||
a_lineMatch.match1.rid_L = line_L.pt1.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt2.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt1.matchFlag = 1;
|
||
right_buf[i].pt2.matchFlag = 1;
|
||
}
|
||
}
|
||
else if( (dist_21 < maxPtPlaneDist) && (dist_21 < dist_22))
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 2;
|
||
a_lineMatch.match1.pt_idx_R = 1;
|
||
a_lineMatch.match1.rid_L = line_L.pt2.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt1.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt2.matchFlag = 1;
|
||
right_buf[i].pt1.matchFlag = 1;
|
||
}
|
||
else if( (dist_22 < maxPtPlaneDist) && (dist_22 < dist_21))
|
||
{
|
||
a_lineMatch.matchNum = 1;
|
||
|
||
a_lineMatch.match1.match_flag = 1;
|
||
a_lineMatch.match1.pt_idx_L = 2;
|
||
a_lineMatch.match1.pt_idx_R = 2;
|
||
a_lineMatch.match1.rid_L = line_L.pt2.rid;
|
||
a_lineMatch.match1.rid_R = line_R.pt2.rid;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
|
||
left_buf[i].pt2.matchFlag = 1;
|
||
right_buf[i].pt2.matchFlag = 1;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
a_lineMatch.matchNum = 0;
|
||
|
||
a_lineMatch.match1.match_flag = 0;
|
||
a_lineMatch.match1.pt_idx_L = 0;
|
||
a_lineMatch.match1.pt_idx_R = 0;
|
||
a_lineMatch.match1.rid_L = 0;
|
||
a_lineMatch.match1.rid_R = 0;
|
||
|
||
a_lineMatch.match2.match_flag = 0;
|
||
a_lineMatch.match2.pt_idx_L = 0;
|
||
a_lineMatch.match2.pt_idx_R = 0;
|
||
a_lineMatch.match2.rid_L = 0;
|
||
a_lineMatch.match2.rid_R = 0;
|
||
}
|
||
|
||
matchInfo_buf[i] = a_lineMatch;
|
||
}
|
||
|
||
// Step 3: ·ÖÅäÆ¥Åä¶Î±àºÅ
|
||
// ²Ûλ1·ÖÅäÆæÊý±àºÅ£¬²Ûλ2·ÖÅäżÊý±àºÅ¡£
|
||
ap_uint<CAM4K_Y_WIDTH> match_seg_id_1 = 0;
|
||
ap_uint<CAM4K_Y_WIDTH> match_seg_id_2 = 0;
|
||
// ÆÕͨ¾Ö²¿±äÁ¿£¬Ã¿Ö¡´Ó 1 ¿ªÊ¼£»0 ±£ÁôΪÎÞЧ ID
|
||
ap_uint<CAM4K_Y_WIDTH> next_seg_id = 1;
|
||
|
||
ap_uint<11> m1_ridL = 0;
|
||
ap_uint<11> m1_ridR = 0;
|
||
ap_uint<11> m2_ridL = 0;
|
||
ap_uint<11> m2_ridR = 0;
|
||
|
||
MATCH_SEGMENT:
|
||
for (uint16_t i = minY; i <= maxY; ++i)
|
||
{
|
||
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
||
#pragma HLS PIPELINE II=3
|
||
|
||
SLineMatchInfo a_lineMatch = matchInfo_buf[i];
|
||
const bool current1Valid = (a_lineMatch.matchNum > 0) && (a_lineMatch.match1.rid_L != 0) && (a_lineMatch.match1.rid_R != 0);
|
||
const bool current2Valid = (a_lineMatch.matchNum == 2) && (a_lineMatch.match2.rid_L != 0) && (a_lineMatch.match2.rid_R != 0);
|
||
|
||
// ÀúÊ·²ÛλÊÇ·ñ±»±¾ÐÐʹÓÃ
|
||
bool previousSlot1Used = false;
|
||
bool previousSlot2Used = false;
|
||
// µ±Ç°Á½¸öÆ¥ÅäÊÇ·ñÒѾ·ÖÅä±àºÅ
|
||
bool currentMatch1Assigned = false;
|
||
bool currentMatch2Assigned = false;
|
||
|
||
a_lineMatch.match1.match_seg_id = 0;
|
||
a_lineMatch.match2.match_seg_id = 0;
|
||
// --------------------------------------------------
|
||
// 1. µ±Ç° match1 ÓÅÏÈÆ¥ÅäÀúÊ·²Ûλ1£¬Æä´Î²Ûλ2
|
||
// --------------------------------------------------
|
||
if (current1Valid)
|
||
{
|
||
if ((m1_ridL != 0) &&
|
||
(m1_ridL == a_lineMatch.match1.rid_L) &&
|
||
(m1_ridR == a_lineMatch.match1.rid_R))
|
||
{
|
||
a_lineMatch.match1.match_seg_id = match_seg_id_1;
|
||
previousSlot1Used = true;
|
||
currentMatch1Assigned = true;
|
||
}
|
||
else if ((m2_ridL != 0) &&
|
||
(m2_ridL == a_lineMatch.match1.rid_L) &&
|
||
(m2_ridR == a_lineMatch.match1.rid_R))
|
||
{
|
||
a_lineMatch.match1.match_seg_id = match_seg_id_2;
|
||
previousSlot2Used = true;
|
||
currentMatch1Assigned = true;
|
||
}
|
||
}
|
||
|
||
// --------------------------------------------------
|
||
// 2. µ±Ç° match2 Æ¥ÅäÉÐδ±»Ê¹ÓõÄÀúÊ·²Ûλ
|
||
// Ò»¸öÀúÊ·²Ûλ×î¶àÆ¥Åä±¾ÐеÄÒ»¸öÆ¥ÅäÏî
|
||
// --------------------------------------------------
|
||
if (current2Valid)
|
||
{
|
||
if (!previousSlot1Used &&
|
||
(m1_ridL != 0) &&
|
||
(m1_ridL == a_lineMatch.match2.rid_L) &&
|
||
(m1_ridR == a_lineMatch.match2.rid_R))
|
||
{
|
||
a_lineMatch.match2.match_seg_id = match_seg_id_1;
|
||
previousSlot1Used = true;
|
||
currentMatch2Assigned = true;
|
||
}
|
||
else if (!previousSlot2Used &&
|
||
(m2_ridL != 0) &&
|
||
(m2_ridL == a_lineMatch.match2.rid_L) &&
|
||
(m2_ridR == a_lineMatch.match2.rid_R))
|
||
{
|
||
a_lineMatch.match2.match_seg_id = match_seg_id_2;
|
||
previousSlot2Used = true;
|
||
currentMatch2Assigned = true;
|
||
}
|
||
}
|
||
|
||
// --------------------------------------------------
|
||
// 3. ΪÉÐδƥÅäµÄ match1 ·ÖÅäжÎ
|
||
// Ö»¸²¸Ç±¾ÐÐûÓÐʹÓõÄÀúÊ·²Ûλ
|
||
// --------------------------------------------------
|
||
if (current1Valid && !currentMatch1Assigned)
|
||
{
|
||
if (!previousSlot1Used)
|
||
{
|
||
m1_ridL = a_lineMatch.match1.rid_L;
|
||
m1_ridR = a_lineMatch.match1.rid_R;
|
||
|
||
match_seg_id_1 = next_seg_id;
|
||
next_seg_id++;
|
||
a_lineMatch.match1.match_seg_id = match_seg_id_1;
|
||
previousSlot1Used = true;
|
||
currentMatch1Assigned = true;
|
||
}
|
||
else if (!previousSlot2Used)
|
||
{
|
||
m2_ridL = a_lineMatch.match1.rid_L;
|
||
m2_ridR = a_lineMatch.match1.rid_R;
|
||
|
||
match_seg_id_2 = next_seg_id;
|
||
next_seg_id++;
|
||
a_lineMatch.match1.match_seg_id = match_seg_id_2;
|
||
previousSlot2Used = true;
|
||
currentMatch1Assigned = true;
|
||
}
|
||
}
|
||
|
||
// --------------------------------------------------
|
||
// 4. ΪÉÐδƥÅäµÄ match2 ·ÖÅäжÎ
|
||
// --------------------------------------------------
|
||
if (current2Valid && !currentMatch2Assigned)
|
||
{
|
||
if (!previousSlot1Used)
|
||
{
|
||
m1_ridL = a_lineMatch.match2.rid_L;
|
||
m1_ridR = a_lineMatch.match2.rid_R;
|
||
|
||
match_seg_id_1 = next_seg_id;
|
||
next_seg_id++;
|
||
a_lineMatch.match2.match_seg_id = match_seg_id_1;
|
||
//previousSlot1Used = true;
|
||
//currentMatch2Assigned = true;
|
||
}
|
||
else if (!previousSlot2Used)
|
||
{
|
||
m2_ridL = a_lineMatch.match2.rid_L;
|
||
m2_ridR = a_lineMatch.match2.rid_R;
|
||
|
||
match_seg_id_2 = next_seg_id;
|
||
next_seg_id++;
|
||
a_lineMatch.match2.match_seg_id = match_seg_id_2;
|
||
//previousSlot2Used = true;
|
||
//currentMatch2Assigned = true;
|
||
}
|
||
}
|
||
|
||
// ÿÐÐͳһд»ØÒ»´Î
|
||
matchInfo_buf[i] = a_lineMatch;
|
||
}
|
||
|
||
//output
|
||
SLRMatchOutput outWord =
|
||
{
|
||
0, {0.0f, 0, 0, 0, 0, 0 }, {0.0f, 0, 0, 0, 0, 0 },
|
||
0, {0.0f, 0, 0, 0, 0, 0 }, {0.0f, 0, 0, 0, 0, 0 },
|
||
{0, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
|
||
0
|
||
};
|
||
|
||
//1st: frameNo
|
||
UintFloat outTmp;
|
||
outTmp.unData = roiL_FrmNo;
|
||
outWord.left_pt1.x = outTmp.fdata;
|
||
outTmp.unData = roiL_TimeStamp;
|
||
outWord.left_pt2.x = outTmp.fdata;
|
||
outTmp.unData = roiL_encInfo;
|
||
outWord.right_pt1.x = outTmp.fdata;
|
||
outTmp.unData = minY | (maxY << 16);
|
||
outWord.right_pt2.x = outTmp.fdata;
|
||
outWord.syncWord = 0b10;
|
||
outMatchPt.write(outWord);
|
||
for(uint16_t i = minY; i <= maxY; i ++)
|
||
{
|
||
#pragma HLS LOOP_TRIPCOUNT min=3072 max=3072
|
||
#pragma HLS PIPELINE II=1
|
||
const ap_uint<42> stateL = leftState[i];
|
||
const ap_uint<40> tagL = stateL.range(41, 2);
|
||
const ap_uint<2> numL = stateL.range(1, 0);
|
||
const ap_uint<42> stateR = rightState[i];
|
||
const ap_uint<40> tagR = stateR.range(41, 2);
|
||
const ap_uint<2> numR = stateR.range(1, 0);
|
||
const bool validL = (tagL == roiL_frameTag);
|
||
const bool validR = (tagR == roiR_frameTag);
|
||
|
||
outWord.left_ptNum = 0;
|
||
outWord.left_pt1 = nullRowData.pt1;
|
||
outWord.left_pt2 = nullRowData.pt2;
|
||
if (validL && numL > 0)
|
||
{
|
||
outWord.left_ptNum = numL;
|
||
outWord.left_pt1 = left_buf[i].pt1;
|
||
|
||
if (numL == 2)
|
||
outWord.left_pt2 = left_buf[i].pt2;
|
||
}
|
||
|
||
outWord.right_ptNum = 0;
|
||
outWord.right_pt1 = nullRowData.pt1;
|
||
outWord.right_pt2 = nullRowData.pt2;
|
||
if (validR && numR > 0)
|
||
{
|
||
outWord.right_ptNum = numR;
|
||
outWord.right_pt1 = right_buf[i].pt1;
|
||
|
||
if (numR == 2)
|
||
outWord.right_pt2 = right_buf[i].pt2;
|
||
}
|
||
|
||
outWord.matchInfo = matchInfo_buf[i];
|
||
if(i == maxY)
|
||
outWord.syncWord = 0b11;
|
||
else
|
||
outWord.syncWord = 0b00;
|
||
outMatchPt.write(outWord);
|
||
}
|
||
}
|