443 lines
12 KiB
C++
443 lines
12 KiB
C++
|
#include <vector>
|
|||
|
|
#include "SG_baseDataType.h"
|
|||
|
|
#include "SG_baseAlgo_Export.h"
|
|||
|
|
#include "BQ_assemblyPosition_Export.h"
|
|||
|
|
#include <opencv2/opencv.hpp>
|
|||
|
|
#include <limits>
|
|||
|
|
|
|||
|
|
//version 1.0.0 : base version release to customer
|
|||
|
|
std::string m_strVersion = "1.0.0";
|
|||
|
|
const char* wd_BQAssemblyPositionVersion(void)
|
|||
|
|
{
|
|||
|
|
return m_strVersion.c_str();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
|
|||
|
|
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
|||
|
|
{
|
|||
|
|
return sg_getPlaneCalibPara2(scanLines);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
void sx_BQ_lineDataR(
|
|||
|
|
std::vector< SVzNL3DPosition>& a_line,
|
|||
|
|
const double* camPoseR,
|
|||
|
|
double groundH)
|
|||
|
|
{
|
|||
|
|
lineDataRT_vector(a_line, camPoseR, groundH);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
|
|||
|
|
{
|
|||
|
|
int ptNum = 0;
|
|||
|
|
for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
if ((abs(lineData[i].pt3D.z) > 1e-4) ||
|
|||
|
|
(abs(lineData[i].pt3D.x > 1e-4)) ||
|
|||
|
|
(abs(lineData[i].pt3D.y > 1e-4)))
|
|||
|
|
ptNum++;
|
|||
|
|
}
|
|||
|
|
return ptNum;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
|
|||
|
|
return a.angle < b.angle;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
|||
|
|
const SSG_cornerParam cornerPara,
|
|||
|
|
const SSG_outlierFilterParam filterParam,
|
|||
|
|
SSG_treeGrowParam growParam,
|
|||
|
|
SSG_planeCalibPara groundCalibPara,
|
|||
|
|
SSX_BQAssemblyPara assemblyParam,
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
SSX_BQAssemblyInfo assemblyPose;
|
|||
|
|
memset(&assemblyPose, 0, sizeof(SSX_BQAssemblyInfo));
|
|||
|
|
int lineNum = (int)scanLines.size();
|
|||
|
|
if (lineNum == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_3D_DATA_NULL;
|
|||
|
|
return assemblyPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Ŀհ<C4BF>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
|
|||
|
|
int validStartLine = -1;
|
|||
|
|
for (int i = 0; i < lineNum; i++)
|
|||
|
|
{
|
|||
|
|
int linePtNum = _counterLinePtNum(scanLines[i]);
|
|||
|
|
if (linePtNum > 0)
|
|||
|
|
{
|
|||
|
|
validStartLine = i;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
int validEndLine = -1;
|
|||
|
|
for (int i = lineNum - 1; i >= 0; i--)
|
|||
|
|
{
|
|||
|
|
int linePtNum = _counterLinePtNum(scanLines[i]);
|
|||
|
|
if (linePtNum > 0)
|
|||
|
|
{
|
|||
|
|
validEndLine = i;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if ((validStartLine < 0) || (validEndLine < 0))
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_3D_DATA_NULL;
|
|||
|
|
return assemblyPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int linePtNum = (int)scanLines[0].size();
|
|||
|
|
bool isGridData = true;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
|
|||
|
|
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 250)
|
|||
|
|
int kkk = 1;
|
|||
|
|
|
|||
|
|
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
|||
|
|
if (linePtNum != (int)lineData.size())
|
|||
|
|
isGridData = false;
|
|||
|
|
|
|||
|
|
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
|
|||
|
|
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
|||
|
|
|
|||
|
|
std::vector<SSG_basicFeature1D> line_features;
|
|||
|
|
int dataSize = (int)lineData.size();
|
|||
|
|
sg_getLineCornerFeature_BQ(
|
|||
|
|
&lineData[0],
|
|||
|
|
dataSize,
|
|||
|
|
line,
|
|||
|
|
groundCalibPara.planeHeight,
|
|||
|
|
cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
|
|||
|
|
line_features);
|
|||
|
|
jumpFeatures_v_raw.push_back(line_features);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
|||
|
|
return assemblyPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
|
|||
|
|
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
|||
|
|
hLines_raw.resize(linePtNum);
|
|||
|
|
for (int i = 0; i < linePtNum; i++)
|
|||
|
|
hLines_raw[i].resize(lineNum);
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
|||
|
|
hLines_raw[j][line] = scanLines[line][j];
|
|||
|
|
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
|||
|
|
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
|
|||
|
|
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
|
|||
|
|
int lineNum_h_raw = (int)hLines_raw.size();
|
|||
|
|
for (int line = 0; line < lineNum_h_raw; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 416)
|
|||
|
|
int kkk = 1;
|
|||
|
|
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
|
|||
|
|
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
|
|||
|
|
int ptNum = (int)lineData.size();
|
|||
|
|
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
|||
|
|
|
|||
|
|
std::vector<SSG_basicFeature1D> line_features;
|
|||
|
|
int dataSize = (int)lineData.size();
|
|||
|
|
sg_getLineCornerFeature_BQ(
|
|||
|
|
&hLines_raw[line][0],
|
|||
|
|
dataSize,
|
|||
|
|
line,
|
|||
|
|
groundCalibPara.planeHeight,
|
|||
|
|
cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
|
|||
|
|
line_features);
|
|||
|
|
jumpFeatures_h_raw.push_back(line_features);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<SSG_featureTree> v_trees;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
bool isLastLine = false;
|
|||
|
|
if (line == lineNum - 1)
|
|||
|
|
isLastLine = true;
|
|||
|
|
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
|
|||
|
|
if (a_lineJumpFeature.size() > 0)
|
|||
|
|
int kkk = 1;
|
|||
|
|
if (line == 202)
|
|||
|
|
int kkk = 1;
|
|||
|
|
sg_lineFeaturesGrowing(
|
|||
|
|
line,
|
|||
|
|
isLastLine,
|
|||
|
|
a_lineJumpFeature,
|
|||
|
|
v_trees,
|
|||
|
|
growParam);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<SSG_featureTree> h_trees;
|
|||
|
|
for (int line = 0; line < lineNum_h_raw; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 650)
|
|||
|
|
int kkk = 1;
|
|||
|
|
bool isLastLine = false;
|
|||
|
|
if (line == lineNum_h_raw - 1)
|
|||
|
|
isLastLine = true;
|
|||
|
|
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
|
|||
|
|
sg_lineFeaturesGrowing(
|
|||
|
|
line,
|
|||
|
|
isLastLine,
|
|||
|
|
a_lineJumpFeature,
|
|||
|
|
h_trees,
|
|||
|
|
growParam);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<SWD_polarPt> polarPoints;
|
|||
|
|
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
SSG_featureTree* a_vTree = &v_trees[i];
|
|||
|
|
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
|
|||
|
|
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
|
|||
|
|
{
|
|||
|
|
int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
|
|||
|
|
int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
|
|||
|
|
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
|||
|
|
{
|
|||
|
|
SWD_polarPt a_polarPt;
|
|||
|
|
a_polarPt.lineIdx = lineIdx;
|
|||
|
|
a_polarPt.ptIdx = ptIdx;
|
|||
|
|
a_polarPt.R = 0;
|
|||
|
|
a_polarPt.angle = 0;
|
|||
|
|
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
|||
|
|
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
|||
|
|
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
|||
|
|
polarPoints.push_back(a_polarPt);
|
|||
|
|
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
SSG_featureTree* a_hTree = &h_trees[i];
|
|||
|
|
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
|
|||
|
|
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
|
|||
|
|
{
|
|||
|
|
int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
|
|||
|
|
int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
|
|||
|
|
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
|||
|
|
{
|
|||
|
|
SWD_polarPt a_polarPt;
|
|||
|
|
a_polarPt.lineIdx = lineIdx;
|
|||
|
|
a_polarPt.ptIdx = ptIdx;
|
|||
|
|
a_polarPt.cptIndex = -1;
|
|||
|
|
a_polarPt.R = 0;
|
|||
|
|
a_polarPt.angle = 0;
|
|||
|
|
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
|||
|
|
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
|||
|
|
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
|||
|
|
polarPoints.push_back(a_polarPt);
|
|||
|
|
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>㼸<EFBFBD><E3BCB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int contourPtSize = (int)polarPoints.size();
|
|||
|
|
if (contourPtSize == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SX_ERR_ZERO_CONTOUR_PT;
|
|||
|
|
return assemblyPose;
|
|||
|
|
}
|
|||
|
|
double center_x = 0;
|
|||
|
|
double center_y = 0;
|
|||
|
|
for (int pi = 0; pi < contourPtSize; pi++)
|
|||
|
|
{
|
|||
|
|
center_x += polarPoints[pi].x;
|
|||
|
|
center_y += polarPoints[pi].y;
|
|||
|
|
}
|
|||
|
|
center_x = center_x / (double)contourPtSize;
|
|||
|
|
center_y = center_y / (double)contourPtSize;
|
|||
|
|
//<2F><><EFBFBD>㼫<EFBFBD><E3BCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52>Theta
|
|||
|
|
for (int pi = 0; pi < contourPtSize; pi++)
|
|||
|
|
{
|
|||
|
|
double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
|
|||
|
|
angle = (angle / PI) * 180 + 180.0;
|
|||
|
|
double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
|
|||
|
|
polarPoints[pi].R = R;
|
|||
|
|
polarPoints[pi].angle = angle;
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD>Ƕȴ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
|
|||
|
|
std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
|
|||
|
|
for (int pi = 0; pi < contourPtSize; pi++)
|
|||
|
|
polarPoints[pi].cptIndex = pi; // index
|
|||
|
|
|
|||
|
|
//<2F><>ȡR<C8A1><52>ֵ<EFBFBD><D6B5>
|
|||
|
|
double minR = -1, maxR = -1; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>з<EFBFBD>֧<EFBFBD><D6A7>minR<6E><52>maxR<78><52><EFBFBD><EFBFBD>Сʱ<D0A1><CAB1>ΪԲ<CEAA>λ<EFBFBD>8<EFBFBD><38><EFBFBD>Σ<EFBFBD>û<EFBFBD>з<EFBFBD>֧
|
|||
|
|
int minRPos = -1;
|
|||
|
|
std::vector<SWD_polarPt> polarRPeakPts;
|
|||
|
|
int winSize = contourPtSize / 36; //+-10<31>ȷ<EFBFBD>Χ
|
|||
|
|
if (winSize < 5)
|
|||
|
|
winSize = 5;
|
|||
|
|
for (int pi = 0; pi < contourPtSize; pi++)
|
|||
|
|
{
|
|||
|
|
double currR = polarPoints[pi].R;
|
|||
|
|
if (minR < 0)
|
|||
|
|
{
|
|||
|
|
minR = currR;
|
|||
|
|
maxR = currR;
|
|||
|
|
minRPos = pi;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
minRPos = minR > currR ? pi : minRPos;
|
|||
|
|
minR = minR > currR ? currR : minR;
|
|||
|
|
maxR = maxR < currR ? currR : maxR;
|
|||
|
|
}
|
|||
|
|
bool isPeak = true;
|
|||
|
|
for (int k = -winSize; k <= winSize; k++)
|
|||
|
|
{
|
|||
|
|
int idx = (pi + k + contourPtSize) % contourPtSize; //Ͳ<>νṹ
|
|||
|
|
if (polarPoints[idx].R > currR)
|
|||
|
|
{
|
|||
|
|
isPeak = false;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (true == isPeak)
|
|||
|
|
polarRPeakPts.push_back(polarPoints[pi]);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
double ratio_MaxMin = maxR / minR;
|
|||
|
|
bool hasBranch = ratio_MaxMin < 1.25 ? false : true;
|
|||
|
|
|
|||
|
|
std::vector<SWD_polarPt> validPolarRPeakPts;
|
|||
|
|
std::vector<SWD_polarPeakInfo> polarPeakInfo;
|
|||
|
|
int pkId = 0;
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>εļ<CEB5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>ƣ<EFBFBD>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>γɼ<CEB3>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>L=ֱ<>߶γ<DFB6><CEB3>ȹ<EFBFBD><C8B9>ɵ<EFBFBD><C9B5>Ž<EFBFBD><C5BD>ж<EFBFBD>
|
|||
|
|
double arcAngleChkLen = 100; //<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>Žǵij߶<C4B3>
|
|||
|
|
for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
int ptidx = polarRPeakPts[i].cptIndex;
|
|||
|
|
double px, py, pz;
|
|||
|
|
px = polarRPeakPts[i].x;
|
|||
|
|
py = polarRPeakPts[i].y;
|
|||
|
|
|
|||
|
|
int LL1 = -1;
|
|||
|
|
int halfLL1 = -1;
|
|||
|
|
for (int j = ptidx - 1; j > -contourPtSize; j--)
|
|||
|
|
{
|
|||
|
|
int idx = (j + contourPtSize) % contourPtSize; //Ͳ<>νṹ
|
|||
|
|
double cx = polarPoints[idx].x;
|
|||
|
|
double cy = polarPoints[idx].y;
|
|||
|
|
double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
|
|||
|
|
if (len < arcAngleChkLen)
|
|||
|
|
halfLL1 = idx;
|
|||
|
|
if (len > (assemblyParam.lineLen))
|
|||
|
|
{
|
|||
|
|
LL1 = idx;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int LL2 = -1;
|
|||
|
|
int halfLL2 = -1;
|
|||
|
|
for (int j = ptidx + 1; j < contourPtSize * 2; j++)
|
|||
|
|
{
|
|||
|
|
int idx = j % contourPtSize; //Ͳ<>νṹ
|
|||
|
|
double cx = polarPoints[idx].x;
|
|||
|
|
double cy = polarPoints[idx].y;
|
|||
|
|
double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
|
|||
|
|
if (len < arcAngleChkLen)
|
|||
|
|
halfLL2 = idx;
|
|||
|
|
if (len > (assemblyParam.lineLen))
|
|||
|
|
{
|
|||
|
|
LL2 = idx;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if ((LL1 >= 0) && (LL2 >= 0))
|
|||
|
|
{
|
|||
|
|
double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
|
|||
|
|
double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
|
|||
|
|
double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
|
|||
|
|
pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
|
|||
|
|
double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
|
|||
|
|
double theta = acos(cosTheta) * 180.0 / PI;
|
|||
|
|
if (theta < 150)
|
|||
|
|
{
|
|||
|
|
SWD_polarPeakInfo a_pkInfo;
|
|||
|
|
a_pkInfo.cptIndex = ptidx;
|
|||
|
|
a_pkInfo.L1_ptIndex = LL1;
|
|||
|
|
a_pkInfo.L2_ptIndex = LL2;
|
|||
|
|
a_pkInfo.cornerAngle = theta;
|
|||
|
|
a_pkInfo.cornerDir = 0;
|
|||
|
|
polarRPeakPts[i].cptIndex = ptidx;
|
|||
|
|
polarRPeakPts[i].pkId = pkId;
|
|||
|
|
pkId++;
|
|||
|
|
validPolarRPeakPts.push_back(polarRPeakPts[i]);
|
|||
|
|
polarPeakInfo.push_back(a_pkInfo);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ι<EFBFBD><CEB9><EFBFBD>
|
|||
|
|
if (validPolarRPeakPts.size() != 4)
|
|||
|
|
{
|
|||
|
|
*errCode = SX_ERR_INVLID_RPEAK_NUM;
|
|||
|
|
return assemblyPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD>ᣬ<EFBFBD>̱<EFBFBD><CCB1><EFBFBD>Y<EFBFBD>ᣬ<EFBFBD><E1A3AC><EFBFBD><EFBFBD>ΪZ<CEAA>ᣬ<EFBFBD><E1A3AC><EFBFBD>ĵ<EFBFBD>ΪO<CEAA><4F>
|
|||
|
|
std::vector<SVzNL3DPoint> sidePts_1;
|
|||
|
|
std::vector<SVzNL3DPoint> sidePts_2;
|
|||
|
|
std::vector<SVzNL3DPoint> sidePts_3;
|
|||
|
|
std::vector<SVzNL3DPoint> sidePts_4;
|
|||
|
|
return assemblyPose;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>Mark<72><6B><EFBFBD>㹤<EFBFBD><E3B9A4>λ<EFBFBD>ú<EFBFBD><C3BA><EFBFBD>̬
|
|||
|
|
SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark(
|
|||
|
|
SSX_BQAssemblyInfo originPos,
|
|||
|
|
std::vector<cv::Point3d> originMarkPos,
|
|||
|
|
std::vector<cv::Point3d> currMarkPos,
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
SSX_BQAssemblyInfo resultPos;
|
|||
|
|
memset(&resultPos, 0, sizeof(SSX_BQAssemblyInfo));
|
|||
|
|
if ((originMarkPos.size() < 3) || (currMarkPos.size() < 3))
|
|||
|
|
{
|
|||
|
|
*errCode = SX_ERR_INVLID_MARK_NUM;
|
|||
|
|
return resultPos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
cv::Mat R, T; //<2F><>תƽ<D7AA><C6BD>
|
|||
|
|
cv::Point3d C_origin, C_curr; //<2F><><EFBFBD><EFBFBD>
|
|||
|
|
caculateRT(originMarkPos, currMarkPos, R, T, C_origin, C_curr);
|
|||
|
|
|
|||
|
|
// 5. <20><><EFBFBD><EFBFBD>Pn<50><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
pointRT(R, T, C_origin, C_curr, originPos.O, resultPos.O);
|
|||
|
|
pointRT(R, T, C_origin, C_curr, originPos.X, resultPos.X);
|
|||
|
|
pointRT(R, T, C_origin, C_curr, originPos.Y, resultPos.Y);
|
|||
|
|
pointRT(R, T, C_origin, C_curr, originPos.Z, resultPos.Z);
|
|||
|
|
return resultPos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|