Files
ANSCORE/modules/ANSODEngine/ANSRTRTMPOSE.cpp
T

848 lines
41 KiB
C++
Raw Normal View History

2026-05-14 17:48:09 +10:00
#include "ANSRTRTMPOSE.h"
#include "Utility.h"
#include "ANSLicense.h" // ANS_DBG macro for DebugView
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudawarping.hpp>
#include <algorithm>
#include <cmath>
namespace ANSCENTER {
// ====================================================================
// ANSODBase interface — OptimizeModel
// ====================================================================
bool ANSRTRTMPOSE::OptimizeModel(bool fp16, std::string& optimizedModelFolder) {
std::lock_guard<std::recursive_mutex> lock(_mutex);
ANS_DBG("ANSRTRTMPOSE", "OptimizeModel: fp16=%d", fp16 ? 1 : 0);
if (!ANSODBase::OptimizeModel(fp16, optimizedModelFolder)) return false;
if (!FileExist(_modelFilePath)) {
_logger.LogFatal("ANSRTRTMPOSE::OptimizeModel",
"Raw model file path does not exist: " + _modelFilePath, __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "OptimizeModel: missing onnx '%s'", _modelFilePath.c_str());
return false;
}
try {
_fp16 = fp16;
optimizedModelFolder = GetParentFolder(_modelFilePath);
if (!m_trtEngine) {
m_options.optBatchSize = _modelConfig.gpuOptBatchSize;
m_options.maxBatchSize = _modelConfig.gpuMaxBatchSize;
m_options.deviceIndex = _modelConfig.gpuDeviceIndex;
m_options.maxInputHeight = _modelConfig.maxInputHeight;
m_options.minInputHeight = _modelConfig.minInputHeight;
m_options.optInputHeight = _modelConfig.optInputHeight;
m_options.maxInputWidth = _modelConfig.maxInputWidth;
m_options.minInputWidth = _modelConfig.minInputWidth;
m_options.optInputWidth = _modelConfig.optInputWidth;
m_options.engineFileDir = optimizedModelFolder;
m_options.precision = (_fp16 ? Precision::FP16 : Precision::FP32);
m_trtEngine = std::make_shared<Engine<float>>(m_options);
}
auto succ = m_trtEngine->buildWithRetry(_modelFilePath, SUB_VALS, DIV_VALS, NORMALIZE);
if (!succ) {
_logger.LogError("ANSRTRTMPOSE::OptimizeModel",
"Error: Unable to build TensorRT engine. " + _modelFilePath, __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "OptimizeModel: buildWithRetry FAILED for '%s'", _modelFilePath.c_str());
return false;
}
ANS_DBG("ANSRTRTMPOSE", "OptimizeModel: build OK -> '%s'", optimizedModelFolder.c_str());
2026-05-14 19:05:20 +10:00
// Also build the bundled person-detector engine if present.
// TryLoadPersonDetector triggers ANSRTYOLO::LoadModelFromFolder
// which performs the engine build (= optimization) and caches
// it to disk; the next Initialize/LoadModel will hit the cache.
(void)TryLoadPersonDetector(_modelFolder);
2026-05-14 17:48:09 +10:00
return true;
}
catch (std::exception& e) {
_logger.LogFatal("ANSRTRTMPOSE::OptimizeModel", e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "OptimizeModel: EXCEPTION '%s'", e.what());
return false;
}
}
// ====================================================================
// ANSODBase interface — LoadModel
// ====================================================================
bool ANSRTRTMPOSE::LoadModel(const std::string& modelZipFilePath,
const std::string& modelZipPassword) {
std::lock_guard<std::recursive_mutex> lock(_mutex);
ModelLoadingGuard mlg(_modelLoading);
ANS_DBG("ANSRTRTMPOSE", "LoadModel: zip='%s'", modelZipFilePath.c_str());
try {
bool result = ANSODBase::LoadModel(modelZipFilePath, modelZipPassword);
if (!result) return false;
_modelConfig.modelType = ModelType::RTRTMPOSE;
// RTMPose has fixed input dims — we still propagate them through ModelConfig
// so the rest of the pipeline (caching, logging) can introspect them.
if (_modelConfig.inpHeight <= 0) _modelConfig.inpHeight = RTM_INPUT_H;
if (_modelConfig.inpWidth <= 0) _modelConfig.inpWidth = RTM_INPUT_W;
if (_modelConfig.numKPS <= 0 || _modelConfig.numKPS > 133)
_modelConfig.numKPS = 26; // default to RTMPose body 2d-26
if (_modelConfig.kpsThreshold <= 0)
_modelConfig.kpsThreshold = KPS_THRESHOLD;
_fp16 = true;
KPS_THRESHOLD = _modelConfig.kpsThreshold;
m_options.optBatchSize = _modelConfig.gpuOptBatchSize;
m_options.maxBatchSize = _modelConfig.gpuMaxBatchSize;
m_options.deviceIndex = _modelConfig.gpuDeviceIndex;
m_options.maxInputHeight = _modelConfig.maxInputHeight;
m_options.minInputHeight = _modelConfig.minInputHeight;
m_options.optInputHeight = _modelConfig.optInputHeight;
m_options.maxInputWidth = _modelConfig.maxInputWidth;
m_options.minInputWidth = _modelConfig.minInputWidth;
m_options.optInputWidth = _modelConfig.optInputWidth;
m_options.engineFileDir = _modelFolder;
m_options.precision = (_fp16 ? Precision::FP16 : Precision::FP32);
_modelFilePath = CreateFilePath(_modelFolder, "train_last.onnx");
if (FileExist(_modelConfigFile)) {
ModelType modelType;
std::vector<int> inputShape;
_classes = ANSUtilityHelper::GetConfigFileContent(_modelConfigFile, modelType, inputShape);
if (inputShape.size() == 2) {
if (inputShape[0] > 0) _modelConfig.inpHeight = inputShape[0];
if (inputShape[1] > 0) _modelConfig.inpWidth = inputShape[1];
}
}
else {
_classFilePath = CreateFilePath(_modelFolder, "classes.names");
std::ifstream isValid(_classFilePath);
if (!isValid) LoadClassesFromString();
else LoadClassesFromFile();
}
if (this->_loadEngineOnCreation) {
if (!m_trtEngine) {
m_poolKey = { _modelFilePath,
static_cast<int>(m_options.precision), m_options.maxBatchSize };
m_trtEngine = EnginePoolManager<float>::instance().acquire(
m_poolKey, m_options, _modelFilePath,
SUB_VALS, DIV_VALS, NORMALIZE, m_maxSlotsPerGpu);
m_usingSharedPool = (m_trtEngine != nullptr);
}
if (!m_trtEngine) {
_logger.LogError("ANSRTRTMPOSE::LoadModel",
"Error: Unable to load TensorRT engine. " + _modelFilePath, __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "LoadModel: engine acquire FAILED '%s'", _modelFilePath.c_str());
_modelLoadValid = false;
return false;
}
m_options.maxBatchSize = m_trtEngine->getOptions().maxBatchSize;
m_options.optBatchSize = m_trtEngine->getOptions().optBatchSize;
m_trtEngine->warmUp();
// Pull true input geometry from the engine; RTMPose is fixed-size,
// but reading it back protects us against unexpected ONNX exports.
const auto& inputDims = m_trtEngine->getInputDims();
if (!inputDims.empty()) {
if (inputDims[0].d[1] > 0) RTM_INPUT_H = inputDims[0].d[1];
if (inputDims[0].d[2] > 0) RTM_INPUT_W = inputDims[0].d[2];
}
ANS_DBG("ANSRTRTMPOSE", "LoadModel: engine ready %dx%d kps=%d sharedPool=%d",
RTM_INPUT_W, RTM_INPUT_H, _modelConfig.numKPS, m_usingSharedPool ? 1 : 0);
}
2026-05-14 19:05:20 +10:00
// Top-down pipeline: try to load the bundled person detector.
// Falls back gracefully (whole-image pose) when the files are missing.
(void)TryLoadPersonDetector(_modelFolder);
2026-05-14 17:48:09 +10:00
_modelLoadValid = true;
_isInitialized = true;
return true;
}
catch (std::exception& e) {
_logger.LogFatal("ANSRTRTMPOSE::LoadModel", e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "LoadModel: EXCEPTION '%s'", e.what());
return false;
}
}
// ====================================================================
// ANSODBase interface — LoadModelFromFolder
// ====================================================================
bool ANSRTRTMPOSE::LoadModelFromFolder(std::string licenseKey, ModelConfig modelConfig,
std::string modelName, std::string className,
const std::string& modelFolder, std::string& labelMap) {
std::lock_guard<std::recursive_mutex> lock(_mutex);
ModelLoadingGuard mlg(_modelLoading);
ANS_DBG("ANSRTRTMPOSE", "LoadModelFromFolder: folder='%s' model='%s'",
modelFolder.c_str(), modelName.c_str());
try {
bool result = ANSODBase::LoadModelFromFolder(licenseKey, modelConfig,
modelName, className, modelFolder, labelMap);
if (!result) return false;
_modelConfig = modelConfig;
_modelConfig.modelType = ModelType::RTRTMPOSE;
if (_modelConfig.inpHeight <= 0) _modelConfig.inpHeight = RTM_INPUT_H;
if (_modelConfig.inpWidth <= 0) _modelConfig.inpWidth = RTM_INPUT_W;
_modelConfig.precisionType = PrecisionType::FP32;
if (_modelConfig.numKPS <= 0 || _modelConfig.numKPS > 133)
_modelConfig.numKPS = 26;
if (_modelConfig.kpsThreshold <= 0)
_modelConfig.kpsThreshold = KPS_THRESHOLD;
_fp16 = true;
KPS_THRESHOLD = _modelConfig.kpsThreshold;
std::string _modelName = modelName;
if (_modelName.empty()) _modelName = "train_last";
std::string modelFullName = _modelName + ".onnx";
m_options.optBatchSize = _modelConfig.gpuOptBatchSize;
m_options.maxBatchSize = _modelConfig.gpuMaxBatchSize;
m_options.deviceIndex = _modelConfig.gpuDeviceIndex;
m_options.maxInputHeight = _modelConfig.maxInputHeight;
m_options.minInputHeight = _modelConfig.minInputHeight;
m_options.optInputHeight = _modelConfig.optInputHeight;
m_options.maxInputWidth = _modelConfig.maxInputWidth;
m_options.minInputWidth = _modelConfig.minInputWidth;
m_options.optInputWidth = _modelConfig.optInputWidth;
m_options.engineFileDir = _modelFolder;
m_options.precision = (_fp16 ? Precision::FP16 : Precision::FP32);
_modelFilePath = CreateFilePath(_modelFolder, modelFullName);
if (FileExist(_modelConfigFile)) {
ModelType modelType;
std::vector<int> inputShape;
_classes = ANSUtilityHelper::GetConfigFileContent(_modelConfigFile, modelType, inputShape);
if (inputShape.size() == 2) {
if (inputShape[0] > 0) _modelConfig.inpHeight = inputShape[0];
if (inputShape[1] > 0) _modelConfig.inpWidth = inputShape[1];
}
}
else {
_classFilePath = CreateFilePath(_modelFolder, className);
std::ifstream isValid(_classFilePath);
if (!isValid) LoadClassesFromString();
else LoadClassesFromFile();
}
labelMap.clear();
if (!_classes.empty())
labelMap = VectorToCommaSeparatedString(_classes);
if (this->_loadEngineOnCreation) {
if (!m_trtEngine) {
m_poolKey = { _modelFilePath,
static_cast<int>(m_options.precision), m_options.maxBatchSize };
m_trtEngine = EnginePoolManager<float>::instance().acquire(
m_poolKey, m_options, _modelFilePath,
SUB_VALS, DIV_VALS, NORMALIZE, m_maxSlotsPerGpu);
m_usingSharedPool = (m_trtEngine != nullptr);
}
if (!m_trtEngine) {
_logger.LogError("ANSRTRTMPOSE::LoadModelFromFolder",
"Error: Unable to load TensorRT engine. " + _modelFilePath, __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "LoadModelFromFolder: engine acquire FAILED '%s'",
_modelFilePath.c_str());
_modelLoadValid = false;
return false;
}
m_options.maxBatchSize = m_trtEngine->getOptions().maxBatchSize;
m_options.optBatchSize = m_trtEngine->getOptions().optBatchSize;
m_trtEngine->warmUp();
const auto& inputDims = m_trtEngine->getInputDims();
if (!inputDims.empty()) {
if (inputDims[0].d[1] > 0) RTM_INPUT_H = inputDims[0].d[1];
if (inputDims[0].d[2] > 0) RTM_INPUT_W = inputDims[0].d[2];
}
ANS_DBG("ANSRTRTMPOSE", "LoadModelFromFolder: engine ready %dx%d kps=%d sharedPool=%d",
RTM_INPUT_W, RTM_INPUT_H, _modelConfig.numKPS, m_usingSharedPool ? 1 : 0);
}
2026-05-14 19:05:20 +10:00
(void)TryLoadPersonDetector(_modelFolder);
2026-05-14 17:48:09 +10:00
_modelLoadValid = true;
_isInitialized = true;
return true;
}
catch (std::exception& e) {
_logger.LogFatal("ANSRTRTMPOSE::LoadModelFromFolder", e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "LoadModelFromFolder: EXCEPTION '%s'", e.what());
return false;
}
}
// ====================================================================
// ANSODBase interface — Initialize
// ====================================================================
bool ANSRTRTMPOSE::Initialize(std::string licenseKey, ModelConfig modelConfig,
const std::string& modelZipFilePath,
const std::string& modelZipPassword,
std::string& labelMap) {
std::lock_guard<std::recursive_mutex> lock(_mutex);
ModelLoadingGuard mlg(_modelLoading);
ANS_DBG("ANSRTRTMPOSE", "Initialize: zip='%s'", modelZipFilePath.c_str());
try {
const bool engineAlreadyLoaded =
_modelLoadValid && _isInitialized && m_trtEngine != nullptr;
_modelLoadValid = false;
bool result = ANSODBase::Initialize(licenseKey, modelConfig,
modelZipFilePath, modelZipPassword, labelMap);
if (!result) return false;
_modelConfig = modelConfig;
_modelConfig.modelType = ModelType::RTRTMPOSE;
if (_modelConfig.inpHeight <= 0) _modelConfig.inpHeight = RTM_INPUT_H;
if (_modelConfig.inpWidth <= 0) _modelConfig.inpWidth = RTM_INPUT_W;
_modelConfig.precisionType = PrecisionType::FP32;
if (_modelConfig.numKPS <= 0 || _modelConfig.numKPS > 133)
_modelConfig.numKPS = 26;
if (_modelConfig.kpsThreshold <= 0)
_modelConfig.kpsThreshold = KPS_THRESHOLD;
_fp16 = true;
KPS_THRESHOLD = _modelConfig.kpsThreshold;
m_options.optBatchSize = _modelConfig.gpuOptBatchSize;
m_options.maxBatchSize = _modelConfig.gpuMaxBatchSize;
m_options.deviceIndex = _modelConfig.gpuDeviceIndex;
m_options.maxInputHeight = _modelConfig.maxInputHeight;
m_options.minInputHeight = _modelConfig.minInputHeight;
m_options.optInputHeight = _modelConfig.optInputHeight;
m_options.maxInputWidth = _modelConfig.maxInputWidth;
m_options.minInputWidth = _modelConfig.minInputWidth;
m_options.optInputWidth = _modelConfig.optInputWidth;
m_options.engineFileDir = _modelFolder;
m_options.precision = (_fp16 ? Precision::FP16 : Precision::FP32);
_modelFilePath = CreateFilePath(_modelFolder, "train_last.onnx");
if (FileExist(_modelConfigFile)) {
ModelType modelType;
std::vector<int> inputShape;
_classes = ANSUtilityHelper::GetConfigFileContent(_modelConfigFile, modelType, inputShape);
if (inputShape.size() == 2) {
if (inputShape[0] > 0) _modelConfig.inpHeight = inputShape[0];
if (inputShape[1] > 0) _modelConfig.inpWidth = inputShape[1];
}
}
else {
_classFilePath = CreateFilePath(_modelFolder, "classes.names");
std::ifstream isValid(_classFilePath);
if (!isValid) LoadClassesFromString();
else LoadClassesFromFile();
}
labelMap.clear();
if (!_classes.empty())
labelMap = VectorToCommaSeparatedString(_classes);
if (this->_loadEngineOnCreation && !engineAlreadyLoaded) {
if (!m_trtEngine) {
m_poolKey = { _modelFilePath,
static_cast<int>(m_options.precision), m_options.maxBatchSize };
m_trtEngine = EnginePoolManager<float>::instance().acquire(
m_poolKey, m_options, _modelFilePath,
SUB_VALS, DIV_VALS, NORMALIZE, m_maxSlotsPerGpu);
m_usingSharedPool = (m_trtEngine != nullptr);
}
if (!m_trtEngine) {
_logger.LogError("ANSRTRTMPOSE::Initialize",
"Error: Unable to load TensorRT engine. " + _modelFilePath, __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "Initialize: engine acquire FAILED '%s'", _modelFilePath.c_str());
_modelLoadValid = false;
return false;
}
m_options.maxBatchSize = m_trtEngine->getOptions().maxBatchSize;
m_options.optBatchSize = m_trtEngine->getOptions().optBatchSize;
m_trtEngine->warmUp();
const auto& inputDims = m_trtEngine->getInputDims();
if (!inputDims.empty()) {
if (inputDims[0].d[1] > 0) RTM_INPUT_H = inputDims[0].d[1];
if (inputDims[0].d[2] > 0) RTM_INPUT_W = inputDims[0].d[2];
}
ANS_DBG("ANSRTRTMPOSE", "Initialize: engine ready %dx%d kps=%d sharedPool=%d",
RTM_INPUT_W, RTM_INPUT_H, _modelConfig.numKPS, m_usingSharedPool ? 1 : 0);
}
2026-05-14 19:05:20 +10:00
(void)TryLoadPersonDetector(_modelFolder);
2026-05-14 17:48:09 +10:00
_modelLoadValid = true;
_isInitialized = true;
return true;
}
catch (std::exception& e) {
_logger.LogFatal("ANSRTRTMPOSE::Initialize", e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "Initialize: EXCEPTION '%s'", e.what());
return false;
}
}
// ====================================================================
// RunInference / Destroy / Destructor
// ====================================================================
std::vector<Object> ANSRTRTMPOSE::RunInference(const cv::Mat& inputImgBGR) {
return RunInference(inputImgBGR, "");
}
std::vector<Object> ANSRTRTMPOSE::RunInference(const cv::Mat& inputImgBGR,
const std::string& camera_id) {
if (!PreInferenceCheck("ANSRTRTMPOSE::RunInference")) return {};
if (inputImgBGR.empty()) {
ANS_DBG("ANSRTRTMPOSE", "RunInference: empty input cam='%s'", camera_id.c_str());
return {};
}
try {
2026-05-14 19:05:20 +10:00
// Two pipelines:
//
// (a) Top-down (preferred) — m_personDetector loaded from
// the bundled person.onnx + person.names. Detect people
// first, then run RTMPose ONCE per person bbox. This is
// what RTMPose is designed for; works on multi-person
// scenes with small / distant subjects.
//
// (b) Standalone fallback — no detector files in the model
// folder. Treat the whole image as a single person crop.
// Only sensible for inputs that are already a person
// (selfies, single-subject fitness clips, unit-test
// crops). Higher-level callers can also use the
// ANSODBase::RunInference(input, vector<Rect>, cam)
// overload which crops each bbox and routes per-crop
// through our Preprocess.
if (m_personDetector) {
std::vector<Object> persons = m_personDetector->RunInference(inputImgBGR, camera_id);
if (persons.empty()) {
ANS_DBG("ANSRTRTMPOSE", "RunInference: detector found 0 persons cam='%s'",
camera_id.c_str());
return {};
}
std::vector<Object> results;
results.reserve(persons.size());
for (const auto& person : persons) {
// person.onnx is a single-class model — accept anything
// the detector returned, but defend against degenerate
// boxes (zero/negative area, off-image).
if (person.box.width <= 0 || person.box.height <= 0) continue;
auto poseResults = InferenceOnBox(inputImgBGR, person.box, camera_id);
for (auto& obj : poseResults) {
// Carry the detector's track-id and confidence so
// downstream tracking/visualization stays coherent
// across frames. RTMPose itself has no class /
// tracking concept of its own.
obj.trackId = person.trackId;
obj.confidence = person.confidence;
if (!person.className.empty()) obj.className = person.className;
results.push_back(std::move(obj));
}
}
ANS_DBG("ANSRTRTMPOSE", "RunInference: %zu persons -> %zu poses cam='%s'",
persons.size(), results.size(), camera_id.c_str());
return results;
}
// Fallback — single-person whole-image pose
2026-05-14 17:48:09 +10:00
const cv::Rect wholeImage(0, 0, inputImgBGR.cols, inputImgBGR.rows);
return InferenceOnBox(inputImgBGR, wholeImage, camera_id);
}
catch (const std::exception& e) {
_logger.LogFatal("ANSRTRTMPOSE::RunInference", e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "RunInference: EXCEPTION '%s'", e.what());
return {};
}
}
ANSRTRTMPOSE::~ANSRTRTMPOSE() {
try { Destroy(); }
catch (std::exception& e) {
_logger.LogError("ANSRTRTMPOSE::~ANSRTRTMPOSE()", e.what(), __FILE__, __LINE__);
}
}
bool ANSRTRTMPOSE::Destroy() {
try {
2026-05-14 19:05:20 +10:00
if (m_personDetector) {
m_personDetector->Destroy();
m_personDetector.reset();
}
2026-05-14 17:48:09 +10:00
if (m_usingSharedPool) {
EnginePoolManager<float>::instance().release(m_poolKey);
m_trtEngine.reset();
m_usingSharedPool = false;
}
else {
m_trtEngine.reset();
}
ANS_DBG("ANSRTRTMPOSE", "Destroy: engine released");
return true;
}
catch (std::exception& e) {
_logger.LogError("ANSRTRTMPOSE::Destroy()", e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "Destroy: EXCEPTION '%s'", e.what());
return false;
}
}
// ====================================================================
// Per-bbox single-shot inference pipeline
// ====================================================================
std::vector<Object> ANSRTRTMPOSE::InferenceOnBox(const cv::Mat& input,
const cv::Rect& box,
const std::string& camera_id) {
if (!m_trtEngine) {
ANS_DBG("ANSRTRTMPOSE", "InferenceOnBox: engine null");
return {};
}
if (box.width <= 0 || box.height <= 0) {
ANS_DBG("ANSRTRTMPOSE", "InferenceOnBox: invalid box %dx%d", box.width, box.height);
return {};
}
m_trtEngine->setDeviceContext();
// Phase 1 — preprocess under lock (affine warp on CPU, upload to GPU)
ImageMetadata meta;
std::vector<std::vector<cv::cuda::GpuMat>> trtInput;
{
std::lock_guard<std::recursive_mutex> lock(_mutex);
trtInput = Preprocess(input, box, meta);
}
if (trtInput.empty() || trtInput[0].empty()) {
ANS_DBG("ANSRTRTMPOSE", "InferenceOnBox: preprocess failed box=%d,%d %dx%d",
box.x, box.y, box.width, box.height);
return {};
}
// Phase 2 — inference (mutex released; pool dispatches to idle slot)
std::vector<std::vector<std::vector<float>>> featureVectors;
if (!m_trtEngine->runInference(trtInput, featureVectors)) {
_logger.LogError("ANSRTRTMPOSE::InferenceOnBox", "runInference failed", __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "InferenceOnBox: runInference FAILED");
return {};
}
if (featureVectors.empty() || featureVectors[0].size() < 2) {
_logger.LogError("ANSRTRTMPOSE::InferenceOnBox",
"Expected 2 SimCC outputs (x,y), got " +
std::to_string(featureVectors.empty() ? 0 : featureVectors[0].size()),
__FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "InferenceOnBox: bad output count outer=%zu inner=%zu",
featureVectors.size(),
featureVectors.empty() ? size_t{ 0 } : featureVectors[0].size());
return {};
}
// Phase 3 — postprocess. Output ordering in featureVectors matches the
// engine's output binding order; for RTMPose ONNX exports that's
// [simcc_x, simcc_y]. We disambiguate by feature length anyway.
const auto& outputDims = m_trtEngine->getOutputDims();
if (outputDims.size() < 2 ||
outputDims[0].nbDims < 3 || outputDims[1].nbDims < 3) {
_logger.LogError("ANSRTRTMPOSE::InferenceOnBox",
"Unexpected output dims (need 2 outputs of rank>=3)", __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "InferenceOnBox: outputDims rank issue count=%zu",
outputDims.size());
return {};
}
const int K0 = outputDims[0].d[1];
const int W0 = outputDims[0].d[2];
const int K1 = outputDims[1].d[1];
const int H1 = outputDims[1].d[2];
if (K0 != K1) {
_logger.LogError("ANSRTRTMPOSE::InferenceOnBox",
"SimCC keypoint count mismatch x=" + std::to_string(K0) +
" y=" + std::to_string(K1), __FILE__, __LINE__);
return {};
}
// Pick which output is X (longer of the model-input dims) vs Y.
// For 192x256 input, ext_w=384 is shorter than ext_h=512, but the order
// of the two SimCC outputs in the ONNX is fixed, so we identify by
// length proportional to RTM_INPUT_W vs RTM_INPUT_H rather than by
// assuming x-first.
const std::vector<float>* simcc_x = &featureVectors[0][0];
const std::vector<float>* simcc_y = &featureVectors[0][1];
int extW = W0;
int extH = H1;
const float ratioFirst = (RTM_INPUT_W > 0)
? static_cast<float>(W0) / static_cast<float>(RTM_INPUT_W) : 0.f;
const float ratioSecond = (RTM_INPUT_H > 0)
? static_cast<float>(W0) / static_cast<float>(RTM_INPUT_H) : 0.f;
// If the first output's last-dim looks like H (proportional to input H,
// not W), swap so simcc_x indexes the W-axis.
if (std::abs(ratioFirst - 2.f) > std::abs(ratioSecond - 2.f)) {
std::swap(simcc_x, simcc_y);
extW = H1;
extH = W0;
}
std::lock_guard<std::recursive_mutex> lock(_mutex);
Object obj = PostprocessSimCC(*simcc_x, *simcc_y, K0, extW, extH, meta, camera_id);
std::vector<Object> out;
if (!obj.kps.empty()) out.push_back(std::move(obj));
ANS_DBG("ANSRTRTMPOSE", "InferenceOnBox: kps=%d cam='%s'",
out.empty() ? 0 : K0, camera_id.c_str());
return out;
}
// ====================================================================
// Preprocess — affine crop -> upload BGR (Engine handles RGB norm)
//
// NOTE on color order: Engine<float>::runInference calls blobFromGpuMats
// with swapRB=false (see EngineRunInference.inl). So the GpuMat we hand
// it is interpreted channel-by-channel as the model input. RTMPose was
// trained on RGB, so we cvtColor BGR -> RGB before upload (mirroring
// ANSTENSORRTPOSE / ANSRTYOLO).
// ====================================================================
std::vector<std::vector<cv::cuda::GpuMat>> ANSRTRTMPOSE::Preprocess(
const cv::Mat& inputImage, const cv::Rect& box, ImageMetadata& outMeta) {
try {
if (!_licenseValid) {
_logger.LogFatal("ANSRTRTMPOSE::Preprocess", "Invalid license", __FILE__, __LINE__);
return {};
}
cv::Mat srcImg = inputImage;
if (srcImg.channels() == 1) {
cv::cvtColor(srcImg, srcImg, cv::COLOR_GRAY2BGR);
}
outMeta.imgHeight = static_cast<float>(srcImg.rows);
outMeta.imgWidth = static_cast<float>(srcImg.cols);
outMeta.personBox = box;
if (outMeta.imgHeight <= 0 || outMeta.imgWidth <= 0) return {};
cv::Mat crop;
CropImageByDetectBox(srcImg, box, crop, outMeta.affine_rev);
if (crop.empty()) {
ANS_DBG("ANSRTRTMPOSE", "Preprocess: warpAffine produced empty crop");
return {};
}
cv::Mat rgb;
cv::cvtColor(crop, rgb, cv::COLOR_BGR2RGB);
cv::cuda::Stream stream;
cv::cuda::GpuMat gpuRGB;
gpuRGB.upload(rgb, stream);
stream.waitForCompletion();
std::vector<cv::cuda::GpuMat> input{ std::move(gpuRGB) };
std::vector<std::vector<cv::cuda::GpuMat>> inputs{ std::move(input) };
return inputs;
}
catch (const std::exception& e) {
_logger.LogWarn("ANSRTRTMPOSE::Preprocess",
std::string("Skipped frame: ") + e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "Preprocess: EXCEPTION '%s'", e.what());
return {};
}
}
// ====================================================================
// PostprocessSimCC
//
// For each keypoint k:
// px = argmax(simcc_x[k]) ; sx = simcc_x[k][px]
// py = argmax(simcc_y[k]) ; sy = simcc_y[k][py]
// score = max(sx, sy)
// raw_x = px * 0.5 ; raw_y = py * 0.5
// [x, y]^T = affine_rev * [raw_x, raw_y, 1]^T
//
// This matches RTMPose::Inference in rtmpose_tensorrt.cpp.
// ====================================================================
Object ANSRTRTMPOSE::PostprocessSimCC(const std::vector<float>& simcc_x,
const std::vector<float>& simcc_y,
int numKeypoints, int extW, int extH,
const ImageMetadata& meta,
const std::string& camera_id) {
Object obj{};
if (numKeypoints <= 0 || extW <= 0 || extH <= 0) return obj;
if (static_cast<int>(simcc_x.size()) < numKeypoints * extW ||
static_cast<int>(simcc_y.size()) < numKeypoints * extH) {
_logger.LogError("ANSRTRTMPOSE::PostprocessSimCC",
"SimCC buffer underflow: x=" + std::to_string(simcc_x.size()) +
" y=" + std::to_string(simcc_y.size()) +
" expected " + std::to_string(numKeypoints * extW) + "/" +
std::to_string(numKeypoints * extH), __FILE__, __LINE__);
return obj;
}
const double m0 = meta.affine_rev.at<double>(0, 0);
const double m1 = meta.affine_rev.at<double>(0, 1);
const double m2 = meta.affine_rev.at<double>(0, 2);
const double m3 = meta.affine_rev.at<double>(1, 0);
const double m4 = meta.affine_rev.at<double>(1, 1);
const double m5 = meta.affine_rev.at<double>(1, 2);
std::vector<float> kps;
kps.reserve(numKeypoints * 3);
std::vector<cv::Point2f> polygon;
polygon.reserve(numKeypoints);
std::stringstream xss, yss;
float meanScore = 0.f;
int meanCount = 0;
for (int i = 0; i < numKeypoints; ++i) {
const float* xPtr = simcc_x.data() + static_cast<size_t>(i) * extW;
const float* yPtr = simcc_y.data() + static_cast<size_t>(i) * extH;
const int px = static_cast<int>(std::max_element(xPtr, xPtr + extW) - xPtr);
const int py = static_cast<int>(std::max_element(yPtr, yPtr + extH) - yPtr);
const float score = std::max(xPtr[px], yPtr[py]);
// SimCC oversampling factor 2 — same constant as the reference impl.
const double raw_x = px * 0.5;
const double raw_y = py * 0.5;
float worldX = static_cast<float>(m0 * raw_x + m1 * raw_y + m2);
float worldY = static_cast<float>(m3 * raw_x + m4 * raw_y + m5);
// Clamp into image bounds so downstream code can index safely.
worldX = std::clamp(worldX, 0.f, meta.imgWidth - 1.f);
worldY = std::clamp(worldY, 0.f, meta.imgHeight - 1.f);
kps.push_back(worldX);
kps.push_back(worldY);
kps.push_back(score);
polygon.emplace_back(worldX, worldY);
xss << worldX;
yss << worldY;
if (i + 1 < numKeypoints) { xss << ";"; yss << ";"; }
if (score >= KPS_THRESHOLD) { meanScore += score; ++meanCount; }
}
obj.classId = 0;
obj.className = _classes.empty() ? std::string("person") : _classes.front();
obj.confidence = (meanCount > 0) ? meanScore / meanCount : 0.f;
obj.box = meta.personBox;
obj.kps = std::move(kps);
obj.polygon = std::move(polygon);
obj.cameraId = camera_id;
obj.extraInfo = xss.str() + "|" + yss.str();
return obj;
}
// ====================================================================
// Affine helpers — direct port of utils.h / rtmpose_tensorrt.cpp
// ====================================================================
cv::Mat ANSRTRTMPOSE::GetAffineTransform(float center_x, float center_y,
float scale_w, float scale_h,
int output_w, int output_h, bool inverse) {
cv::Point2f src1(center_x, center_y);
cv::Point2f src2(center_x, center_y - scale_w * 0.5f);
cv::Point2f src3(src2.x - (src1.y - src2.y),
src2.y + (src1.x - src2.x));
const float dcx = output_w * 0.5f;
const float dcy = output_h * 0.5f;
cv::Point2f dst1(dcx, dcy);
cv::Point2f dst2(dcx, dcy - output_w * 0.5f);
cv::Point2f dst3(dst2.x - (dst1.y - dst2.y),
dst2.y + (dst1.x - dst2.x));
cv::Point2f srcPts[3] = { src1, src2, src3 };
cv::Point2f dstPts[3] = { dst1, dst2, dst3 };
return inverse ? cv::getAffineTransform(dstPts, srcPts)
: cv::getAffineTransform(srcPts, dstPts);
}
void ANSRTRTMPOSE::CropImageByDetectBox(const cv::Mat& image, const cv::Rect& box,
cv::Mat& crop, cv::Mat& affine_rev) {
// Aspect-fit the model's W:H (e.g. 192:256) over the bbox, then expand
// by 1.2x for context — same heuristic as the reference RTMPose impl.
float w = static_cast<float>(box.width);
float h = static_cast<float>(box.height);
const float cx = box.x + w * 0.5f;
const float cy = box.y + h * 0.5f;
const float aspect = static_cast<float>(RTM_INPUT_W) /
static_cast<float>(RTM_INPUT_H);
if (w > aspect * h) h = w / aspect;
else w = h * aspect;
const float scale_w = w * 1.2f;
const float scale_h = h * 1.2f;
cv::Mat affine = GetAffineTransform(cx, cy, scale_w, scale_h,
RTM_INPUT_W, RTM_INPUT_H, false);
affine_rev = GetAffineTransform(cx, cy, scale_w, scale_h,
RTM_INPUT_W, RTM_INPUT_H, true);
cv::warpAffine(image, crop, affine,
cv::Size(RTM_INPUT_W, RTM_INPUT_H), cv::INTER_LINEAR);
}
2026-05-14 19:05:20 +10:00
// ====================================================================
// TryLoadPersonDetector
//
// Look for person.onnx + person.names alongside the pose model. If
// both exist, instantiate an ANSRTYOLO front-end and load it via
// LoadModelFromFolder (which builds / loads the TRT engine from
// cache). On success m_personDetector is non-null and RunInference
// switches to the proper top-down pipeline.
//
// Missing files are NOT an error — the wrapper is designed to work
// standalone for single-person test inputs. We log a debug line so
// it's clear which path is active and return false.
// ====================================================================
bool ANSRTRTMPOSE::TryLoadPersonDetector(const std::string& modelFolder) {
const std::string personOnnx = CreateFilePath(modelFolder, "person.onnx");
const std::string personNames = CreateFilePath(modelFolder, "person.names");
if (!FileExist(personOnnx) || !FileExist(personNames)) {
ANS_DBG("ANSRTRTMPOSE",
"TryLoadPersonDetector: person.onnx/person.names not in '%s' "
"-- standalone mode (whole-image pose)", modelFolder.c_str());
return false;
}
if (m_personDetector) {
ANS_DBG("ANSRTRTMPOSE", "TryLoadPersonDetector: already loaded, skipping");
return true;
}
try {
auto detector = std::make_unique<ANSRTYOLO>();
detector->SetLoadEngineOnCreation(this->_loadEngineOnCreation);
// SetMaxSlotsPerGpu is declared private in ANSRTYOLO (it's a
// private override of ANSODBase's public virtual). Call it via
// the base-class interface where access is public.
static_cast<ANSODBase*>(detector.get())->SetMaxSlotsPerGpu(m_maxSlotsPerGpu);
ModelConfig personCfg{};
personCfg.detectionType = DetectionType::DETECTION;
personCfg.modelType = ModelType::RTYOLO;
personCfg.precisionType = PrecisionType::FP16;
personCfg.inpHeight = 640;
personCfg.inpWidth = 640;
personCfg.detectionScoreThreshold = 0.5f;
personCfg.modelConfThreshold = 0.5f;
personCfg.modelMNSThreshold = 0.45f;
personCfg.numKPS = 0;
personCfg.gpuOptBatchSize = 1;
personCfg.gpuMaxBatchSize = 1;
personCfg.gpuDeviceIndex = m_options.deviceIndex;
std::string personLabelMap;
const bool ok = detector->LoadModelFromFolder(
/*licenseKey*/ _licenseKey,
personCfg,
/*modelName*/ "person",
/*className*/ "person.names",
/*modelFolder*/ modelFolder,
personLabelMap);
if (!ok) {
_logger.LogWarn("ANSRTRTMPOSE::TryLoadPersonDetector",
"Person detector LoadModelFromFolder FAILED -- falling back to whole-image pose",
__FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "TryLoadPersonDetector: detector load FAILED");
return false;
}
m_personDetector = std::move(detector);
ANS_DBG("ANSRTRTMPOSE",
"TryLoadPersonDetector: detector loaded OK from '%s' (labels='%s')",
modelFolder.c_str(), personLabelMap.c_str());
return true;
}
catch (const std::exception& e) {
_logger.LogWarn("ANSRTRTMPOSE::TryLoadPersonDetector", e.what(), __FILE__, __LINE__);
ANS_DBG("ANSRTRTMPOSE", "TryLoadPersonDetector: EXCEPTION '%s'", e.what());
m_personDetector.reset();
return false;
}
}
2026-05-14 17:48:09 +10:00
} // namespace ANSCENTER