860 lines
28 KiB
C++
860 lines
28 KiB
C++
#include "DroneScrewAlgoStub.h"
|
|
#include "VrError.h"
|
|
#include "VrLog.h"
|
|
|
|
#include <QCoreApplication>
|
|
#include <QDir>
|
|
#include <QDomDocument>
|
|
#include <QFile>
|
|
#include <QFileInfo>
|
|
#include <QHash>
|
|
#include <QByteArray>
|
|
#include <QStringList>
|
|
#include <QTextStream>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <sstream>
|
|
|
|
#ifdef DRONESCREW_STEREO_BOLT_DIRECT_LINK
|
|
|
|
namespace
|
|
{
|
|
constexpr const char* kDefaultConfigPath = "config/config.rk3588.yaml";
|
|
constexpr const char* kDefaultCalibPath = "calib/stereo_calib.xml";
|
|
constexpr const char* kDefaultModelPath = "weights/best.rknn";
|
|
constexpr int kDefaultYoloImgSize = 960;
|
|
constexpr int kDefaultExpectedBoltCount = 8;
|
|
constexpr int kMaxRangeBolts = 64;
|
|
|
|
void appendUniqueDir(QStringList& dirs, const QString& dir)
|
|
{
|
|
if (dir.isEmpty())
|
|
return;
|
|
|
|
const QString absolute = QDir(dir).absolutePath();
|
|
if (!dirs.contains(absolute))
|
|
dirs.append(absolute);
|
|
}
|
|
|
|
QString resolvePath(const QString& path, const QStringList& baseDirs)
|
|
{
|
|
const QString trimmed = path.trimmed();
|
|
if (trimmed.isEmpty())
|
|
return QString();
|
|
|
|
const QFileInfo direct(trimmed);
|
|
if (direct.isAbsolute())
|
|
return direct.absoluteFilePath();
|
|
|
|
QString fallback;
|
|
for (const QString& baseDir : baseDirs)
|
|
{
|
|
const QFileInfo candidate(QDir(baseDir).filePath(trimmed));
|
|
if (fallback.isEmpty())
|
|
fallback = candidate.absoluteFilePath();
|
|
if (candidate.exists())
|
|
return candidate.absoluteFilePath();
|
|
}
|
|
return fallback.isEmpty() ? QFileInfo(trimmed).absoluteFilePath() : fallback;
|
|
}
|
|
|
|
QString yamlValueAfterColon(QString line)
|
|
{
|
|
const int comment = line.indexOf('#');
|
|
if (comment >= 0)
|
|
line = line.left(comment);
|
|
|
|
const int colon = line.indexOf(':');
|
|
if (colon < 0)
|
|
return QString();
|
|
|
|
QString value = line.mid(colon + 1).trimmed();
|
|
if (value.size() >= 2)
|
|
{
|
|
const QChar first = value.at(0);
|
|
const QChar last = value.at(value.size() - 1);
|
|
if ((first == '"' && last == '"') || (first == '\'' && last == '\''))
|
|
value = value.mid(1, value.size() - 2);
|
|
}
|
|
return value;
|
|
}
|
|
|
|
QHash<QString, QString> readYamlValues(const QString& configPath)
|
|
{
|
|
QHash<QString, QString> values;
|
|
QFile file(configPath);
|
|
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
|
|
return values;
|
|
|
|
QTextStream in(&file);
|
|
QString section;
|
|
int sectionIndent = -1;
|
|
while (!in.atEnd())
|
|
{
|
|
const QString raw = in.readLine();
|
|
int indent = 0;
|
|
while (indent < raw.size() && raw.at(indent).isSpace())
|
|
++indent;
|
|
|
|
QString line = raw.mid(indent);
|
|
const int comment = line.indexOf('#');
|
|
if (comment >= 0)
|
|
line = line.left(comment);
|
|
line = line.trimmed();
|
|
if (line.isEmpty())
|
|
continue;
|
|
|
|
if (indent == 0 && line.endsWith(':'))
|
|
{
|
|
section = line.left(line.size() - 1).trimmed();
|
|
sectionIndent = indent;
|
|
continue;
|
|
}
|
|
if (!section.isEmpty() && indent <= sectionIndent)
|
|
section.clear();
|
|
|
|
const int colon = line.indexOf(':');
|
|
if (colon < 0)
|
|
continue;
|
|
|
|
const QString key = line.left(colon).trimmed();
|
|
const QString value = yamlValueAfterColon(line);
|
|
const QString fullKey = section.isEmpty() ? key : (section + "." + key);
|
|
values[fullKey] = value;
|
|
}
|
|
return values;
|
|
}
|
|
|
|
QString childText(const QDomElement& parent, const QString& name)
|
|
{
|
|
const QDomElement child = parent.firstChildElement(name);
|
|
return child.isNull() ? QString() : child.text().trimmed();
|
|
}
|
|
|
|
int childInt(const QDomElement& parent, const QString& name, int fallback = 0)
|
|
{
|
|
bool ok = false;
|
|
const int value = childText(parent, name).toInt(&ok);
|
|
return ok ? value : fallback;
|
|
}
|
|
|
|
double childDouble(const QDomElement& parent, const QString& name, double fallback = 0.0)
|
|
{
|
|
bool ok = false;
|
|
const double value = childText(parent, name).toDouble(&ok);
|
|
return ok ? value : fallback;
|
|
}
|
|
|
|
std::vector<double> matrixValues(const QDomElement& parent, const QString& name)
|
|
{
|
|
std::vector<double> values;
|
|
const QDomElement element = parent.firstChildElement(name);
|
|
if (element.isNull())
|
|
return values;
|
|
|
|
const QString text = childText(element, "data").simplified();
|
|
const QStringList tokens = text.split(' ', Qt::SkipEmptyParts);
|
|
values.reserve(tokens.size());
|
|
for (const QString& token : tokens)
|
|
{
|
|
bool ok = false;
|
|
const double value = token.toDouble(&ok);
|
|
if (ok)
|
|
values.push_back(value);
|
|
}
|
|
return values;
|
|
}
|
|
|
|
bool copyDoubles(const std::vector<double>& src, double* dst, int count)
|
|
{
|
|
if (static_cast<int>(src.size()) < count)
|
|
return false;
|
|
for (int i = 0; i < count; ++i)
|
|
dst[i] = src[static_cast<size_t>(i)];
|
|
return true;
|
|
}
|
|
|
|
bool loadCalibration(const QString& xmlPath, StereoBoltCalibC& calib, std::string& error)
|
|
{
|
|
QFile file(xmlPath);
|
|
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
|
|
{
|
|
error = "open calibration xml failed: " + file.errorString().toStdString();
|
|
return false;
|
|
}
|
|
|
|
QDomDocument doc;
|
|
QString parseError;
|
|
int line = 0;
|
|
int column = 0;
|
|
if (!doc.setContent(&file, &parseError, &line, &column))
|
|
{
|
|
std::ostringstream oss;
|
|
oss << "parse calibration xml failed at " << line << ":" << column
|
|
<< ": " << parseError.toStdString();
|
|
error = oss.str();
|
|
return false;
|
|
}
|
|
|
|
const QDomElement root = doc.documentElement();
|
|
const QDomElement left = root.firstChildElement("LeftCamera");
|
|
const QDomElement right = root.firstChildElement("RightCamera");
|
|
const QDomElement stereo = root.firstChildElement("Stereo");
|
|
if (left.isNull() || right.isNull() || stereo.isNull())
|
|
{
|
|
error = "calibration xml missing LeftCamera/RightCamera/Stereo";
|
|
return false;
|
|
}
|
|
|
|
std::memset(&calib, 0, sizeof(calib));
|
|
calib.image_width = childInt(left, "ImageWidth");
|
|
calib.image_height = childInt(left, "ImageHeight");
|
|
calib.baseline_mm = childDouble(stereo, "Baseline");
|
|
|
|
if (calib.image_width <= 0 || calib.image_height <= 0)
|
|
{
|
|
error = "calibration xml has invalid image size";
|
|
return false;
|
|
}
|
|
|
|
if (!copyDoubles(matrixValues(left, "CameraMatrix"), calib.left_K, 9) ||
|
|
!copyDoubles(matrixValues(left, "DistCoeffs"), calib.left_D, 5) ||
|
|
!copyDoubles(matrixValues(right, "CameraMatrix"), calib.right_K, 9) ||
|
|
!copyDoubles(matrixValues(right, "DistCoeffs"), calib.right_D, 5) ||
|
|
!copyDoubles(matrixValues(stereo, "R"), calib.R, 9) ||
|
|
!copyDoubles(matrixValues(stereo, "T"), calib.T, 3))
|
|
{
|
|
error = "calibration xml has incomplete matrix data";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool readFileBytes(const QString& path, QByteArray& bytes, std::string& error)
|
|
{
|
|
QFile file(path);
|
|
if (!file.open(QIODevice::ReadOnly))
|
|
{
|
|
error = "open model failed: " + file.errorString().toStdString();
|
|
return false;
|
|
}
|
|
|
|
bytes = file.readAll();
|
|
if (bytes.isEmpty())
|
|
{
|
|
error = "model file is empty";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool yamlBool(const QHash<QString, QString>& values, const QString& key, bool fallback)
|
|
{
|
|
const QString value = values.value(key).trimmed().toLower();
|
|
if (value == "true" || value == "1" || value == "yes")
|
|
return true;
|
|
if (value == "false" || value == "0" || value == "no")
|
|
return false;
|
|
return fallback;
|
|
}
|
|
|
|
double yamlDouble(const QHash<QString, QString>& values, const QString& key, double fallback)
|
|
{
|
|
bool ok = false;
|
|
const double value = values.value(key).toDouble(&ok);
|
|
return ok ? value : fallback;
|
|
}
|
|
|
|
int yamlInt(const QHash<QString, QString>& values, const QString& key, int fallback)
|
|
{
|
|
bool ok = false;
|
|
const int value = values.value(key).toInt(&ok);
|
|
return ok ? value : fallback;
|
|
}
|
|
|
|
void fillBaseResult(const DroneScrewInputImage& leftImage, DroneScrewResult& result)
|
|
{
|
|
result.frameId = leftImage.frameId;
|
|
result.timestampUs = leftImage.timestampUs;
|
|
result.imageWidth = leftImage.width;
|
|
result.imageHeight = leftImage.height;
|
|
result.boxes.clear();
|
|
result.distances.clear();
|
|
}
|
|
|
|
bool validateInput(const DroneScrewInputImage& leftImage,
|
|
const DroneScrewInputImage& rightImage,
|
|
std::string& message)
|
|
{
|
|
if (!leftImage.data || !rightImage.data)
|
|
{
|
|
message = "stereo_bolt input invalid: null image buffer";
|
|
return false;
|
|
}
|
|
if (leftImage.width <= 0 || leftImage.height <= 0 ||
|
|
rightImage.width <= 0 || rightImage.height <= 0)
|
|
{
|
|
message = "stereo_bolt input invalid: empty image size";
|
|
return false;
|
|
}
|
|
if (leftImage.width != rightImage.width || leftImage.height != rightImage.height)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << "stereo_bolt input invalid: left/right size mismatch left="
|
|
<< leftImage.width << "x" << leftImage.height
|
|
<< " right=" << rightImage.width << "x" << rightImage.height;
|
|
message = oss.str();
|
|
return false;
|
|
}
|
|
if (leftImage.pixelFormat != 0 || rightImage.pixelFormat != 0)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << "stereo_bolt input invalid: expected Mono8 pixelFormat=0, got left="
|
|
<< leftImage.pixelFormat << " right=" << rightImage.pixelFormat;
|
|
message = oss.str();
|
|
return false;
|
|
}
|
|
if ((leftImage.stride > 0 && leftImage.stride < leftImage.width) ||
|
|
(rightImage.stride > 0 && rightImage.stride < rightImage.width))
|
|
{
|
|
message = "stereo_bolt input invalid: stride is smaller than width";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void appendRoiBox(const StereoBoltModuleRoiC& roi, DroneScrewResult& result)
|
|
{
|
|
DroneScrewBox box;
|
|
box.classId = roi.class_id;
|
|
box.score = static_cast<float>(roi.score);
|
|
box.x = roi.x;
|
|
box.y = roi.y;
|
|
box.width = roi.width;
|
|
box.height = roi.height;
|
|
result.boxes.push_back(box);
|
|
}
|
|
|
|
void appendDistance(const StereoBoltModuleDistanceC& distance, DroneScrewResult& result)
|
|
{
|
|
DroneScrewDistance d;
|
|
d.fromId = distance.bolt_id_i;
|
|
d.toId = distance.bolt_id_j;
|
|
d.distanceMm = static_cast<float>(distance.distance_mm);
|
|
result.distances.push_back(d);
|
|
}
|
|
|
|
void appendRangeDistance(int index,
|
|
int confidence,
|
|
double distanceMm,
|
|
DroneScrewResult& result)
|
|
{
|
|
DroneScrewDistance d;
|
|
d.fromId = index;
|
|
d.toId = confidence;
|
|
d.distanceMm = static_cast<float>(distanceMm);
|
|
result.distances.push_back(d);
|
|
}
|
|
|
|
void appendRangeBox(const StereoBoltRangeBoltC& bolt, DroneScrewResult& result)
|
|
{
|
|
constexpr int kMarkerSize = 12;
|
|
DroneScrewBox box;
|
|
box.classId = bolt.confidence;
|
|
box.score = static_cast<float>(bolt.ncc_score);
|
|
box.x = bolt.left_x - kMarkerSize / 2;
|
|
box.y = bolt.left_y - kMarkerSize / 2;
|
|
box.width = kMarkerSize;
|
|
box.height = kMarkerSize;
|
|
result.boxes.push_back(box);
|
|
}
|
|
|
|
std::string buildSuccessMessage(const StereoBoltModuleResultC& out, int expectedCount)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << "stereo_bolt ok: bolts=" << out.data.n_bolts
|
|
<< " expected=" << expectedCount
|
|
<< " plane_rms_mm=" << out.data.ground_plane.rms_mm;
|
|
|
|
if (out.data.n_bolts > 0 && out.data.bolts)
|
|
{
|
|
oss << " heights_mm=[";
|
|
const int n = std::min(out.data.n_bolts, 16);
|
|
for (int i = 0; i < n; ++i)
|
|
{
|
|
if (i > 0) oss << ",";
|
|
oss << out.data.bolts[i].height_mm;
|
|
}
|
|
if (out.data.n_bolts > n) oss << ",...";
|
|
oss << "]";
|
|
}
|
|
|
|
if (out.data.n_adjacent_distances > 0 && out.data.adjacent_distances)
|
|
{
|
|
oss << " adjacent_mm=[";
|
|
const int n = std::min(out.data.n_adjacent_distances, 16);
|
|
for (int i = 0; i < n; ++i)
|
|
{
|
|
if (i > 0) oss << ",";
|
|
const auto& d = out.data.adjacent_distances[i];
|
|
oss << d.bolt_id_i << "-" << d.bolt_id_j << ":" << d.distance_mm;
|
|
}
|
|
if (out.data.n_adjacent_distances > n) oss << ",...";
|
|
oss << "]";
|
|
}
|
|
|
|
return oss.str();
|
|
}
|
|
|
|
std::string buildFailureMessage(const StereoBoltModuleResultC& out)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << "stereo_bolt rejected: reason=" << out.failure.reason
|
|
<< " left_rois=" << out.failure.n_left_rois
|
|
<< " right_rois=" << out.failure.n_right_rois
|
|
<< " pairs=" << out.failure.n_pairs;
|
|
return oss.str();
|
|
}
|
|
|
|
std::string buildRangeMessage(const StereoBoltRangeSummaryC& summary,
|
|
const StereoBoltRangeBoltC* bolts,
|
|
int writtenBolts)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << "stereo_bolt range: found=" << summary.found
|
|
<< " status=" << summary.status
|
|
<< " median_mm=" << summary.distance_mm
|
|
<< " bolt_count=" << summary.bolt_count;
|
|
if (writtenBolts > 0 && bolts)
|
|
{
|
|
oss << " bolts_mm=[";
|
|
const int n = std::min(writtenBolts, 16);
|
|
for (int i = 0; i < n; ++i)
|
|
{
|
|
if (i > 0) oss << ",";
|
|
oss << bolts[i].distance_mm
|
|
<< "/ncc=" << bolts[i].ncc_score
|
|
<< "/lrc=" << bolts[i].lrc_px;
|
|
}
|
|
if (writtenBolts > n) oss << ",...";
|
|
oss << "]";
|
|
}
|
|
return oss.str();
|
|
}
|
|
}
|
|
|
|
DroneScrewAlgoStub::DroneScrewAlgoStub() = default;
|
|
|
|
DroneScrewAlgoStub::~DroneScrewAlgoStub()
|
|
{
|
|
UnInit();
|
|
}
|
|
|
|
int DroneScrewAlgoStub::Init(const DroneScrewAlgoParams& params)
|
|
{
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
m_params = params;
|
|
m_initError.clear();
|
|
m_bInited = false;
|
|
|
|
if (m_ctx)
|
|
{
|
|
sb_destroy(m_ctx);
|
|
m_ctx = nullptr;
|
|
}
|
|
|
|
QStringList configBases;
|
|
appendUniqueDir(configBases, QDir::currentPath());
|
|
appendUniqueDir(configBases, QCoreApplication::applicationDirPath());
|
|
|
|
const QString configInput = QString::fromStdString(
|
|
m_params.modelPath.empty() ? std::string(kDefaultConfigPath) : m_params.modelPath);
|
|
const QString configPath = resolvePath(configInput, configBases);
|
|
const QHash<QString, QString> yaml = readYamlValues(configPath);
|
|
|
|
QStringList assetBases = configBases;
|
|
const QFileInfo configInfo(configPath);
|
|
appendUniqueDir(assetBases, configInfo.absolutePath());
|
|
appendUniqueDir(assetBases, QDir(configInfo.absolutePath()).absoluteFilePath(".."));
|
|
|
|
const QString calibRel = yaml.value("calibration.xml_path", kDefaultCalibPath);
|
|
const QString modelRel = yaml.value("yolo.model_path", kDefaultModelPath);
|
|
const QString calibPath = resolvePath(calibRel, assetBases);
|
|
const QString modelPath = resolvePath(modelRel, assetBases);
|
|
|
|
StereoBoltCalibC calib{};
|
|
std::string loadError;
|
|
if (!loadCalibration(calibPath, calib, loadError))
|
|
{
|
|
m_initError = std::string("stereo_bolt load calibration failed xml=") +
|
|
calibPath.toStdString() + " err=" + loadError;
|
|
LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
|
|
return ERR_CODE(DEV_OPEN_ERR);
|
|
}
|
|
|
|
QByteArray modelBytes;
|
|
if (!readFileBytes(modelPath, modelBytes, loadError))
|
|
{
|
|
m_initError = std::string("stereo_bolt load model failed model=") +
|
|
modelPath.toStdString() + " err=" + loadError;
|
|
LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
|
|
return ERR_CODE(DEV_OPEN_ERR);
|
|
}
|
|
|
|
StereoBoltModelC model{};
|
|
model.data = reinterpret_cast<const uint8_t*>(modelBytes.constData());
|
|
model.size = static_cast<size_t>(modelBytes.size());
|
|
const QString backend = yaml.value("yolo.backend", "rknn").trimmed().toLower();
|
|
model.backend = (backend == "opencv" || backend == "opencv_dnn")
|
|
? SB_YOLO_BACKEND_OPENCV
|
|
: (backend == "auto" ? SB_YOLO_BACKEND_AUTO : SB_YOLO_BACKEND_RKNN);
|
|
model.imgsz = yamlInt(yaml, "yolo.imgsz", kDefaultYoloImgSize);
|
|
model.conf = m_params.scoreThreshold > 0.0f
|
|
? m_params.scoreThreshold
|
|
: static_cast<float>(yamlDouble(yaml, "yolo.conf", 0.5));
|
|
model.iou = m_params.nmsThreshold > 0.0f ? m_params.nmsThreshold : 0.45f;
|
|
|
|
StereoBoltParamsC algoParams = sb_default_params();
|
|
algoParams.interpolation =
|
|
yaml.value("rectification.interpolation", "cubic").trimmed().toLower() == "linear" ? 0 : 1;
|
|
algoParams.rect_alpha = yamlDouble(yaml, "rectification.alpha", algoParams.rect_alpha);
|
|
algoParams.wd_z_min_mm = yamlDouble(yaml, "working_distance.z_min_mm",
|
|
yamlDouble(yaml, "pointcloud.z_min_mm", 1200.0));
|
|
algoParams.wd_z_max_mm = yamlDouble(yaml, "working_distance.z_max_mm",
|
|
yamlDouble(yaml, "pointcloud.z_max_mm", 2800.0));
|
|
algoParams.sgbm_min_disparity =
|
|
yamlDouble(yaml, "sgbm.min_disparity", algoParams.sgbm_min_disparity);
|
|
algoParams.sgbm_num_disparities =
|
|
yamlDouble(yaml, "sgbm.num_disparities", algoParams.sgbm_num_disparities);
|
|
algoParams.foot_rim_correction =
|
|
yamlBool(yaml, "measurement.foot_rim_correction", false) ? 1 : 0;
|
|
algoParams.rng_z_min_mm = yamlDouble(yaml, "ranging.z_min_mm", algoParams.rng_z_min_mm);
|
|
algoParams.rng_z_max_mm = yamlDouble(yaml, "ranging.z_max_mm", algoParams.rng_z_max_mm);
|
|
algoParams.rng_lrc_max_mm = yamlDouble(yaml, "ranging.lrc_max_mm", algoParams.rng_lrc_max_mm);
|
|
algoParams.rng_lrc_trusted_mm =
|
|
yamlDouble(yaml, "ranging.lrc_trusted_mm", algoParams.rng_lrc_trusted_mm);
|
|
|
|
m_ctx = sb_create_ex(&calib, &model, &algoParams);
|
|
|
|
if (!m_ctx)
|
|
{
|
|
const char* err = sb_last_error(nullptr);
|
|
m_initError = std::string("stereo_bolt create_ex failed config=") +
|
|
configPath.toStdString() +
|
|
" calib=" + calibPath.toStdString() +
|
|
" model=" + modelPath.toStdString() +
|
|
" err=" + (err ? err : "unknown");
|
|
LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
|
|
return ERR_CODE(DEV_OPEN_ERR);
|
|
}
|
|
|
|
m_configPath = configPath.toStdString();
|
|
m_calibPath = calibPath.toStdString();
|
|
m_modelPath = modelPath.toStdString();
|
|
m_bInited = true;
|
|
|
|
LOG_INFO("[ALGO] stereo_bolt init ok create_ex config=%s calib=%s model=%s imgsz=%d wd=%.0f..%.0f rng=%.0f..%.0f expectedBoltCount=%d\n",
|
|
m_configPath.c_str(),
|
|
m_calibPath.c_str(),
|
|
m_modelPath.c_str(),
|
|
model.imgsz,
|
|
algoParams.wd_z_min_mm,
|
|
algoParams.wd_z_max_mm,
|
|
algoParams.rng_z_min_mm,
|
|
algoParams.rng_z_max_mm,
|
|
m_params.expectedBoltCount > 0 ? m_params.expectedBoltCount : kDefaultExpectedBoltCount);
|
|
return 0;
|
|
}
|
|
|
|
int DroneScrewAlgoStub::UnInit()
|
|
{
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
m_bInited = false;
|
|
|
|
if (m_ctx)
|
|
{
|
|
sb_destroy(m_ctx);
|
|
m_ctx = nullptr;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int DroneScrewAlgoStub::Detect(const DroneScrewInputImage& leftImage,
|
|
const DroneScrewInputImage& rightImage,
|
|
DroneScrewResult& result)
|
|
{
|
|
fillBaseResult(leftImage, result);
|
|
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
if (!m_bInited.load() || !m_ctx)
|
|
{
|
|
result.success = false;
|
|
result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
|
|
result.message = m_initError.empty()
|
|
? "stereo_bolt not initialized"
|
|
: ("stereo_bolt not initialized: " + m_initError);
|
|
return result.errorCode;
|
|
}
|
|
|
|
std::string validationMessage;
|
|
if (!validateInput(leftImage, rightImage, validationMessage))
|
|
{
|
|
result.success = false;
|
|
result.errorCode = ERR_CODE(DATA_ERR_INVALID);
|
|
result.message = validationMessage;
|
|
return result.errorCode;
|
|
}
|
|
|
|
StereoBoltModuleResultC out{};
|
|
const int expectedCount =
|
|
m_params.expectedBoltCount > 0 ? m_params.expectedBoltCount : kDefaultExpectedBoltCount;
|
|
const int leftStride =
|
|
(leftImage.stride == leftImage.width) ? 0 : leftImage.stride;
|
|
const int rightStride =
|
|
(rightImage.stride == rightImage.width) ? 0 : rightImage.stride;
|
|
|
|
const sb_status_t st = sb_process_bolt_module_buffers(
|
|
m_ctx,
|
|
leftImage.data, leftImage.width, leftImage.height, leftStride,
|
|
rightImage.data, rightImage.width, rightImage.height, rightStride,
|
|
1,
|
|
expectedCount,
|
|
&out);
|
|
|
|
if (st == SB_OK && out.success)
|
|
{
|
|
result.success = true;
|
|
result.errorCode = 0;
|
|
if (out.data.n_bolts > 0 && out.data.bolts)
|
|
{
|
|
for (int i = 0; i < out.data.n_bolts; ++i)
|
|
appendRoiBox(out.data.bolts[i].left_roi, result);
|
|
}
|
|
if (out.data.n_adjacent_distances > 0 && out.data.adjacent_distances)
|
|
{
|
|
for (int i = 0; i < out.data.n_adjacent_distances; ++i)
|
|
appendDistance(out.data.adjacent_distances[i], result);
|
|
}
|
|
result.message = buildSuccessMessage(out, expectedCount);
|
|
sb_free_bolt_module_result(&out);
|
|
return 0;
|
|
}
|
|
|
|
if (st == SB_OK || st == SB_ERR_ALGORITHM_REJECTED)
|
|
{
|
|
result.success = false;
|
|
result.errorCode = static_cast<int>(st == SB_OK ? SB_ERR_ALGORITHM_REJECTED : st);
|
|
if (out.failure.n_left_rois > 0 && out.failure.left_rois)
|
|
{
|
|
for (int i = 0; i < out.failure.n_left_rois; ++i)
|
|
appendRoiBox(out.failure.left_rois[i], result);
|
|
}
|
|
result.message = buildFailureMessage(out);
|
|
sb_free_bolt_module_result(&out);
|
|
return 0;
|
|
}
|
|
|
|
const char* err = sb_last_error(m_ctx);
|
|
result.success = false;
|
|
result.errorCode = static_cast<int>(st);
|
|
result.message = std::string("stereo_bolt process failed: status=") +
|
|
std::to_string(static_cast<int>(st)) +
|
|
" err=" + (err ? err : "unknown");
|
|
sb_free_bolt_module_result(&out);
|
|
return result.errorCode;
|
|
}
|
|
|
|
int DroneScrewAlgoStub::DetectDistance(const DroneScrewInputImage& leftImage,
|
|
const DroneScrewInputImage& rightImage,
|
|
DroneScrewResult& result)
|
|
{
|
|
fillBaseResult(leftImage, result);
|
|
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
if (!m_bInited.load() || !m_ctx)
|
|
{
|
|
result.success = false;
|
|
result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
|
|
result.message = m_initError.empty()
|
|
? "stereo_bolt not initialized"
|
|
: ("stereo_bolt not initialized: " + m_initError);
|
|
return result.errorCode;
|
|
}
|
|
|
|
std::string validationMessage;
|
|
if (!validateInput(leftImage, rightImage, validationMessage))
|
|
{
|
|
result.success = false;
|
|
result.errorCode = ERR_CODE(DATA_ERR_INVALID);
|
|
result.message = validationMessage;
|
|
return result.errorCode;
|
|
}
|
|
|
|
StereoBoltRangeSummaryC summary{};
|
|
StereoBoltRangeBoltC bolts[kMaxRangeBolts]{};
|
|
const int leftStride =
|
|
(leftImage.stride == leftImage.width) ? 0 : leftImage.stride;
|
|
const int rightStride =
|
|
(rightImage.stride == rightImage.width) ? 0 : rightImage.stride;
|
|
|
|
const sb_status_t st = sb_range_bolt_binned(
|
|
m_ctx,
|
|
leftImage.data, leftImage.width, leftImage.height, leftStride,
|
|
rightImage.data, rightImage.width, rightImage.height, rightStride,
|
|
1,
|
|
&summary,
|
|
bolts,
|
|
kMaxRangeBolts);
|
|
|
|
if (st != SB_OK)
|
|
{
|
|
const char* err = sb_last_error(m_ctx);
|
|
result.success = false;
|
|
result.errorCode = static_cast<int>(st);
|
|
result.message = std::string("stereo_bolt range failed: status=") +
|
|
std::to_string(static_cast<int>(st)) +
|
|
" err=" + (err ? err : "unknown");
|
|
return result.errorCode;
|
|
}
|
|
|
|
result.success = true;
|
|
result.errorCode = 0;
|
|
result.message = buildRangeMessage(summary, bolts,
|
|
std::min(summary.bolt_count, kMaxRangeBolts));
|
|
|
|
if (summary.found)
|
|
appendRangeDistance(-1, 0, summary.distance_mm, result);
|
|
|
|
const int writtenBolts = std::min(summary.bolt_count, kMaxRangeBolts);
|
|
for (int i = 0; i < writtenBolts; ++i)
|
|
{
|
|
appendRangeDistance(i, bolts[i].confidence, bolts[i].distance_mm, result);
|
|
appendRangeBox(bolts[i], result);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int DroneScrewAlgoStub::UpdateParams(const DroneScrewAlgoParams& params)
|
|
{
|
|
{
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
const bool needReinit =
|
|
params.modelPath != m_params.modelPath ||
|
|
std::fabs(params.scoreThreshold - m_params.scoreThreshold) > 0.0001f ||
|
|
std::fabs(params.nmsThreshold - m_params.nmsThreshold) > 0.0001f ||
|
|
params.inputWidth != m_params.inputWidth ||
|
|
params.inputHeight != m_params.inputHeight ||
|
|
params.modelType != m_params.modelType;
|
|
const bool wasInited = m_bInited.load();
|
|
|
|
if (wasInited && !needReinit)
|
|
{
|
|
m_params = params;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
return Init(params);
|
|
}
|
|
|
|
std::string DroneScrewAlgoStub::GetVersion() const
|
|
{
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
if (m_bInited.load())
|
|
return std::string("StereoBoltDelivery C API direct link, create_ex config=") +
|
|
m_configPath + " calib=" + m_calibPath + " model=" + m_modelPath;
|
|
|
|
if (!m_initError.empty())
|
|
return std::string("StereoBoltDelivery C API direct link not initialized: ") + m_initError;
|
|
|
|
return "StereoBoltDelivery C API direct link";
|
|
}
|
|
|
|
std::unique_ptr<IDroneScrewAlgo> IDroneScrewAlgo::CreateDefault()
|
|
{
|
|
return std::unique_ptr<IDroneScrewAlgo>(new DroneScrewAlgoStub());
|
|
}
|
|
|
|
#else
|
|
|
|
DroneScrewAlgoStub::DroneScrewAlgoStub() = default;
|
|
|
|
DroneScrewAlgoStub::~DroneScrewAlgoStub()
|
|
{
|
|
UnInit();
|
|
}
|
|
|
|
int DroneScrewAlgoStub::Init(const DroneScrewAlgoParams& params)
|
|
{
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
m_params = params;
|
|
m_ctx = nullptr;
|
|
m_bInited = false;
|
|
m_initError = "stereo_bolt_delivery is only linked in Linux/ARM builds";
|
|
LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
|
|
return ERR_CODE(DEV_UNSUPPORT);
|
|
}
|
|
|
|
int DroneScrewAlgoStub::UnInit()
|
|
{
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
m_ctx = nullptr;
|
|
m_bInited = false;
|
|
return 0;
|
|
}
|
|
|
|
int DroneScrewAlgoStub::Detect(const DroneScrewInputImage& leftImage,
|
|
const DroneScrewInputImage&,
|
|
DroneScrewResult& result)
|
|
{
|
|
result.frameId = leftImage.frameId;
|
|
result.timestampUs = leftImage.timestampUs;
|
|
result.imageWidth = leftImage.width;
|
|
result.imageHeight = leftImage.height;
|
|
result.boxes.clear();
|
|
result.distances.clear();
|
|
result.success = false;
|
|
result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
|
|
result.message = "stereo_bolt_delivery is not available in this build";
|
|
return result.errorCode;
|
|
}
|
|
|
|
int DroneScrewAlgoStub::DetectDistance(const DroneScrewInputImage& leftImage,
|
|
const DroneScrewInputImage&,
|
|
DroneScrewResult& result)
|
|
{
|
|
result.frameId = leftImage.frameId;
|
|
result.timestampUs = leftImage.timestampUs;
|
|
result.imageWidth = leftImage.width;
|
|
result.imageHeight = leftImage.height;
|
|
result.boxes.clear();
|
|
result.distances.clear();
|
|
result.success = false;
|
|
result.errorCode = ERR_CODE(DRONESCREW_ERR_ALGO_NOT_INIT);
|
|
result.message = "stereo_bolt_delivery is not available in this build";
|
|
return result.errorCode;
|
|
}
|
|
|
|
int DroneScrewAlgoStub::UpdateParams(const DroneScrewAlgoParams& params)
|
|
{
|
|
std::lock_guard<std::mutex> lk(m_mutex);
|
|
m_params = params;
|
|
return 0;
|
|
}
|
|
|
|
std::string DroneScrewAlgoStub::GetVersion() const
|
|
{
|
|
return "StereoBoltDelivery unavailable: not a Linux/ARM direct-link build";
|
|
}
|
|
|
|
std::unique_ptr<IDroneScrewAlgo> IDroneScrewAlgo::CreateDefault()
|
|
{
|
|
return std::unique_ptr<IDroneScrewAlgo>(new DroneScrewAlgoStub());
|
|
}
|
|
|
|
#endif
|