115 lines
4.7 KiB
C++
115 lines
4.7 KiB
C++
#include "AlgoParamConverter.h"
|
|
#include "VrLog.h"
|
|
#include <cstring>
|
|
|
|
namespace AlgoParamConverter
|
|
{
|
|
|
|
WD_workpieceHoleParam ToAlgoParam(const VrWorkpieceHoleParam& param)
|
|
{
|
|
WD_workpieceHoleParam algo;
|
|
algo.workpieceType = param.workpieceType;
|
|
algo.holeDiameter = param.holeDiameter;
|
|
algo.holeDist_L = param.holeDist_L;
|
|
algo.holeDist_W = param.holeDist_W;
|
|
algo.xLen = param.xLen;
|
|
algo.yLen = param.yLen;
|
|
algo.H = param.H;
|
|
return algo;
|
|
}
|
|
|
|
SSG_lineSegParam ToAlgoParam(const VrLineSegParam& param)
|
|
{
|
|
SSG_lineSegParam algo;
|
|
algo.distScale = param.distScale;
|
|
algo.segGapTh_y = param.segGapTh_y;
|
|
algo.segGapTh_z = param.segGapTh_z;
|
|
return algo;
|
|
}
|
|
|
|
SSG_outlierFilterParam ToAlgoParam(const VrOutlierFilterParam& param)
|
|
{
|
|
SSG_outlierFilterParam algo;
|
|
algo.continuityTh = param.continuityTh;
|
|
algo.outlierTh = param.outlierTh;
|
|
return algo;
|
|
}
|
|
|
|
SSG_treeGrowParam ToAlgoParam(const VrTreeGrowParam& param)
|
|
{
|
|
SSG_treeGrowParam algo;
|
|
algo.yDeviation_max = param.yDeviation_max;
|
|
algo.zDeviation_max = param.zDeviation_max;
|
|
algo.maxLineSkipNum = param.maxLineSkipNum;
|
|
algo.maxSkipDistance = param.maxSkipDistance;
|
|
algo.minLTypeTreeLen = param.minLTypeTreeLen;
|
|
algo.minVTypeTreeLen = param.minVTypeTreeLen;
|
|
return algo;
|
|
}
|
|
|
|
SSG_planeCalibPara ToAlgoPlaneCalibParam(const VrCameraPlaneCalibParam* cameraCalibParam)
|
|
{
|
|
SSG_planeCalibPara algo;
|
|
if (cameraCalibParam) {
|
|
memcpy(algo.planeCalib, cameraCalibParam->planeCalib, sizeof(double) * 9);
|
|
memcpy(algo.invRMatrix, cameraCalibParam->invRMatrix, sizeof(double) * 9);
|
|
algo.planeHeight = cameraCalibParam->planeHeight;
|
|
} else {
|
|
// 使用默认单位矩阵
|
|
double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
|
memcpy(algo.planeCalib, identity, sizeof(double) * 9);
|
|
memcpy(algo.invRMatrix, identity, sizeof(double) * 9);
|
|
algo.planeHeight = -1.0;
|
|
}
|
|
return algo;
|
|
}
|
|
|
|
void LogAlgoParams(const std::string& logTag,
|
|
const WD_workpieceHoleParam& workpiecePara,
|
|
const SSG_lineSegParam& lineSegPara,
|
|
const SSG_outlierFilterParam& filterParam,
|
|
const SSG_treeGrowParam& growParam,
|
|
const SSG_planeCalibPara& groundCalibPara,
|
|
const double clibMatrix[16])
|
|
{
|
|
LOG_INFO("%s clibMatrix: \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f]\n",
|
|
logTag.c_str(),
|
|
clibMatrix[0], clibMatrix[1], clibMatrix[2], clibMatrix[3],
|
|
clibMatrix[4], clibMatrix[5], clibMatrix[6], clibMatrix[7],
|
|
clibMatrix[8], clibMatrix[9], clibMatrix[10], clibMatrix[11],
|
|
clibMatrix[12], clibMatrix[13], clibMatrix[14], clibMatrix[15]);
|
|
|
|
LOG_INFO("%s WorkpieceHole: type=%d, holeDiameter=%.1f, holeDist_L=%.1f, holeDist_W=%.1f, xLen=%.1f, yLen=%.1f, H=%.1f\n",
|
|
logTag.c_str(),
|
|
workpiecePara.workpieceType, workpiecePara.holeDiameter, workpiecePara.holeDist_L, workpiecePara.holeDist_W,
|
|
workpiecePara.xLen, workpiecePara.yLen, workpiecePara.H);
|
|
|
|
LOG_INFO("%s LineSeg: distScale=%.1f, segGapTh_y=%.1f, segGapTh_z=%.1f\n",
|
|
logTag.c_str(),
|
|
lineSegPara.distScale, lineSegPara.segGapTh_y, lineSegPara.segGapTh_z);
|
|
|
|
LOG_INFO("%s Tree Grow: yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
|
|
logTag.c_str(),
|
|
growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum,
|
|
growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
|
|
|
LOG_INFO("%s Filter: continuityTh=%.1f, outlierTh=%.1f\n",
|
|
logTag.c_str(),
|
|
filterParam.continuityTh, filterParam.outlierTh);
|
|
|
|
LOG_INFO("%s Plane height: %.3f\n", logTag.c_str(), groundCalibPara.planeHeight);
|
|
LOG_INFO("%s Plane calibration matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
|
|
logTag.c_str(),
|
|
groundCalibPara.planeCalib[0], groundCalibPara.planeCalib[1], groundCalibPara.planeCalib[2],
|
|
groundCalibPara.planeCalib[3], groundCalibPara.planeCalib[4], groundCalibPara.planeCalib[5],
|
|
groundCalibPara.planeCalib[6], groundCalibPara.planeCalib[7], groundCalibPara.planeCalib[8]);
|
|
|
|
LOG_INFO("%s Plane invRMatrix matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
|
|
logTag.c_str(),
|
|
groundCalibPara.invRMatrix[0], groundCalibPara.invRMatrix[1], groundCalibPara.invRMatrix[2],
|
|
groundCalibPara.invRMatrix[3], groundCalibPara.invRMatrix[4], groundCalibPara.invRMatrix[5],
|
|
groundCalibPara.invRMatrix[6], groundCalibPara.invRMatrix[7], groundCalibPara.invRMatrix[8]);
|
|
}
|
|
|
|
} // namespace AlgoParamConverter
|