1148 lines
38 KiB
C++
1148 lines
38 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 <exception>
|
|
#include <limits>
|
|
#include <sstream>
|
|
|
|
#ifdef DRONESCREW_STEREO_BOLT_DIRECT_LINK
|
|
#include <opencv2/calib3d.hpp>
|
|
#include <opencv2/core.hpp>
|
|
|
|
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;
|
|
|
|
cv::Mat mat64(int rows, int cols, const double* values)
|
|
{
|
|
cv::Mat m(rows, cols, CV_64F);
|
|
std::memcpy(m.ptr<double>(0), values, sizeof(double) * rows * cols);
|
|
return m;
|
|
}
|
|
|
|
int roiSampleCount(double span)
|
|
{
|
|
if (span <= 1.0)
|
|
return 2;
|
|
return std::max(3, std::min(17, static_cast<int>(std::ceil(span / 64.0)) + 1));
|
|
}
|
|
|
|
bool sampleMapBilinear(const std::vector<float>& mapX,
|
|
const std::vector<float>& mapY,
|
|
int mapWidth,
|
|
int mapHeight,
|
|
double rectX,
|
|
double rectY,
|
|
double& rawX,
|
|
double& rawY)
|
|
{
|
|
if (mapWidth <= 0 || mapHeight <= 0 ||
|
|
mapX.size() != static_cast<size_t>(mapWidth) * static_cast<size_t>(mapHeight) ||
|
|
mapY.size() != mapX.size())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
rectX = std::max(0.0, std::min(rectX, static_cast<double>(mapWidth - 1)));
|
|
rectY = std::max(0.0, std::min(rectY, static_cast<double>(mapHeight - 1)));
|
|
|
|
const int x0 = static_cast<int>(std::floor(rectX));
|
|
const int y0 = static_cast<int>(std::floor(rectY));
|
|
const int x1 = std::min(x0 + 1, mapWidth - 1);
|
|
const int y1 = std::min(y0 + 1, mapHeight - 1);
|
|
const double tx = rectX - x0;
|
|
const double ty = rectY - y0;
|
|
|
|
const size_t i00 = static_cast<size_t>(y0) * mapWidth + x0;
|
|
const size_t i10 = static_cast<size_t>(y0) * mapWidth + x1;
|
|
const size_t i01 = static_cast<size_t>(y1) * mapWidth + x0;
|
|
const size_t i11 = static_cast<size_t>(y1) * mapWidth + x1;
|
|
|
|
const double xTop = static_cast<double>(mapX[i00]) * (1.0 - tx) +
|
|
static_cast<double>(mapX[i10]) * tx;
|
|
const double xBottom = static_cast<double>(mapX[i01]) * (1.0 - tx) +
|
|
static_cast<double>(mapX[i11]) * tx;
|
|
const double yTop = static_cast<double>(mapY[i00]) * (1.0 - tx) +
|
|
static_cast<double>(mapY[i10]) * tx;
|
|
const double yBottom = static_cast<double>(mapY[i01]) * (1.0 - tx) +
|
|
static_cast<double>(mapY[i11]) * tx;
|
|
|
|
rawX = xTop * (1.0 - ty) + xBottom * ty;
|
|
rawY = yTop * (1.0 - ty) + yBottom * ty;
|
|
return std::isfinite(rawX) && std::isfinite(rawY);
|
|
}
|
|
|
|
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 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();
|
|
}
|
|
|
|
bool DroneScrewAlgoStub::buildRectifiedLeftToRawMap(const StereoBoltCalibC& calib,
|
|
double rectAlpha,
|
|
std::string& error)
|
|
{
|
|
m_rectMapWidth = 0;
|
|
m_rectMapHeight = 0;
|
|
m_leftRectToRawX.clear();
|
|
m_leftRectToRawY.clear();
|
|
|
|
if (calib.image_width <= 0 || calib.image_height <= 0)
|
|
{
|
|
error = "invalid calibration image size for rectified-to-raw map";
|
|
return false;
|
|
}
|
|
if (!std::isfinite(rectAlpha))
|
|
{
|
|
error = "invalid rectification alpha for rectified-to-raw map";
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
const cv::Size imageSize(calib.image_width, calib.image_height);
|
|
const cv::Mat K1 = mat64(3, 3, calib.left_K);
|
|
const cv::Mat D1 = mat64(1, 5, calib.left_D);
|
|
const cv::Mat K2 = mat64(3, 3, calib.right_K);
|
|
const cv::Mat D2 = mat64(1, 5, calib.right_D);
|
|
const cv::Mat R = mat64(3, 3, calib.R);
|
|
const cv::Mat T = mat64(3, 1, calib.T);
|
|
|
|
cv::Mat R1;
|
|
cv::Mat R2;
|
|
cv::Mat P1;
|
|
cv::Mat P2;
|
|
cv::Mat Q;
|
|
cv::Rect validRoi1;
|
|
cv::Rect validRoi2;
|
|
cv::stereoRectify(K1, D1, K2, D2, imageSize, R, T,
|
|
R1, R2, P1, P2, Q,
|
|
cv::CALIB_ZERO_DISPARITY,
|
|
rectAlpha,
|
|
imageSize,
|
|
&validRoi1,
|
|
&validRoi2);
|
|
|
|
cv::Mat mapX;
|
|
cv::Mat mapY;
|
|
cv::initUndistortRectifyMap(K1, D1, R1, P1, imageSize,
|
|
CV_32FC1, mapX, mapY);
|
|
|
|
if (mapX.empty() || mapY.empty() ||
|
|
mapX.cols != imageSize.width || mapX.rows != imageSize.height ||
|
|
mapY.cols != imageSize.width || mapY.rows != imageSize.height)
|
|
{
|
|
error = "OpenCV returned invalid rectified-to-raw map";
|
|
return false;
|
|
}
|
|
|
|
const size_t total = static_cast<size_t>(imageSize.width) *
|
|
static_cast<size_t>(imageSize.height);
|
|
m_leftRectToRawX.resize(total);
|
|
m_leftRectToRawY.resize(total);
|
|
for (int y = 0; y < imageSize.height; ++y)
|
|
{
|
|
const float* srcX = mapX.ptr<float>(y);
|
|
const float* srcY = mapY.ptr<float>(y);
|
|
const size_t offset = static_cast<size_t>(y) * imageSize.width;
|
|
std::copy(srcX, srcX + imageSize.width, m_leftRectToRawX.begin() + offset);
|
|
std::copy(srcY, srcY + imageSize.width, m_leftRectToRawY.begin() + offset);
|
|
}
|
|
|
|
m_calib = calib;
|
|
m_rectAlpha = rectAlpha;
|
|
m_rectMapWidth = imageSize.width;
|
|
m_rectMapHeight = imageSize.height;
|
|
return true;
|
|
}
|
|
catch (const cv::Exception& e)
|
|
{
|
|
error = std::string("OpenCV rectified-to-raw map failed: ") + e.what();
|
|
}
|
|
catch (const std::exception& e)
|
|
{
|
|
error = std::string("rectified-to-raw map failed: ") + e.what();
|
|
}
|
|
catch (...)
|
|
{
|
|
error = "rectified-to-raw map failed: unknown exception";
|
|
}
|
|
|
|
m_rectMapWidth = 0;
|
|
m_rectMapHeight = 0;
|
|
m_leftRectToRawX.clear();
|
|
m_leftRectToRawY.clear();
|
|
return false;
|
|
}
|
|
|
|
bool DroneScrewAlgoStub::mapRectifiedLeftRoi(const StereoBoltModuleRoiC& roi,
|
|
int rawWidth,
|
|
int rawHeight,
|
|
DroneScrewBox& box) const
|
|
{
|
|
if (roi.width <= 0 || roi.height <= 0)
|
|
return false;
|
|
if (m_rectMapWidth <= 0 || m_rectMapHeight <= 0 ||
|
|
m_leftRectToRawX.empty() || m_leftRectToRawY.empty())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
const int dstWidth = rawWidth > 0 ? rawWidth : m_rectMapWidth;
|
|
const int dstHeight = rawHeight > 0 ? rawHeight : m_rectMapHeight;
|
|
if (dstWidth <= 0 || dstHeight <= 0)
|
|
return false;
|
|
|
|
const double rectLeft = static_cast<double>(roi.x);
|
|
const double rectTop = static_cast<double>(roi.y);
|
|
const double rectRight = static_cast<double>(roi.x) + static_cast<double>(roi.width) - 1.0;
|
|
const double rectBottom = static_cast<double>(roi.y) + static_cast<double>(roi.height) - 1.0;
|
|
if (rectRight < 0.0 || rectBottom < 0.0 ||
|
|
rectLeft > static_cast<double>(m_rectMapWidth - 1) ||
|
|
rectTop > static_cast<double>(m_rectMapHeight - 1))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double minRawX = std::numeric_limits<double>::infinity();
|
|
double minRawY = std::numeric_limits<double>::infinity();
|
|
double maxRawX = -std::numeric_limits<double>::infinity();
|
|
double maxRawY = -std::numeric_limits<double>::infinity();
|
|
bool hasSample = false;
|
|
|
|
const int samplesX = roiSampleCount(rectRight - rectLeft);
|
|
const int samplesY = roiSampleCount(rectBottom - rectTop);
|
|
for (int sy = 0; sy < samplesY; ++sy)
|
|
{
|
|
const double ty = (samplesY <= 1) ? 0.0 : static_cast<double>(sy) / (samplesY - 1);
|
|
const double rectY = rectTop + (rectBottom - rectTop) * ty;
|
|
for (int sx = 0; sx < samplesX; ++sx)
|
|
{
|
|
const double tx = (samplesX <= 1) ? 0.0 : static_cast<double>(sx) / (samplesX - 1);
|
|
const double rectX = rectLeft + (rectRight - rectLeft) * tx;
|
|
|
|
double rawX = 0.0;
|
|
double rawY = 0.0;
|
|
if (!sampleMapBilinear(m_leftRectToRawX, m_leftRectToRawY,
|
|
m_rectMapWidth, m_rectMapHeight,
|
|
rectX, rectY, rawX, rawY))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
minRawX = std::min(minRawX, rawX);
|
|
minRawY = std::min(minRawY, rawY);
|
|
maxRawX = std::max(maxRawX, rawX);
|
|
maxRawY = std::max(maxRawY, rawY);
|
|
hasSample = true;
|
|
}
|
|
}
|
|
|
|
if (!hasSample)
|
|
return false;
|
|
if (maxRawX < 0.0 || maxRawY < 0.0 ||
|
|
minRawX > static_cast<double>(dstWidth - 1) ||
|
|
minRawY > static_cast<double>(dstHeight - 1))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
const int left = std::max(0, std::min(dstWidth - 1,
|
|
static_cast<int>(std::floor(minRawX))));
|
|
const int top = std::max(0, std::min(dstHeight - 1,
|
|
static_cast<int>(std::floor(minRawY))));
|
|
const int right = std::max(0, std::min(dstWidth - 1,
|
|
static_cast<int>(std::ceil(maxRawX))));
|
|
const int bottom = std::max(0, std::min(dstHeight - 1,
|
|
static_cast<int>(std::ceil(maxRawY))));
|
|
if (right < left || bottom < top)
|
|
return false;
|
|
|
|
box.classId = roi.class_id;
|
|
box.score = static_cast<float>(roi.score);
|
|
box.x = left;
|
|
box.y = top;
|
|
box.width = right - left + 1;
|
|
box.height = bottom - top + 1;
|
|
return true;
|
|
}
|
|
|
|
void DroneScrewAlgoStub::appendRoiBox(const StereoBoltModuleRoiC& roi,
|
|
DroneScrewResult& result) const
|
|
{
|
|
DroneScrewBox box;
|
|
if (mapRectifiedLeftRoi(roi, result.imageWidth, result.imageHeight, box))
|
|
result.boxes.push_back(box);
|
|
}
|
|
|
|
void DroneScrewAlgoStub::appendBoltBox(const StereoBoltModuleBoltC& bolt,
|
|
DroneScrewResult& result) const
|
|
{
|
|
DroneScrewBox box;
|
|
if (mapRectifiedLeftRoi(bolt.left_roi, result.imageWidth, result.imageHeight, box))
|
|
{
|
|
box.hasPhysicalHeight = true;
|
|
// height_mm is the physical bolt height used by CtrlApp display and txt storage.
|
|
box.physicalHeightMm = static_cast<float>(bolt.height_mm);
|
|
box.hasStereoMatch = bolt.idx_L >= 0 && bolt.idx_R >= 0 && bolt.pair_id >= 0;
|
|
box.trusted = bolt.confidence == 0;
|
|
box.matchConfidence = bolt.confidence;
|
|
box.matchStatus = bolt.status;
|
|
box.pairId = bolt.pair_id;
|
|
box.leftIndex = bolt.idx_L;
|
|
box.rightIndex = bolt.idx_R;
|
|
result.boxes.push_back(box);
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
m_rectMapWidth = 0;
|
|
m_rectMapHeight = 0;
|
|
m_leftRectToRawX.clear();
|
|
m_leftRectToRawY.clear();
|
|
|
|
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);
|
|
|
|
if (!buildRectifiedLeftToRawMap(calib, algoParams.rect_alpha, loadError))
|
|
{
|
|
m_initError = std::string("stereo_bolt build bbox rectified-to-raw map failed calib=") +
|
|
calibPath.toStdString() + " err=" + loadError;
|
|
LOG_ERROR("[ALGO] %s\n", m_initError.c_str());
|
|
return ERR_CODE(DEV_OPEN_ERR);
|
|
}
|
|
|
|
m_ctx = sb_create_ex(&calib, &model, &algoParams);
|
|
|
|
if (!m_ctx)
|
|
{
|
|
m_rectMapWidth = 0;
|
|
m_rectMapHeight = 0;
|
|
m_leftRectToRawX.clear();
|
|
m_leftRectToRawY.clear();
|
|
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;
|
|
}
|
|
m_rectMapWidth = 0;
|
|
m_rectMapHeight = 0;
|
|
m_leftRectToRawX.clear();
|
|
m_leftRectToRawY.clear();
|
|
|
|
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)
|
|
appendBoltBox(out.data.bolts[i], 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
|