96 lines
3.6 KiB
C++
96 lines
3.6 KiB
C++
#include "AlgoParamConverter.h"
|
|
#include "VrLog.h"
|
|
#include <cstring>
|
|
|
|
namespace AlgoParamConverter
|
|
{
|
|
|
|
SSX_rodParam ToAlgoParam(const VrRodParam& param)
|
|
{
|
|
SSX_rodParam algo;
|
|
algo.diameter = param.rodDiameter;
|
|
algo.len = param.rodLen;
|
|
return algo;
|
|
}
|
|
|
|
SSG_cornerParam ToAlgoParam(const VrCornerParam& param)
|
|
{
|
|
SSG_cornerParam algo;
|
|
algo.minEndingGap = param.minEndingGap;
|
|
algo.minEndingGap_z = param.minEndingGap_z;
|
|
algo.scale = param.scale;
|
|
algo.cornerTh = param.cornerTh;
|
|
algo.jumpCornerTh_1 = param.jumpCornerTh_1;
|
|
algo.jumpCornerTh_2 = param.jumpCornerTh_2;
|
|
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 && cameraCalibParam->isCalibrated) {
|
|
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 SSX_rodParam& rodParam,
|
|
const SSG_cornerParam& cornerParam,
|
|
const SSG_outlierFilterParam& filterParam,
|
|
const SSG_treeGrowParam& growParam,
|
|
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 Rod: diameter=%.1f, len=%.1f\n",
|
|
logTag.c_str(), rodParam.diameter, rodParam.len);
|
|
|
|
LOG_INFO("%s Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
|
|
logTag.c_str(), cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap,
|
|
cornerParam.minEndingGap_z, cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);
|
|
|
|
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);
|
|
}
|
|
|
|
} // namespace AlgoParamConverter
|