659 lines
26 KiB
C++
659 lines
26 KiB
C++
#include "DetectPresenter.h"
|
|
#include "IHandEyeCalib.h"
|
|
#include "SG_baseAlgo_Export.h"
|
|
#include "SG_errCode.h"
|
|
|
|
#include <cmath>
|
|
#include <algorithm>
|
|
#include <cstring>
|
|
#include <exception>
|
|
#include <memory>
|
|
|
|
namespace {
|
|
|
|
const int kSupportedWorkpieceClassCount = 3;
|
|
const double kDuplicate2DIouThreshold = 0.5;
|
|
|
|
const char* WorkpieceModelLogName()
|
|
{
|
|
return "WorkpiceTaperedModel";
|
|
}
|
|
|
|
char WorkpieceClassName(int classId)
|
|
{
|
|
if (classId >= 0 && classId < 26) {
|
|
return static_cast<char>('a' + classId);
|
|
}
|
|
return '?';
|
|
}
|
|
|
|
double RectArea(const WorkpiecePositionModelAdapter::Rect& rect)
|
|
{
|
|
const double width = rect.right - rect.left;
|
|
const double height = rect.bottom - rect.top;
|
|
if (width <= 0.0 || height <= 0.0) {
|
|
return 0.0;
|
|
}
|
|
return width * height;
|
|
}
|
|
|
|
double RectIou(const WorkpiecePositionModelAdapter::Rect& lhs,
|
|
const WorkpiecePositionModelAdapter::Rect& rhs)
|
|
{
|
|
const double interLeft = std::max(lhs.left, rhs.left);
|
|
const double interTop = std::max(lhs.top, rhs.top);
|
|
const double interRight = std::min(lhs.right, rhs.right);
|
|
const double interBottom = std::min(lhs.bottom, rhs.bottom);
|
|
WorkpiecePositionModelAdapter::Rect intersection;
|
|
intersection.left = interLeft;
|
|
intersection.top = interTop;
|
|
intersection.right = interRight;
|
|
intersection.bottom = interBottom;
|
|
const double interArea = RectArea(intersection);
|
|
if (interArea <= 0.0) {
|
|
return 0.0;
|
|
}
|
|
|
|
const double unionArea = RectArea(lhs) + RectArea(rhs) - interArea;
|
|
if (unionArea <= 0.0) {
|
|
return 0.0;
|
|
}
|
|
return interArea / unionArea;
|
|
}
|
|
|
|
void FilterDuplicate2DDetections(
|
|
std::vector<WorkpiecePositionModelAdapter::Detection>& detections)
|
|
{
|
|
if (detections.size() < 2) {
|
|
return;
|
|
}
|
|
|
|
std::stable_sort(detections.begin(), detections.end(),
|
|
[](const WorkpiecePositionModelAdapter::Detection& lhs,
|
|
const WorkpiecePositionModelAdapter::Detection& rhs) {
|
|
return lhs.confidence > rhs.confidence;
|
|
});
|
|
|
|
std::vector<WorkpiecePositionModelAdapter::Detection> filtered;
|
|
filtered.reserve(detections.size());
|
|
for (const auto& detection : detections) {
|
|
bool duplicated = false;
|
|
for (const auto& kept : filtered) {
|
|
if (RectIou(detection.objectRoi, kept.objectRoi) >= kDuplicate2DIouThreshold) {
|
|
duplicated = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!duplicated) {
|
|
filtered.push_back(detection);
|
|
}
|
|
}
|
|
|
|
detections.swap(filtered);
|
|
}
|
|
|
|
HECEulerOrder ToHandEyeEulerOrder(int eulerOrder)
|
|
{
|
|
switch (eulerOrder) {
|
|
case 10: return HECEulerOrder::XYZ;
|
|
case 11: return HECEulerOrder::ZYX;
|
|
case 12: return HECEulerOrder::ZXY;
|
|
case 13: return HECEulerOrder::YXZ;
|
|
case 14: return HECEulerOrder::YZX;
|
|
case 15: return HECEulerOrder::XZY;
|
|
default:
|
|
LOG_WARNING("Unsupported tool euler order %d, fallback to 11 (ZYX)\n", eulerOrder);
|
|
return HECEulerOrder::ZYX;
|
|
}
|
|
}
|
|
|
|
void ApplyToolRotationToEyeAxes(IHandEyeCalib& handEyeCalib,
|
|
HECEulerOrder eulerOrder,
|
|
const VrToolParam& toolParam,
|
|
std::vector<HECPoint3D>& axes)
|
|
{
|
|
if (axes.size() != 3 ||
|
|
(std::fabs(toolParam.rotX) < 1e-9 &&
|
|
std::fabs(toolParam.rotY) < 1e-9 &&
|
|
std::fabs(toolParam.rotZ) < 1e-9)) {
|
|
return;
|
|
}
|
|
|
|
HECRotationMatrix toolRotation;
|
|
handEyeCalib.EulerToRotationMatrix(
|
|
HECEulerAngles::fromDegrees(toolParam.rotX, toolParam.rotY, toolParam.rotZ),
|
|
eulerOrder,
|
|
toolRotation);
|
|
|
|
const std::vector<HECPoint3D> originalAxes = axes;
|
|
for (int column = 0; column < 3; ++column) {
|
|
axes[column] = (originalAxes[0] * toolRotation.at(0, column)
|
|
+ originalAxes[1] * toolRotation.at(1, column)
|
|
+ originalAxes[2] * toolRotation.at(2, column)).normalized();
|
|
}
|
|
}
|
|
|
|
void ApplyConfiguredAxisInversion(int dirVectorInvert, std::vector<HECPoint3D>& axes)
|
|
{
|
|
if (axes.size() != 3) {
|
|
return;
|
|
}
|
|
|
|
switch (dirVectorInvert) {
|
|
case DIR_INVERT_XY:
|
|
axes[0] = axes[0] * -1.0;
|
|
axes[1] = axes[1] * -1.0;
|
|
break;
|
|
case DIR_INVERT_XZ:
|
|
axes[0] = axes[0] * -1.0;
|
|
axes[2] = axes[2] * -1.0;
|
|
break;
|
|
case DIR_INVERT_YZ:
|
|
axes[1] = axes[1] * -1.0;
|
|
axes[2] = axes[2] * -1.0;
|
|
break;
|
|
case DIR_INVERT_NONE:
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
SSG_planeCalibPara ToGroundCalibParam(const VrCameraPlaneCalibParam* cameraCalibParam)
|
|
{
|
|
SSG_planeCalibPara groundCalibPara;
|
|
const double identity[9] = {1.0, 0.0, 0.0,
|
|
0.0, 1.0, 0.0,
|
|
0.0, 0.0, 1.0};
|
|
if (cameraCalibParam) {
|
|
std::memcpy(groundCalibPara.planeCalib,
|
|
cameraCalibParam->planeCalib,
|
|
sizeof(groundCalibPara.planeCalib));
|
|
std::memcpy(groundCalibPara.invRMatrix,
|
|
cameraCalibParam->invRMatrix,
|
|
sizeof(groundCalibPara.invRMatrix));
|
|
groundCalibPara.planeHeight = cameraCalibParam->planeHeight;
|
|
} else {
|
|
std::memcpy(groundCalibPara.planeCalib, identity, sizeof(identity));
|
|
std::memcpy(groundCalibPara.invRMatrix, identity, sizeof(identity));
|
|
groundCalibPara.planeHeight = -1.0;
|
|
}
|
|
return groundCalibPara;
|
|
}
|
|
|
|
WorkpiecePoint3D ToResultPoint(const HECPoint3D& point)
|
|
{
|
|
return WorkpiecePoint3D(point.x, point.y, point.z);
|
|
}
|
|
|
|
HECPoint3D ToHandEyePoint(const SVzNL3DPoint& point)
|
|
{
|
|
return HECPoint3D(point.x, point.y, point.z);
|
|
}
|
|
|
|
SVzNL3DPoint ToVzPoint(const double values[3])
|
|
{
|
|
SVzNL3DPoint point;
|
|
point.x = values[0];
|
|
point.y = values[1];
|
|
point.z = values[2];
|
|
return point;
|
|
}
|
|
|
|
WD_HRM_BinInfo ToWdBinInfo(const VrBinParam& param)
|
|
{
|
|
WD_HRM_BinInfo binInfo;
|
|
std::memset(&binInfo, 0, sizeof(binInfo));
|
|
binInfo.length = param.length;
|
|
binInfo.width = param.width;
|
|
binInfo.binTopZ = param.binTopZ;
|
|
binInfo.center = ToVzPoint(param.center);
|
|
binInfo.bottomNormal = ToVzPoint(param.bottomNormal);
|
|
binInfo.x_dir = ToVzPoint(param.xDir);
|
|
binInfo.y_dir = ToVzPoint(param.yDir);
|
|
for (int i = 0; i < 4; ++i) {
|
|
binInfo.minRectVertex[i] = ToVzPoint(param.minRectVertex[i]);
|
|
}
|
|
return binInfo;
|
|
}
|
|
|
|
void LogTaperedAlgorithmInput(
|
|
const std::vector<std::vector<SVzNLPositionD>>& scanLines,
|
|
const std::vector<WD_objArea2D>& objROIs,
|
|
const SSG_planeCalibPara& groundCalibPara,
|
|
const VrAlgorithmParams& algorithmParams,
|
|
const double clibMatrix[16],
|
|
int eulerOrder,
|
|
int dirVectorInvert)
|
|
{
|
|
LOG_INFO("[Tapered Debug] input summary: lines=%zu\n", scanLines.size());
|
|
|
|
LOG_INFO("[Tapered Debug] params: eulerOrder=%d, dirVectorInvert=%d, "
|
|
"toolEulerOrder=%d, toolRot=(%.6f,%.6f,%.6f), "
|
|
"toolOffset=(%.6f,%.6f,%.6f) \n",
|
|
eulerOrder, dirVectorInvert,
|
|
algorithmParams.toolParam.eulerOrder,
|
|
algorithmParams.toolParam.rotX,
|
|
algorithmParams.toolParam.rotY,
|
|
algorithmParams.toolParam.rotZ,
|
|
algorithmParams.toolParam.offsetX,
|
|
algorithmParams.toolParam.offsetY,
|
|
algorithmParams.toolParam.offsetZ);
|
|
|
|
LOG_INFO("[Tapered Debug] planeCalib: planeHeight=%.6f\n", groundCalibPara.planeHeight);
|
|
LOG_INFO("[Tapered Debug] planeCalib=[%.9f %.9f %.9f; %.9f %.9f %.9f; %.9f %.9f %.9f]\n",
|
|
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("[Tapered Debug] invRMatrix=[%.9f %.9f %.9f; %.9f %.9f %.9f; %.9f %.9f %.9f]\n",
|
|
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]);
|
|
LOG_INFO("[Tapered Debug] handEye=[%.9f %.9f %.9f %.9f; %.9f %.9f %.9f %.9f; "
|
|
"%.9f %.9f %.9f %.9f; %.9f %.9f %.9f %.9f]\n",
|
|
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("[Tapered Debug] objROIs=%zu\n", objROIs.size());
|
|
for (size_t index = 0; index < objROIs.size(); ++index) {
|
|
const WD_objArea2D& roi = objROIs[index];
|
|
LOG_INFO("[Tapered Debug] objROI[%zu]: type=%d, score=%.6f, LTRB=(%.3f,%.3f,%.3f,%.3f)\n",
|
|
index, roi.workpieceType, roi.score2D,
|
|
roi.roi.left, roi.roi.top, roi.roi.right, roi.roi.bottom);
|
|
}
|
|
}
|
|
|
|
void LogRawTaperedOutputs(const std::vector<WD_workpieceInfo>& workpiecePositions)
|
|
{
|
|
for (size_t index = 0; index < workpiecePositions.size(); ++index) {
|
|
const WD_workpieceInfo& workpiece = workpiecePositions[index];
|
|
LOG_INFO("[Tapered Debug] rawOutput[%zu]: id=%d, type=%d, value=%.9f, "
|
|
"center=(%.9f,%.9f,%.9f), "
|
|
"x_dir=(%.9f,%.9f,%.9f), y_dir=(%.9f,%.9f,%.9f), z_dir=(%.9f,%.9f,%.9f)\n",
|
|
index, workpiece.id, workpiece.workpieceType, workpiece.value,
|
|
workpiece.center.x, workpiece.center.y, workpiece.center.z,
|
|
workpiece.x_dir.x, workpiece.x_dir.y, workpiece.x_dir.z,
|
|
workpiece.y_dir.x, workpiece.y_dir.y, workpiece.y_dir.z,
|
|
workpiece.z_dir.x, workpiece.z_dir.y, workpiece.z_dir.z);
|
|
}
|
|
|
|
}
|
|
|
|
} // namespace
|
|
|
|
DetectPresenter::DetectPresenter()
|
|
{
|
|
const char* version = wd_hybridPositioningVersion();
|
|
LOG_INFO("DetectPresenter Init hybrid positioning algorithm ver: %s\n",
|
|
version ? version : "unknown");
|
|
}
|
|
|
|
DetectPresenter::~DetectPresenter()
|
|
{
|
|
}
|
|
|
|
QString DetectPresenter::GetAlgoVersion()
|
|
{
|
|
const char* version = wd_hybridPositioningVersion();
|
|
return version ? QString::fromLocal8Bit(version) : QStringLiteral("未知");
|
|
}
|
|
|
|
int DetectPresenter::LocateWorkpiece2D(
|
|
const SVzNLImageData& leftImage,
|
|
const VrWorkpieceModelParam& modelParam,
|
|
const VrDebugParam& debugParam,
|
|
std::vector<WorkpiecePositionModelAdapter::Detection>& detections)
|
|
{
|
|
CVrTimeUtils modelTimer;
|
|
const WorkpiecePositionModelAdapter::Status modelStatus =
|
|
m_workpieceModel.Locate(leftImage, modelParam.minimumScore, detections);
|
|
LOG_INFO("%s: found %zu ROIs, status=%d runtime=%.3fms\n",
|
|
WorkpieceModelLogName(), detections.size(),
|
|
static_cast<int>(modelStatus), modelTimer.GetElapsedTimeInMilliSec());
|
|
if (modelStatus != WorkpiecePositionModelAdapter::Status::Ok) {
|
|
const QString errorMessage = m_workpieceModel.LastError();
|
|
LOG_ERROR("%s failed: %s\n",
|
|
WorkpieceModelLogName(), errorMessage.toStdString().c_str());
|
|
switch (modelStatus) {
|
|
case WorkpiecePositionModelAdapter::Status::RuntimeUnavailable:
|
|
return ERR_CODE(FUN_UNSUPPORT);
|
|
case WorkpiecePositionModelAdapter::Status::InferenceFailed:
|
|
return ERR_CODE(APP_ERR_EXEC);
|
|
case WorkpiecePositionModelAdapter::Status::OutOfMemory:
|
|
return ERR_CODE(DATA_ERR_MEM);
|
|
case WorkpiecePositionModelAdapter::Status::InvalidResult:
|
|
return ERR_CODE(DATA_ERR_INVALID);
|
|
case WorkpiecePositionModelAdapter::Status::InvalidArgument:
|
|
case WorkpiecePositionModelAdapter::Status::UnsupportedImageFormat:
|
|
default:
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|
}
|
|
}
|
|
if (detections.empty()) {
|
|
LOG_WARNING("%s returned no workpiece ROI\n", WorkpieceModelLogName());
|
|
return ERR_CODE(SX_ERR_ZERO_2D_OBJECTS);
|
|
}
|
|
|
|
const size_t rawDetectionCount = detections.size();
|
|
FilterDuplicate2DDetections(detections);
|
|
if (detections.size() != rawDetectionCount) {
|
|
LOG_INFO("%s duplicate ROI filter: raw=%zu, filtered=%zu, iouThreshold=%.2f\n",
|
|
WorkpieceModelLogName(), rawDetectionCount, detections.size(),
|
|
kDuplicate2DIouThreshold);
|
|
}
|
|
|
|
LOG_INFO("%s 2D model output results:\n", WorkpieceModelLogName());
|
|
for (size_t index = 0; index < detections.size(); ++index) {
|
|
const WorkpiecePositionModelAdapter::Detection& detection = detections[index];
|
|
const WorkpiecePositionModelAdapter::Rect& roi = detection.objectRoi;
|
|
LOG_INFO("obj_%zu: C=%c, Rect={ %.3f, %.3f, %.3f, %.3f }, Score=%.4f\n",
|
|
index + 1,
|
|
WorkpieceClassName(detection.classId),
|
|
roi.left,
|
|
roi.top,
|
|
roi.right - roi.left,
|
|
roi.bottom - roi.top,
|
|
detection.confidence);
|
|
}
|
|
|
|
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
|
for (size_t index = 0; index < detections.size(); ++index) {
|
|
const WorkpiecePositionModelAdapter::Detection& detection = detections[index];
|
|
LOG_INFO("[2D ROI %zu] class=%d "
|
|
"label=%s score=%.3f "
|
|
"objectLTRB=(%.1f,%.1f,%.1f,%.1f) mask=%s\n",
|
|
index, detection.classId, detection.label.toStdString().c_str(), detection.confidence,
|
|
detection.objectRoi.left, detection.objectRoi.top,
|
|
detection.objectRoi.right, detection.objectRoi.bottom,
|
|
detection.HasMask() ? "yes" : "no");
|
|
}
|
|
}
|
|
|
|
return SUCCESS;
|
|
}
|
|
|
|
int DetectPresenter::DetectStatorWorkpiecePosition(
|
|
int cameraIndex,
|
|
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
|
const std::vector<WorkpiecePositionModelAdapter::Detection>& detections,
|
|
const VrAlgorithmParams& algorithmParams,
|
|
const VrDebugParam& debugParam,
|
|
LaserDataLoader& dataLoader,
|
|
const double clibMatrix[16],
|
|
int eulerOrder,
|
|
int dirVectorInvert,
|
|
const WD_HRM_BinInfo* binInfo,
|
|
StatorWorkpiecePositionDetectionResult& detectionResult)
|
|
{
|
|
detectionResult.cameraIndex = cameraIndex;
|
|
detectionResult.errorCode = SUCCESS;
|
|
detectionResult.positions.clear();
|
|
|
|
if (laserLines.empty()) {
|
|
detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
|
|
return detectionResult.errorCode;
|
|
}
|
|
if (detections.empty()) {
|
|
detectionResult.errorCode = ERR_CODE(SX_ERR_ZERO_2D_OBJECTS);
|
|
return detectionResult.errorCode;
|
|
}
|
|
|
|
std::vector<std::vector<SVzNLPositionD>> algorithmData;
|
|
const int positionResult = dataLoader.ConvertToSVzNLPositionD(
|
|
laserLines, algorithmData);
|
|
if (positionResult != SUCCESS) {
|
|
detectionResult.errorCode = positionResult;
|
|
LOG_WARNING("Failed to convert VzNLSDK data to SVzNLPositionD, err=%d, detail=%s\n",
|
|
positionResult, dataLoader.GetLastError().c_str());
|
|
return positionResult;
|
|
}
|
|
|
|
if (algorithmData.empty()) {
|
|
detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
|
|
LOG_WARNING("Tapered algorithm input has no valid scan lines\n");
|
|
return detectionResult.errorCode;
|
|
}
|
|
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
|
LOG_INFO("[Tapered Debug] SVzNLPositionD input lines: %zu\n",
|
|
algorithmData.size());
|
|
}
|
|
|
|
VrCameraPlaneCalibParam cameraCalibParamValue;
|
|
const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
|
|
if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue) &&
|
|
cameraCalibParamValue.isCalibrated) {
|
|
cameraCalibParam = &cameraCalibParamValue;
|
|
}
|
|
const SSG_planeCalibPara groundCalibPara = ToGroundCalibParam(cameraCalibParam);
|
|
|
|
std::vector<WD_objArea2D> objROIs;
|
|
objROIs.reserve(detections.size());
|
|
std::vector<int> sourceDetectionIndices;
|
|
sourceDetectionIndices.reserve(detections.size());
|
|
for (size_t index = 0; index < detections.size(); ++index) {
|
|
const WorkpiecePositionModelAdapter::Detection& detection = detections[index];
|
|
if (detection.classId < 0 || detection.classId >= kSupportedWorkpieceClassCount) {
|
|
LOG_WARNING("Ignoring unsupported 2D class at ROI %zu: class=%d\n", index, detection.classId);
|
|
continue;
|
|
}
|
|
|
|
const int workpieceType = algorithmParams.modelParam.WorkpieceTypeForClass(detection.classId);
|
|
if (workpieceType <= 0) {
|
|
LOG_INFO("Ignoring disabled 2D class at ROI %zu: class=%d, configured workpieceType=%d\n",
|
|
index, detection.classId, workpieceType);
|
|
continue;
|
|
}
|
|
|
|
const WorkpiecePositionModelAdapter::Rect& roi = detection.objectRoi;
|
|
if (!std::isfinite(detection.confidence) ||
|
|
detection.confidence < 0.0 || detection.confidence > 1.0 ||
|
|
!std::isfinite(roi.left) || !std::isfinite(roi.top) ||
|
|
!std::isfinite(roi.right) || !std::isfinite(roi.bottom) ||
|
|
roi.right <= roi.left || roi.bottom <= roi.top) {
|
|
detectionResult.errorCode = ERR_CODE(DATA_ERR_INVALID);
|
|
LOG_ERROR("Invalid 2D ROI at index %zu: class=%d score=%.6f "
|
|
"LTRB=(%.3f,%.3f,%.3f,%.3f)\n",
|
|
index,detection.classId, detection.confidence,
|
|
roi.left, roi.top, roi.right, roi.bottom);
|
|
return detectionResult.errorCode;
|
|
}
|
|
|
|
WD_objArea2D objROI = {};
|
|
objROI.workpieceType = workpieceType; // class0/1/2 -> a/b/c -> 1/2/3
|
|
objROI.roi.left = roi.left;
|
|
objROI.roi.top = roi.top;
|
|
objROI.roi.right = roi.right;
|
|
objROI.roi.bottom = roi.bottom;
|
|
objROI.score2D = detection.confidence;
|
|
objROIs.push_back(objROI);
|
|
sourceDetectionIndices.push_back(static_cast<int>(index));
|
|
}
|
|
|
|
if (objROIs.empty()) {
|
|
detectionResult.errorCode = ERR_CODE(SX_ERR_ZERO_2D_OBJECTS);
|
|
LOG_WARNING("No enabled 2D ROI is available for tapered workpiece positioning\n");
|
|
return detectionResult.errorCode;
|
|
}
|
|
|
|
std::vector<WD_workpieceInfo> workpiecePositions;
|
|
CVrTimeUtils timer;
|
|
int algorithmResult = SUCCESS;
|
|
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
|
LogTaperedAlgorithmInput(algorithmData, objROIs, groundCalibPara, algorithmParams, clibMatrix, eulerOrder, dirVectorInvert);
|
|
}
|
|
(void)binInfo;
|
|
size_t totalPoints = 0;
|
|
for (const auto& line : algorithmData) {
|
|
totalPoints += line.size();
|
|
}
|
|
LOG_INFO("[Tapered] entering wd_HRM_TaperedWorkpiecePositioning: lines=%zu points=%zu objROIs=%zu\n",
|
|
algorithmData.size(), totalPoints, objROIs.size());
|
|
try {
|
|
wd_HRM_TaperedWorkpiecePositioning(algorithmData, objROIs, groundCalibPara, workpiecePositions, &algorithmResult);
|
|
LOG_INFO("wd_HRM_TaperedWorkpiecePositioning returned: found %zu workpieces, err=%d runtime=%.3fms\n",
|
|
workpiecePositions.size(), algorithmResult, timer.GetElapsedTimeInMilliSec());
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("wd_HRM_TaperedWorkpiecePositioning threw exception: %s\n", e.what());
|
|
detectionResult.errorCode = ERR_CODE(APP_ERR_EXEC);
|
|
return detectionResult.errorCode;
|
|
} catch (...) {
|
|
LOG_ERROR("wd_HRM_TaperedWorkpiecePositioning threw unknown exception\n");
|
|
detectionResult.errorCode = ERR_CODE(APP_ERR_EXEC);
|
|
return detectionResult.errorCode;
|
|
}
|
|
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
|
LogRawTaperedOutputs(workpiecePositions);
|
|
}
|
|
if (algorithmResult != SUCCESS) {
|
|
detectionResult.errorCode = algorithmResult;
|
|
return algorithmResult;
|
|
}
|
|
if (workpiecePositions.empty()) {
|
|
detectionResult.errorCode = ERR_CODE(SX_ERR_ZERO_OBJECTS);
|
|
LOG_WARNING("Tapered workpiece algorithm returned success without a target\n");
|
|
return detectionResult.errorCode;
|
|
}
|
|
std::unique_ptr<IHandEyeCalib, decltype(&DestroyHandEyeCalibInstance)> handEyeCalib(
|
|
CreateHandEyeCalibInstance(), DestroyHandEyeCalibInstance);
|
|
if (!handEyeCalib) {
|
|
detectionResult.errorCode = ERR_CODE(DEV_NOT_FIND);
|
|
return detectionResult.errorCode;
|
|
}
|
|
|
|
(void)eulerOrder; // The supplied 4x4 matrix already fixes the hand-eye rotation.
|
|
const HECCalibResult calibResult = HECCalibResult::fromHomogeneousArray(clibMatrix);
|
|
const VrToolParam& toolParam = algorithmParams.toolParam;
|
|
const HECEulerOrder toolEulerOrder = ToHandEyeEulerOrder(toolParam.eulerOrder);
|
|
|
|
for (size_t index = 0; index < workpiecePositions.size(); ++index) {
|
|
const WD_workpieceInfo& workpiece = workpiecePositions[index];
|
|
WorkpiecePosition position;
|
|
position.workpieceType = workpiece.workpieceType;
|
|
if (workpiece.id > 0 && workpiece.id <= static_cast<int>(sourceDetectionIndices.size())) {
|
|
position.sourceDetectionIndex = sourceDetectionIndices[workpiece.id - 1];
|
|
} else if (index < sourceDetectionIndices.size()) {
|
|
position.sourceDetectionIndex = sourceDetectionIndices[index];
|
|
LOG_WARNING("Invalid workpiece id=%d at algorithm output %zu, fallback sourceDetectionIndex=%d\n",
|
|
workpiece.id, index, position.sourceDetectionIndex);
|
|
} else {
|
|
position.sourceDetectionIndex = static_cast<int>(index);
|
|
LOG_WARNING("Invalid workpiece id=%d at algorithm output %zu, fallback output index=%d\n",
|
|
workpiece.id, index, position.sourceDetectionIndex);
|
|
}
|
|
position.value = workpiece.value;
|
|
|
|
HECPoint3D robotCenter;
|
|
handEyeCalib->TransformPoint(calibResult.R, calibResult.T, ToHandEyePoint(workpiece.center), robotCenter);
|
|
position.center = ToResultPoint(robotCenter);
|
|
position.x = robotCenter.x + toolParam.offsetX;
|
|
position.y = robotCenter.y + toolParam.offsetY;
|
|
position.z = robotCenter.z + toolParam.offsetZ;
|
|
|
|
std::vector<HECPoint3D> eyeAxes;
|
|
eyeAxes.push_back(ToHandEyePoint(workpiece.x_dir).normalized());
|
|
eyeAxes.push_back(ToHandEyePoint(workpiece.y_dir).normalized());
|
|
eyeAxes.push_back(ToHandEyePoint(workpiece.z_dir).normalized());
|
|
ApplyConfiguredAxisInversion(dirVectorInvert, eyeAxes);
|
|
ApplyToolRotationToEyeAxes(*handEyeCalib, toolEulerOrder, toolParam, eyeAxes);
|
|
|
|
std::vector<HECPoint3D> robotAxes(3);
|
|
for (int axis = 0; axis < 3; ++axis) {
|
|
handEyeCalib->RotatePoint(calibResult.R, eyeAxes[axis], robotAxes[axis]);
|
|
robotAxes[axis] = robotAxes[axis].normalized();
|
|
}
|
|
position.xDir = ToResultPoint(robotAxes[0]);
|
|
position.yDir = ToResultPoint(robotAxes[1]);
|
|
position.zDir = ToResultPoint(robotAxes[2]);
|
|
|
|
double rotation[3][3] = {
|
|
{robotAxes[0].x, robotAxes[1].x, robotAxes[2].x},
|
|
{robotAxes[0].y, robotAxes[1].y, robotAxes[2].y},
|
|
{robotAxes[0].z, robotAxes[1].z, robotAxes[2].z}
|
|
};
|
|
const SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(rotation);
|
|
position.roll = robotRpy.roll;
|
|
position.pitch = robotRpy.pitch;
|
|
position.yaw = robotRpy.yaw;
|
|
|
|
if (debugParam.enableDebug && debugParam.printDetailLog) {
|
|
LOG_INFO("[Tapered Debug] finalOutput[%zu]: id=%d, sourceDetectionIndex=%d, type=%d, value=%.9f, "
|
|
"centerRobot=(%.9f,%.9f,%.9f), targetXYZ=(%.9f,%.9f,%.9f), "
|
|
"RPY=(%.9f,%.9f,%.9f)\n",
|
|
index,
|
|
workpiece.id,
|
|
position.sourceDetectionIndex,
|
|
position.workpieceType,
|
|
position.value,
|
|
position.center.x, position.center.y, position.center.z,
|
|
position.x, position.y, position.z,
|
|
position.roll, position.pitch, position.yaw);
|
|
}
|
|
|
|
detectionResult.positions.push_back(position);
|
|
}
|
|
|
|
return SUCCESS;
|
|
}
|
|
|
|
int DetectPresenter::DetectBin(
|
|
int cameraIndex,
|
|
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
|
const VrAlgorithmParams& algorithmParams,
|
|
const VrDebugParam& debugParam,
|
|
LaserDataLoader& dataLoader,
|
|
WD_HRM_BinInfo& binInfo)
|
|
{
|
|
(void)debugParam;
|
|
if (laserLines.empty()) {
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|
}
|
|
|
|
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
|
const int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, scanLines);
|
|
if (convertResult != SUCCESS) {
|
|
LOG_WARNING("Failed to convert VzNLSDK data to SVzNL3DPosition for bin detection, err=%d, detail=%s\n",
|
|
convertResult, dataLoader.GetLastError().c_str());
|
|
return convertResult;
|
|
}
|
|
if (scanLines.empty()) {
|
|
return ERR_CODE(DEV_DATA_INVALID);
|
|
}
|
|
|
|
VrCameraPlaneCalibParam cameraCalibParamValue;
|
|
const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
|
|
if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue) &&
|
|
cameraCalibParamValue.isCalibrated) {
|
|
cameraCalibParam = &cameraCalibParamValue;
|
|
}
|
|
const SSG_planeCalibPara groundCalibPara = ToGroundCalibParam(cameraCalibParam);
|
|
|
|
CVrTimeUtils timer;
|
|
int errCode = SUCCESS;
|
|
size_t totalPoints = 0;
|
|
for (const auto& line : scanLines) {
|
|
totalPoints += line.size();
|
|
}
|
|
LOG_INFO("[Tapered] entering wd_HRM_getBinSize: lines=%zu points=%zu binHeight=%.3f\n",
|
|
scanLines.size(), totalPoints, algorithmParams.binParam.binHeight);
|
|
try {
|
|
binInfo = wd_HRM_getBinSize(
|
|
scanLines,
|
|
groundCalibPara,
|
|
algorithmParams.binParam.binHeight,
|
|
&errCode);
|
|
LOG_INFO("wd_HRM_getBinSize returned: err=%d runtime=%.3fms, binHeight=%.3f, length=%.3f, width=%.3f, topZ=%.3f, center=(%.3f,%.3f,%.3f)\n",
|
|
errCode, timer.GetElapsedTimeInMilliSec(),
|
|
algorithmParams.binParam.binHeight,
|
|
binInfo.length, binInfo.width, binInfo.binTopZ,
|
|
binInfo.center.x, binInfo.center.y, binInfo.center.z);
|
|
} catch (const std::exception& e) {
|
|
LOG_ERROR("wd_HRM_getBinSize threw exception: %s\n", e.what());
|
|
return ERR_CODE(APP_ERR_EXEC);
|
|
} catch (...) {
|
|
LOG_ERROR("wd_HRM_getBinSize threw unknown exception\n");
|
|
return ERR_CODE(APP_ERR_EXEC);
|
|
}
|
|
return errCode;
|
|
}
|