876 lines
48 KiB
Plaintext
876 lines
48 KiB
Plaintext
#include "pose_engine.hpp"
|
|
#include "common.hpp"
|
|
#include "gpu_preprocess.hpp"
|
|
|
|
#include <opencv2/imgproc.hpp>
|
|
|
|
#include <algorithm>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <numeric>
|
|
#include <set>
|
|
#include <tuple>
|
|
|
|
namespace fangjia {
|
|
namespace {
|
|
|
|
constexpr int kInputWidth = 288;
|
|
constexpr int kInputHeight = 384;
|
|
constexpr int kKeypoints = 133;
|
|
constexpr int kSimccX = 576;
|
|
constexpr int kSimccY = 768;
|
|
|
|
__global__ void poseNormalizeKernel(
|
|
float* output, std::uint8_t const* input, int people, int inputStep) {
|
|
int index = blockIdx.x * blockDim.x + threadIdx.x;
|
|
int pixels = kInputWidth * kInputHeight;
|
|
int total = people * pixels;
|
|
if (index >= total) return;
|
|
int person = index / pixels;
|
|
int pixel = index - person * pixels;
|
|
int y = pixel / kInputWidth;
|
|
int x = pixel - y * kInputWidth;
|
|
auto const* value = input + person * inputStep * kInputHeight + y * inputStep + x * 3;
|
|
float* destination = output + static_cast<std::size_t>(person) * 3 * pixels;
|
|
destination[pixel] = static_cast<float>((static_cast<double>(value[0]) - 123.675) / 58.395);
|
|
destination[pixels + pixel] = static_cast<float>((static_cast<double>(value[1]) - 116.28) / 57.12);
|
|
destination[2 * pixels + pixel] = static_cast<float>((static_cast<double>(value[2]) - 103.53) / 57.375);
|
|
}
|
|
|
|
float iou(Box const& a, Box const& b) {
|
|
float x1 = std::max(a.x1, b.x1);
|
|
float y1 = std::max(a.y1, b.y1);
|
|
float x2 = std::min(a.x2, b.x2);
|
|
float y2 = std::min(a.y2, b.y2);
|
|
float intersection = std::max(x2 - x1, 0.0F) * std::max(y2 - y1, 0.0F);
|
|
float areaA = std::max(a.x2 - a.x1, 0.0F) * std::max(a.y2 - a.y1, 0.0F);
|
|
float areaB = std::max(b.x2 - b.x1, 0.0F) * std::max(b.y2 - b.y1, 0.0F);
|
|
return intersection / std::max(areaA + areaB - intersection, 1.0F);
|
|
}
|
|
|
|
float distance(float x1, float y1, float x2, float y2) {
|
|
return std::hypot(x1 - x2, y1 - y2);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
std::vector<int> BoxTracker::update(
|
|
std::vector<Box>& boxes, int frameIndex, int width, int height) {
|
|
// Tracks older than Python's max_age can never be matched again.
|
|
// Reclaim them to bound memory during continuous camera operation.
|
|
for(auto it=tracks_.begin();it!=tracks_.end();) {
|
|
if(frameIndex-it->second.lastFrame>30)it=tracks_.erase(it);
|
|
else ++it;
|
|
}
|
|
struct Pair { float score; int detection; int track; };
|
|
std::vector<Pair> pairs;
|
|
float imageDiagonal = std::hypot(static_cast<float>(width), static_cast<float>(height));
|
|
for (int d = 0; d < static_cast<int>(boxes.size()); ++d) {
|
|
float cx = 0.5F * (boxes[d].x1 + boxes[d].x2);
|
|
float cy = 0.5F * (boxes[d].y1 + boxes[d].y2);
|
|
for (auto const& [id, track] : tracks_) {
|
|
if (frameIndex - track.lastFrame > 30) continue;
|
|
float pcx = 0.5F * (track.box.x1 + track.box.x2);
|
|
float pcy = 0.5F * (track.box.y1 + track.box.y2);
|
|
float overlap = iou(boxes[d], track.box);
|
|
float centerDistance = distance(cx, cy, pcx, pcy) / imageDiagonal;
|
|
if (overlap >= 0.05F || centerDistance <= 0.18F) {
|
|
pairs.push_back({2.0F * overlap + std::max(0.0F, 1.0F - centerDistance / 0.18F), d, id});
|
|
}
|
|
}
|
|
}
|
|
std::sort(pairs.begin(), pairs.end(), [](Pair const& a, Pair const& b) {
|
|
if (a.score != b.score) return a.score > b.score;
|
|
if (a.detection != b.detection) return a.detection > b.detection;
|
|
return a.track > b.track;
|
|
});
|
|
std::vector<int> result(boxes.size(), 0);
|
|
std::unordered_map<int, bool> assigned;
|
|
for (auto const& pair : pairs) {
|
|
if (result[pair.detection] || assigned[pair.track]) continue;
|
|
result[pair.detection] = pair.track;
|
|
assigned[pair.track] = true;
|
|
}
|
|
for (int d = 0; d < static_cast<int>(boxes.size()); ++d) {
|
|
if (!result[d]) result[d] = nextId_++;
|
|
boxes[d].trackId = result[d];
|
|
tracks_[result[d]] = {boxes[d], frameIndex};
|
|
}
|
|
return result;
|
|
}
|
|
|
|
RtmwEngine::RtmwEngine(
|
|
std::filesystem::path const& enginePath, TrtLogger& logger,
|
|
int maxSourceWidth, int maxSourceHeight, int maxPeople)
|
|
: engine_(enginePath, logger), maxSourceWidth_(maxSourceWidth),
|
|
maxSourceHeight_(maxSourceHeight), maxPeople_(maxPeople),
|
|
centersHost_(maxPeople * 2), scalesHost_(maxPeople * 2),
|
|
simccXHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccX),
|
|
simccYHost_(static_cast<std::size_t>(maxPeople) * kKeypoints * kSimccY) {
|
|
engine_.setInputShape("input", nvinfer1::Dims4{1,3,kInputHeight,kInputWidth});
|
|
engine_.prepare();
|
|
auto xs=engine_.shape("simcc_x"),ys=engine_.shape("simcc_y");
|
|
if(xs.nbDims!=3||ys.nbDims!=3||xs.d[0]!=1||ys.d[0]!=1||xs.d[1]!=kKeypoints||ys.d[1]!=kKeypoints||xs.d[2]!=kSimccX||ys.d[2]!=kSimccY)
|
|
throw std::runtime_error("RTMW engine I/O shape does not match the 133-point model");
|
|
for(auto name:{"input","simcc_x","simcc_y"})if(engine_.dataType(name)!=nvinfer1::DataType::kFLOAT)
|
|
throw std::runtime_error("RTMW requires float32 I/O bindings");
|
|
std::size_t const values = static_cast<std::size_t>(maxPeople) * 3 * kInputWidth * kInputHeight;
|
|
CUDA_CHECK(cudaMalloc(
|
|
&cropsDevice_, static_cast<std::size_t>(maxPeople_) * kInputWidth * kInputHeight * 3));
|
|
CUDA_CHECK(cudaMalloc(&networkDevice_, values * sizeof(float)));
|
|
CUDA_CHECK(cudaMalloc(&sourceDevice_, static_cast<size_t>(maxSourceWidth_)*maxSourceHeight_*3));
|
|
}
|
|
|
|
RtmwEngine::~RtmwEngine() {
|
|
if (sourceDevice_) cudaFree(sourceDevice_);
|
|
if (cropsDevice_) cudaFree(cropsDevice_);
|
|
if (networkDevice_) cudaFree(networkDevice_);
|
|
}
|
|
|
|
void RtmwEngine::warmup(cudaStream_t stream) {
|
|
std::vector<Box> boxes{{0, 0, static_cast<float>(maxSourceWidth_), static_cast<float>(maxSourceHeight_), 1, 1}};
|
|
cv::Mat black(maxSourceHeight_, maxSourceWidth_, CV_8UC3, cv::Scalar(0, 0, 0));
|
|
for (int i = 0; i < 2; ++i) infer(black, boxes, stream);
|
|
}
|
|
|
|
std::vector<PersonPose> RtmwEngine::infer(
|
|
cv::Mat const& frame, std::vector<Box> const& boxes, cudaStream_t stream,
|
|
float* preprocessMs, float* inferenceMs, float* postprocessMs) {
|
|
int batch = std::min(static_cast<int>(boxes.size()), maxPeople_);
|
|
if (batch <= 0) return {};
|
|
auto const preprocessStarted = std::chrono::steady_clock::now();
|
|
int const cropStep = kInputWidth * 3;
|
|
int const pixels = kInputWidth * kInputHeight;
|
|
if (frame.cols>maxSourceWidth_ || frame.rows>maxSourceHeight_) throw std::runtime_error("Pose input exceeds allocation");
|
|
auto npp = cms::nppContext(stream);
|
|
CUDA_CHECK(cudaMemcpy2DAsync(sourceDevice_, frame.cols*3, frame.data, frame.step, frame.cols*3, frame.rows, cudaMemcpyHostToDevice, stream));
|
|
for (int i = 0; i < batch; ++i) {
|
|
auto const& box = boxes[i];
|
|
double width = (static_cast<double>(box.x2) - static_cast<double>(box.x1)) * 1.25;
|
|
double height = (static_cast<double>(box.y2) - static_cast<double>(box.y1)) * 1.25;
|
|
constexpr double aspect = static_cast<double>(kInputWidth) / kInputHeight;
|
|
if (width > height * aspect) height = width / aspect;
|
|
else width = height * aspect;
|
|
centersHost_[i * 2] = 0.5 * (static_cast<double>(box.x1) + static_cast<double>(box.x2));
|
|
centersHost_[i * 2 + 1] = 0.5 * (static_cast<double>(box.y1) + static_cast<double>(box.y2));
|
|
scalesHost_[i * 2] = width;
|
|
scalesHost_[i * 2 + 1] = height;
|
|
|
|
cv::Point2f source[3]{
|
|
{static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1])},
|
|
{static_cast<float>(centersHost_[i * 2]), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)},
|
|
{static_cast<float>(centersHost_[i * 2] - width * 0.5), static_cast<float>(centersHost_[i * 2 + 1] - width * 0.5)}};
|
|
cv::Point2f destination[3]{
|
|
{kInputWidth * 0.5F, kInputHeight * 0.5F},
|
|
{kInputWidth * 0.5F, kInputHeight * 0.5F - kInputWidth * 0.5F},
|
|
{0.0F, kInputHeight * 0.5F - kInputWidth * 0.5F}};
|
|
cv::Mat affine = cv::getAffineTransform(source, destination);
|
|
auto* cropDevice=cropsDevice_+static_cast<size_t>(i)*cropStep*kInputHeight;
|
|
Npp8u fill[3]={0,0,0};
|
|
cms::nppCheck(nppiSet_8u_C3R_Ctx(fill,cropDevice,cropStep,{kInputWidth,kInputHeight},npp));
|
|
double coeff[2][3];
|
|
for(int row=0;row<2;++row) for(int col=0;col<3;++col) coeff[row][col]=affine.at<double>(row,col);
|
|
cms::nppCheck(nppiWarpAffine_8u_C3R_Ctx(sourceDevice_,{frame.cols,frame.rows},frame.cols*3,{0,0,frame.cols,frame.rows},cropDevice,cropStep,{0,0,kInputWidth,kInputHeight},coeff,NPPI_INTER_LINEAR,npp));
|
|
}
|
|
int const totalPixels = batch * pixels;
|
|
poseNormalizeKernel<<<(totalPixels + 255) / 256, 256, 0, stream>>>(
|
|
networkDevice_, cropsDevice_, batch, cropStep);
|
|
CUDA_CHECK(cudaGetLastError());
|
|
CUDA_CHECK(cudaStreamSynchronize(stream));
|
|
if (preprocessMs) {
|
|
*preprocessMs = static_cast<float>(std::chrono::duration<double, std::milli>(
|
|
std::chrono::steady_clock::now() - preprocessStarted).count());
|
|
}
|
|
|
|
// RTMPose's Python API invokes the ONNX/TensorRT session once per person.
|
|
// Preserve that execution contract instead of batching people together.
|
|
engine_.setInputShape("input", nvinfer1::Dims4{1, 3, kInputHeight, kInputWidth});
|
|
engine_.prepare();
|
|
CudaEventTimer timer;
|
|
float inferTime = 0.0F;
|
|
std::size_t const inputValues = static_cast<std::size_t>(3) * pixels;
|
|
std::size_t const xValues = static_cast<std::size_t>(kKeypoints) * kSimccX;
|
|
std::size_t const yValues = static_cast<std::size_t>(kKeypoints) * kSimccY;
|
|
for (int person = 0; person < batch; ++person) {
|
|
CUDA_CHECK(cudaMemcpyAsync(
|
|
engine_.device("input"), networkDevice_ + static_cast<std::size_t>(person) * inputValues,
|
|
inputValues * sizeof(float), cudaMemcpyDeviceToDevice, stream));
|
|
timer.start(stream);
|
|
if (!engine_.enqueue(stream)) throw std::runtime_error("RTMW enqueue failed");
|
|
inferTime += timer.stop(stream);
|
|
CUDA_CHECK(cudaMemcpyAsync(
|
|
simccXHost_.data() + static_cast<std::size_t>(person) * xValues,
|
|
engine_.device("simcc_x"), xValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
|
|
CUDA_CHECK(cudaMemcpyAsync(
|
|
simccYHost_.data() + static_cast<std::size_t>(person) * yValues,
|
|
engine_.device("simcc_y"), yValues * sizeof(float), cudaMemcpyDeviceToHost, stream));
|
|
}
|
|
if (inferenceMs) *inferenceMs = inferTime;
|
|
|
|
auto const postprocessStarted = std::chrono::steady_clock::now();
|
|
CUDA_CHECK(cudaStreamSynchronize(stream));
|
|
std::vector<PersonPose> result(batch);
|
|
for (int person = 0; person < batch; ++person) {
|
|
result[person].box = boxes[person];
|
|
for (int keypoint = 0; keypoint < kKeypoints; ++keypoint) {
|
|
auto xBegin = simccXHost_.begin() + (person * kKeypoints + keypoint) * kSimccX;
|
|
auto yBegin = simccYHost_.begin() + (person * kKeypoints + keypoint) * kSimccY;
|
|
auto xIt = std::max_element(xBegin, xBegin + kSimccX);
|
|
auto yIt = std::max_element(yBegin, yBegin + kSimccY);
|
|
float score = 0.5F * (*xIt + *yIt);
|
|
if(!std::isfinite(score))throw std::runtime_error("Non-finite RTMW output");
|
|
float localX = score > 0.0F ? static_cast<float>(xIt - xBegin) * 0.5F : -0.5F;
|
|
float localY = score > 0.0F ? static_cast<float>(yIt - yBegin) * 0.5F : -0.5F;
|
|
double scaleX = scalesHost_[person * 2];
|
|
double scaleY = scalesHost_[person * 2 + 1];
|
|
result[person].keypoints[keypoint] = {
|
|
static_cast<float>(static_cast<double>(localX) / kInputWidth * scaleX + centersHost_[person * 2] - scaleX * 0.5),
|
|
static_cast<float>(static_cast<double>(localY) / kInputHeight * scaleY + centersHost_[person * 2 + 1] - scaleY * 0.5),
|
|
score};
|
|
}
|
|
}
|
|
float post = static_cast<float>(std::chrono::duration<double, std::milli>(
|
|
std::chrono::steady_clock::now() - postprocessStarted).count());
|
|
if (postprocessMs) *postprocessMs = post;
|
|
return result;
|
|
}
|
|
|
|
PoseFrameResult PoseFusionTracker::update(
|
|
std::vector<PersonPose> const& rawPeople,
|
|
std::vector<PersonPose> const& isolatedPeople,
|
|
int frameIndex) {
|
|
for(auto it=states_.begin();it!=states_.end();) {
|
|
if(it->second.lastFrame>=0&&frameIndex-it->second.lastFrame>30)it=states_.erase(it);
|
|
else ++it;
|
|
}
|
|
struct SideMeta {
|
|
bool limbConsensus{};
|
|
bool handValid{};
|
|
bool distanceEligible{};
|
|
bool alarmEligible{};
|
|
double handScore{};
|
|
double linkToSelectedLimb{};
|
|
std::string handSource;
|
|
std::string handState;
|
|
};
|
|
struct FusedPerson {
|
|
PersonPose pose;
|
|
std::array<SideMeta, 2> meta;
|
|
};
|
|
struct Measurement {
|
|
std::array<Vec2, 21> hand{};
|
|
bool precise{};
|
|
bool provisional{};
|
|
double quality{};
|
|
std::string source{"model"};
|
|
};
|
|
auto vec = [](Keypoint const& point) { return Vec2{point.x, point.y}; };
|
|
auto add = [](Vec2 a, Vec2 b) { return Vec2{a.x + b.x, a.y + b.y}; };
|
|
auto sub = [](Vec2 a, Vec2 b) { return Vec2{a.x - b.x, a.y - b.y}; };
|
|
auto mul = [](Vec2 a, double value) { return Vec2{a.x * value, a.y * value}; };
|
|
auto norm = [](Vec2 a) { return std::hypot(a.x, a.y); };
|
|
auto median = [](std::vector<double> values) {
|
|
if (values.empty()) return 0.0;
|
|
std::sort(values.begin(), values.end());
|
|
auto middle = values.size() / 2;
|
|
return values.size() % 2 ? values[middle] : 0.5 * (values[middle - 1] + values[middle]);
|
|
};
|
|
auto percentile90 = [](std::vector<double> values) {
|
|
if (values.empty()) return 0.0;
|
|
std::sort(values.begin(), values.end());
|
|
double position = 0.9 * (values.size() - 1);
|
|
auto lower = static_cast<std::size_t>(position);
|
|
auto upper = std::min(lower + 1, values.size() - 1);
|
|
return values[lower] * (upper - position) + values[upper] * (position - lower);
|
|
};
|
|
auto boxDiagonal = [](Box const& box) {
|
|
return std::max(std::hypot(box.x2 - box.x1, box.y2 - box.y1), 1.0F);
|
|
};
|
|
auto normalized = [](Keypoint const& point, Box const& box) {
|
|
return Vec2{
|
|
(point.x - box.x1) / std::max(box.x2 - box.x1, 1.0F),
|
|
(point.y - box.y1) / std::max(box.y2 - box.y1, 1.0F)};
|
|
};
|
|
auto denormalized = [](Vec2 point, Box const& box) {
|
|
return Vec2{
|
|
point.x * std::max(box.x2 - box.x1, 1.0F) + box.x1,
|
|
point.y * std::max(box.y2 - box.y1, 1.0F) + box.y1};
|
|
};
|
|
auto solveTwoBoneElbow = [&](Vec2 shoulder, Vec2 wrist, double upperLength,
|
|
double forearmLength, Vec2 reference) {
|
|
Vec2 direction = sub(wrist, shoulder);
|
|
double length = norm(direction);
|
|
if (length < 1e-6) return reference;
|
|
Vec2 unit = mul(direction, 1.0 / length);
|
|
if (length >= upperLength + forearmLength) return add(shoulder, mul(unit, upperLength));
|
|
if (length <= std::abs(upperLength - forearmLength)) {
|
|
double sign = upperLength >= forearmLength ? 1.0 : -1.0;
|
|
return add(shoulder, mul(unit, upperLength * sign));
|
|
}
|
|
double along = (upperLength * upperLength - forearmLength * forearmLength + length * length) /
|
|
(2.0 * length);
|
|
double height = std::sqrt(std::max(upperLength * upperLength - along * along, 0.0));
|
|
Vec2 base = add(shoulder, mul(unit, along));
|
|
Vec2 perpendicular{-unit.y, unit.x};
|
|
Vec2 first = add(base, mul(perpendicular, height));
|
|
Vec2 second = sub(base, mul(perpendicular, height));
|
|
return norm(sub(first, reference)) <= norm(sub(second, reference)) ? first : second;
|
|
};
|
|
struct Candidate {
|
|
PersonPose const* pose{};
|
|
std::string source;
|
|
std::array<Vec2, 3> limb{};
|
|
std::array<Vec2, 21> hand{};
|
|
double limbScore{};
|
|
double handScore{};
|
|
double handSpan{};
|
|
double wristLink{};
|
|
bool limbValid{};
|
|
bool handValid{};
|
|
};
|
|
auto buildCandidate = [&](PersonPose const& pose, int side, std::string source) {
|
|
int shoulder = side == 0 ? 5 : 6;
|
|
int elbow = side == 0 ? 7 : 8;
|
|
int wrist = side == 0 ? 9 : 10;
|
|
int handStart = side == 0 ? 91 : 112;
|
|
Candidate candidate;
|
|
candidate.pose = &pose;
|
|
candidate.source = std::move(source);
|
|
candidate.limb = {normalized(pose.keypoints[shoulder], pose.box),
|
|
normalized(pose.keypoints[elbow], pose.box),
|
|
normalized(pose.keypoints[wrist], pose.box)};
|
|
double minX = pose.keypoints[handStart].x, maxX = minX;
|
|
double minY = pose.keypoints[handStart].y, maxY = minY;
|
|
for (int index = 0; index < 21; ++index) {
|
|
candidate.hand[index] = normalized(pose.keypoints[handStart + index], pose.box);
|
|
minX = std::min<double>(minX, pose.keypoints[handStart + index].x);
|
|
maxX = std::max<double>(maxX, pose.keypoints[handStart + index].x);
|
|
minY = std::min<double>(minY, pose.keypoints[handStart + index].y);
|
|
maxY = std::max<double>(maxY, pose.keypoints[handStart + index].y);
|
|
candidate.handScore += pose.keypoints[handStart + index].score;
|
|
}
|
|
candidate.handScore /= 21.0;
|
|
candidate.limbScore = (pose.keypoints[shoulder].score + pose.keypoints[elbow].score + pose.keypoints[wrist].score) / 3.0;
|
|
double upper = norm(sub(candidate.limb[0], candidate.limb[1]));
|
|
double forearm = norm(sub(candidate.limb[1], candidate.limb[2]));
|
|
candidate.handSpan = std::hypot(maxX - minX, maxY - minY) / boxDiagonal(pose.box);
|
|
candidate.wristLink = distance(
|
|
pose.keypoints[wrist].x, pose.keypoints[wrist].y,
|
|
pose.keypoints[handStart].x, pose.keypoints[handStart].y) / boxDiagonal(pose.box);
|
|
candidate.limbValid = upper >= 0.025 && upper <= 0.8 && forearm >= 0.025 && forearm <= 0.8;
|
|
candidate.handValid = candidate.handSpan >= 0.008 && candidate.handSpan <= 0.35 && candidate.wristLink <= 0.12;
|
|
return candidate;
|
|
};
|
|
auto robustDistance = [&](auto const& points, auto const& target, bool available) {
|
|
if (!available) return 0.0;
|
|
std::vector<double> distances;
|
|
for (std::size_t index = 0; index < points.size(); ++index) {
|
|
distances.push_back(norm(sub(points[index], target[index])));
|
|
}
|
|
return median(std::move(distances));
|
|
};
|
|
|
|
std::vector<FusedPerson> fused;
|
|
fused.reserve(rawPeople.size());
|
|
for (std::size_t personIndex = 0; personIndex < rawPeople.size(); ++personIndex) {
|
|
auto const& raw = rawPeople[personIndex];
|
|
auto const& isolated = personIndex < isolatedPeople.size() ? isolatedPeople[personIndex] : raw;
|
|
auto& state = states_[raw.box.trackId];
|
|
FusedPerson output;
|
|
output.pose = raw;
|
|
for (int side = 0; side < 2; ++side) {
|
|
int shoulder = side == 0 ? 5 : 6;
|
|
int elbow = side == 0 ? 7 : 8;
|
|
int wrist = side == 0 ? 9 : 10;
|
|
int handStart = side == 0 ? 91 : 112;
|
|
auto& track = state.fusion[side];
|
|
std::array<Candidate, 2> candidates{
|
|
buildCandidate(raw, side, "raw"), buildCandidate(isolated, side, "isolated")};
|
|
int limbChoice = 0;
|
|
double bestLimbScore = -1e30;
|
|
for (int choice = 0; choice < 2; ++choice) {
|
|
std::array<Vec2, 3> target{};
|
|
if (track.hasLimbPrevious) {
|
|
for (int i = 0; i < 3; ++i) target[i] = add(track.limbPrevious[i], track.hasLimbVelocity ? mul(track.limbVelocity[i], 0.65) : Vec2{});
|
|
}
|
|
double temporal = robustDistance(candidates[choice].limb, target, track.hasLimbPrevious);
|
|
double score = candidates[choice].limbScore + (candidates[choice].limbValid ? 1.2 : -5.0)
|
|
+ (track.limbSource == candidates[choice].source ? 0.35 : 0.0) - 7.0 * temporal;
|
|
if (score > bestLimbScore) { bestLimbScore = score; limbChoice = choice; }
|
|
}
|
|
int handChoice = 0;
|
|
double bestHandScore = -1e30;
|
|
double chosenTemporal = 0.0;
|
|
double chosenLink = 1.0;
|
|
for (int choice = 0; choice < 2; ++choice) {
|
|
std::array<Vec2, 21> target{};
|
|
if (track.hasHandPrevious) {
|
|
for (int i = 0; i < 21; ++i) target[i] = add(track.handPrevious[i], track.hasHandVelocity ? mul(track.handVelocity[i], 0.65) : Vec2{});
|
|
}
|
|
double temporal = robustDistance(candidates[choice].hand, target, track.hasHandPrevious);
|
|
auto const& limbPose = *candidates[limbChoice].pose;
|
|
double link = distance(
|
|
candidates[choice].pose->keypoints[handStart].x,
|
|
candidates[choice].pose->keypoints[handStart].y,
|
|
limbPose.keypoints[wrist].x, limbPose.keypoints[wrist].y) / boxDiagonal(raw.box);
|
|
double linkPenalty = 1.5 * std::min(std::max(link - 0.12, 0.0), 0.2);
|
|
double score = candidates[choice].handScore + (candidates[choice].handValid ? 3.0 : -7.0)
|
|
+ (track.handSource == candidates[choice].source ? 0.45 : 0.0)
|
|
+ (candidates[choice].source == candidates[limbChoice].source ? 0.15 : 0.0)
|
|
- linkPenalty - 8.0 * std::min(temporal, 0.15);
|
|
if (score > bestHandScore) {
|
|
bestHandScore = score;
|
|
handChoice = choice;
|
|
chosenTemporal = temporal;
|
|
chosenLink = link;
|
|
}
|
|
}
|
|
double endpointAdvantage = candidates[handChoice].handScore - candidates[1 - handChoice].handScore;
|
|
if (handChoice != limbChoice && candidates[handChoice].handValid && endpointAdvantage >= 0.6) {
|
|
limbChoice = handChoice;
|
|
// fuse_person() rewrites this link after its visible-endpoint
|
|
// override. Temporal recovery consumes the rewritten value.
|
|
chosenLink = candidates[handChoice].wristLink;
|
|
}
|
|
auto const& limb = candidates[limbChoice];
|
|
auto const& hand = candidates[handChoice];
|
|
for (int index : {shoulder, elbow, wrist}) output.pose.keypoints[index] = limb.pose->keypoints[index];
|
|
for (int index = 0; index < 21; ++index) output.pose.keypoints[handStart + index] = hand.pose->keypoints[handStart + index];
|
|
std::vector<double> limbDistances, handDistances;
|
|
for (int index = 0; index < 3; ++index) limbDistances.push_back(norm(sub(candidates[0].limb[index], candidates[1].limb[index])));
|
|
for (int index = 0; index < 21; ++index) handDistances.push_back(norm(sub(candidates[0].hand[index], candidates[1].hand[index])));
|
|
double handDisagreement = percentile90(handDistances);
|
|
double scoreAdvantage = candidates[0].handScore - candidates[1].handScore;
|
|
auto& meta = output.meta[side];
|
|
meta.limbConsensus = median(limbDistances) <= 0.05;
|
|
meta.handValid = hand.handValid;
|
|
meta.distanceEligible = hand.handValid && (
|
|
handDisagreement <= 0.06 ||
|
|
(hand.source == "raw" && scoreAdvantage >= 0.6 && chosenTemporal <= 0.08));
|
|
meta.alarmEligible = meta.distanceEligible || (hand.handValid && hand.source == "raw" && scoreAdvantage >= 0.6);
|
|
meta.handScore = std::nearbyint(hand.handScore * 1e4) / 1e4;
|
|
meta.linkToSelectedLimb = std::nearbyint(chosenLink * 1e5) / 1e5;
|
|
meta.handSource = hand.source;
|
|
if (!hand.handValid) meta.handState = "rejected";
|
|
else if (handDisagreement <= 0.06) meta.handState = "consensus";
|
|
else if (hand.source == "raw" && scoreAdvantage >= 0.6 && chosenTemporal <= 0.08) meta.handState = "visible_endpoint";
|
|
else if (hand.source == "isolated") meta.handState = "inferred";
|
|
else meta.handState = "conflict";
|
|
if (track.hasLimbPrevious) {
|
|
for (int i = 0; i < 3; ++i) {
|
|
auto delta = sub(limb.limb[i], track.limbPrevious[i]);
|
|
track.limbVelocity[i] = track.hasLimbVelocity ? add(mul(track.limbVelocity[i], 0.65), mul(delta, 0.35)) : delta;
|
|
}
|
|
track.hasLimbVelocity = true;
|
|
}
|
|
if (hand.handValid) {
|
|
if (track.hasHandPrevious) {
|
|
for (int i = 0; i < 21; ++i) {
|
|
auto delta = sub(hand.hand[i], track.handPrevious[i]);
|
|
track.handVelocity[i] = track.hasHandVelocity ? add(mul(track.handVelocity[i], 0.65), mul(delta, 0.35)) : delta;
|
|
}
|
|
track.hasHandVelocity = true;
|
|
}
|
|
track.handPrevious = hand.hand;
|
|
track.hasHandPrevious = true;
|
|
track.handSource = hand.source;
|
|
}
|
|
track.limbPrevious = limb.limb;
|
|
track.hasLimbPrevious = true;
|
|
track.limbSource = limb.source;
|
|
}
|
|
// Same-person duplicate suppression from the reference fusion.
|
|
auto handCenter = [&](int side) {
|
|
int start = side == 0 ? 91 : 112;
|
|
Vec2 center{};
|
|
for (int i = 0; i < 21; ++i) center = add(center, vec(output.pose.keypoints[start + i]));
|
|
return mul(center, 1.0 / 21.0);
|
|
};
|
|
if (norm(sub(handCenter(0), handCenter(1))) / boxDiagonal(raw.box) < 0.08) {
|
|
int weaker = output.meta[0].handScore <= output.meta[1].handScore ? 0 : 1;
|
|
output.meta[weaker].handState = "duplicate";
|
|
output.meta[weaker].distanceEligible = false;
|
|
output.meta[weaker].alarmEligible = false;
|
|
}
|
|
// Motion-adaptive EMA in person-box coordinates.
|
|
std::array<Vec2, 133> now{};
|
|
for (int i = 0; i < 133; ++i) now[i] = normalized(output.pose.keypoints[i], raw.box);
|
|
if (!state.hasSmoothed) { state.smoothed = now; state.hasSmoothed = true; }
|
|
else {
|
|
for (int i = 0; i < 133; ++i) {
|
|
double displacement = norm(sub(now[i], state.smoothed[i]));
|
|
double alpha = 0.32 + 0.58 * std::clamp(displacement / 0.08, 0.0, 1.0);
|
|
state.smoothed[i] = add(mul(now[i], alpha), mul(state.smoothed[i], 1.0 - alpha));
|
|
}
|
|
}
|
|
fused.push_back(std::move(output));
|
|
}
|
|
|
|
// Exact build_global_hands() evidence: selected fusion output, stable
|
|
// score ordering, then the same cross-person duplicate collapse.
|
|
std::vector<GlobalHand> globalCandidates;
|
|
for (std::size_t person = 0; person < fused.size(); ++person) {
|
|
for (int side = 0; side < 2; ++side) {
|
|
auto const& meta = fused[person].meta[side];
|
|
if (!meta.alarmEligible) continue;
|
|
int const start = side == 0 ? 91 : 112;
|
|
GlobalHand hand;
|
|
hand.personId = static_cast<int>(person) + 1;
|
|
hand.trackId = fused[person].pose.box.trackId;
|
|
hand.left = side == 0;
|
|
hand.source = meta.handSource;
|
|
hand.state = meta.handState;
|
|
hand.distanceEligible = meta.distanceEligible;
|
|
hand.alarmEligible = meta.alarmEligible;
|
|
hand.score = static_cast<float>(meta.handScore);
|
|
float minX = fused[person].pose.keypoints[start].x;
|
|
float maxX = minX;
|
|
float minY = fused[person].pose.keypoints[start].y;
|
|
float maxY = minY;
|
|
for (int point = 0; point < 21; ++point) {
|
|
hand.keypoints[point] = fused[person].pose.keypoints[start + point];
|
|
minX = std::min(minX, hand.keypoints[point].x);
|
|
maxX = std::max(maxX, hand.keypoints[point].x);
|
|
minY = std::min(minY, hand.keypoints[point].y);
|
|
maxY = std::max(maxY, hand.keypoints[point].y);
|
|
}
|
|
hand.spanPx = std::hypot(maxX - minX, maxY - minY);
|
|
globalCandidates.push_back(std::move(hand));
|
|
}
|
|
}
|
|
std::stable_sort(globalCandidates.begin(), globalCandidates.end(), [](auto const& first, auto const& second) {
|
|
return first.score > second.score;
|
|
});
|
|
std::vector<GlobalHand> globalHands;
|
|
for (auto& candidate : globalCandidates) {
|
|
Vec2 candidateCenter{};
|
|
for (auto const& point : candidate.keypoints) candidateCenter = add(candidateCenter, vec(point));
|
|
candidateCenter = mul(candidateCenter, 1.0 / 21.0);
|
|
auto duplicate = globalHands.end();
|
|
for (auto iterator = globalHands.begin(); iterator != globalHands.end(); ++iterator) {
|
|
Vec2 existingCenter{};
|
|
for (auto const& point : iterator->keypoints) existingCenter = add(existingCenter, vec(point));
|
|
existingCenter = mul(existingCenter, 1.0 / 21.0);
|
|
double const threshold = std::clamp(
|
|
0.45 * std::max<double>(candidate.spanPx, iterator->spanPx), 18.0, 70.0);
|
|
if (norm(sub(candidateCenter, existingCenter)) <= threshold) {
|
|
duplicate = iterator;
|
|
break;
|
|
}
|
|
}
|
|
if (duplicate == globalHands.end()) globalHands.push_back(std::move(candidate));
|
|
else duplicate->aliases.emplace_back(candidate.personId, candidate.left);
|
|
}
|
|
|
|
// Build the model measurements, then collapse cross-person duplicates.
|
|
std::vector<std::array<std::optional<Measurement>, 2>> measurements(fused.size());
|
|
for (std::size_t person = 0; person < fused.size(); ++person) {
|
|
for (int side = 0; side < 2; ++side) {
|
|
if (!fused[person].meta[side].alarmEligible) continue;
|
|
Measurement measurement;
|
|
int start = side == 0 ? 91 : 112;
|
|
for (int i = 0; i < 21; ++i) measurement.hand[i] = vec(fused[person].pose.keypoints[start + i]);
|
|
measurement.precise = fused[person].meta[side].distanceEligible;
|
|
measurement.provisional = !measurement.precise;
|
|
measurement.quality = fused[person].meta[side].handScore;
|
|
measurements[person][side] = measurement;
|
|
}
|
|
}
|
|
auto handSpan = [&](Measurement const& measurement) {
|
|
double minX = measurement.hand[0].x, maxX = minX, minY = measurement.hand[0].y, maxY = minY;
|
|
for (auto point : measurement.hand) { minX = std::min(minX, point.x); maxX = std::max(maxX, point.x); minY = std::min(minY, point.y); maxY = std::max(maxY, point.y); }
|
|
return std::hypot(maxX - minX, maxY - minY);
|
|
};
|
|
auto handCenter = [&](Measurement const& measurement) {
|
|
Vec2 center{}; for (auto point : measurement.hand) center = add(center, point); return mul(center, 1.0 / 21.0);
|
|
};
|
|
for (std::size_t firstPerson = 0; firstPerson < fused.size(); ++firstPerson) {
|
|
for (int firstSide = 0; firstSide < 2; ++firstSide) {
|
|
if (!measurements[firstPerson][firstSide]) continue;
|
|
for (std::size_t secondPerson = firstPerson + 1; secondPerson < fused.size(); ++secondPerson) {
|
|
for (int secondSide = 0; secondSide < 2; ++secondSide) {
|
|
if (!measurements[firstPerson][firstSide] || !measurements[secondPerson][secondSide]) continue;
|
|
auto const& first = *measurements[firstPerson][firstSide];
|
|
auto const& second = *measurements[secondPerson][secondSide];
|
|
double firstSpan = handSpan(first), secondSpan = handSpan(second);
|
|
if (norm(sub(handCenter(first), handCenter(second))) > std::max(70.0, 0.5 * std::max(firstSpan, secondSpan))) continue;
|
|
double firstArea = std::max((fused[firstPerson].pose.box.x2 - fused[firstPerson].pose.box.x1) * (fused[firstPerson].pose.box.y2 - fused[firstPerson].pose.box.y1), 1.0F);
|
|
double secondArea = std::max((fused[secondPerson].pose.box.x2 - fused[secondPerson].pose.box.x1) * (fused[secondPerson].pose.box.y2 - fused[secondPerson].pose.box.y1), 1.0F);
|
|
if (firstArea >= 2.5 * secondArea &&
|
|
(firstSpan >= 1.45 * secondSpan || first.quality >= second.quality + 0.6)) {
|
|
measurements[secondPerson][secondSide].reset();
|
|
states_[fused[secondPerson].pose.box.trackId].endpoints[secondSide] = {};
|
|
} else if (secondArea >= 2.5 * firstArea &&
|
|
(secondSpan >= 1.45 * firstSpan || second.quality >= first.quality + 0.6)) {
|
|
measurements[firstPerson][firstSide].reset();
|
|
states_[fused[firstPerson].pose.box.trackId].endpoints[firstSide] = {};
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Match resolve_global_hand_assignments(): a large foreground track may reclaim
|
|
// a nearby hand that RTMW emitted only from a smaller background crop.
|
|
struct Reassignment {
|
|
double normalizedDistance{};
|
|
std::size_t targetPerson{};
|
|
int targetSide{};
|
|
std::size_t sourcePerson{};
|
|
int sourceSide{};
|
|
Vec2 sourceCenter{};
|
|
};
|
|
std::vector<Reassignment> reassignments;
|
|
for (std::size_t targetPerson = 0; targetPerson < fused.size(); ++targetPerson) {
|
|
for (int targetSide = 0; targetSide < 2; ++targetSide) {
|
|
if (measurements[targetPerson][targetSide]) continue;
|
|
auto& targetEndpoint = states_[fused[targetPerson].pose.box.trackId].endpoints[targetSide];
|
|
if (!targetEndpoint.hasWrist || !targetEndpoint.hasHandShape || targetEndpoint.missedFrames > 30) continue;
|
|
double targetArea = std::max(
|
|
(fused[targetPerson].pose.box.x2 - fused[targetPerson].pose.box.x1) *
|
|
(fused[targetPerson].pose.box.y2 - fused[targetPerson].pose.box.y1), 1.0F);
|
|
Vec2 shapeCenter{};
|
|
for (auto point : targetEndpoint.handShape) shapeCenter = add(shapeCenter, point);
|
|
shapeCenter = mul(shapeCenter, 1.0 / 21.0);
|
|
Vec2 predictedCenter = add(targetEndpoint.wrist, shapeCenter);
|
|
Measurement targetShape;
|
|
for (int point = 0; point < 21; ++point) targetShape.hand[point] = targetEndpoint.handShape[point];
|
|
double targetSpan = handSpan(targetShape);
|
|
double gate = 0.20 * boxDiagonal(fused[targetPerson].pose.box);
|
|
for (std::size_t sourcePerson = 0; sourcePerson < fused.size(); ++sourcePerson) {
|
|
if (sourcePerson == targetPerson) continue;
|
|
for (int sourceSide = 0; sourceSide < 2; ++sourceSide) {
|
|
auto const& source = measurements[sourcePerson][sourceSide];
|
|
if (!source || source->source != "model") continue;
|
|
double sourceArea = std::max(
|
|
(fused[sourcePerson].pose.box.x2 - fused[sourcePerson].pose.box.x1) *
|
|
(fused[sourcePerson].pose.box.y2 - fused[sourcePerson].pose.box.y1), 1.0F);
|
|
if (targetArea < 2.5 * sourceArea) continue;
|
|
Vec2 sourceCenter = handCenter(*source);
|
|
double sourceSpan = handSpan(*source);
|
|
double candidateDistance = norm(sub(sourceCenter, predictedCenter));
|
|
if (candidateDistance > gate || targetSpan < 1.45 * std::max(sourceSpan, 1.0)) continue;
|
|
reassignments.push_back({
|
|
candidateDistance / std::max(gate, 1e-6), targetPerson, targetSide,
|
|
sourcePerson, sourceSide, sourceCenter});
|
|
}
|
|
}
|
|
}
|
|
}
|
|
std::sort(reassignments.begin(), reassignments.end(), [](auto const& first, auto const& second) {
|
|
return std::tie(first.normalizedDistance, first.targetPerson, first.targetSide,
|
|
first.sourcePerson, first.sourceSide) <
|
|
std::tie(second.normalizedDistance, second.targetPerson, second.targetSide,
|
|
second.sourcePerson, second.sourceSide);
|
|
});
|
|
std::set<std::pair<std::size_t, int>> usedTargets, usedSources;
|
|
for (auto const& item : reassignments) {
|
|
auto targetKey = std::make_pair(item.targetPerson, item.targetSide);
|
|
auto sourceKey = std::make_pair(item.sourcePerson, item.sourceSide);
|
|
if (usedTargets.count(targetKey) || usedSources.count(sourceKey) ||
|
|
measurements[item.targetPerson][item.targetSide] ||
|
|
!measurements[item.sourcePerson][item.sourceSide]) continue;
|
|
auto& targetEndpoint = states_[fused[item.targetPerson].pose.box.trackId].endpoints[item.targetSide];
|
|
Vec2 shapeCenter{};
|
|
for (auto point : targetEndpoint.handShape) shapeCenter = add(shapeCenter, point);
|
|
shapeCenter = mul(shapeCenter, 1.0 / 21.0);
|
|
Vec2 reassignedWrist = sub(item.sourceCenter, shapeCenter);
|
|
Measurement reassigned;
|
|
for (int point = 0; point < 21; ++point) {
|
|
reassigned.hand[point] = add(reassignedWrist, targetEndpoint.handShape[point]);
|
|
}
|
|
reassigned.provisional = true;
|
|
reassigned.source = "global_reassignment";
|
|
reassigned.quality = measurements[item.sourcePerson][item.sourceSide]->quality;
|
|
measurements[item.targetPerson][item.targetSide] = reassigned;
|
|
measurements[item.sourcePerson][item.sourceSide].reset();
|
|
auto& sourceEndpoint = states_[fused[item.sourcePerson].pose.box.trackId].endpoints[item.sourceSide];
|
|
sourceEndpoint = {};
|
|
usedTargets.insert(targetKey);
|
|
usedSources.insert(sourceKey);
|
|
}
|
|
|
|
PoseFrameResult result;
|
|
result.globalHands = std::move(globalHands);
|
|
result.selectedPeople.reserve(fused.size());
|
|
result.smoothedPeople.reserve(fused.size());
|
|
result.v2People.reserve(fused.size());
|
|
for (auto const& person : fused) {
|
|
std::array<FusionMeta,2> meta{};
|
|
for (int side=0;side<2;++side) {
|
|
auto const& m=person.meta[side];
|
|
meta[side]={m.limbConsensus,m.handValid,m.distanceEligible,m.alarmEligible,m.handScore,m.handSource,m.handState};
|
|
}
|
|
result.metadata.push_back(meta);
|
|
result.selectedPeople.push_back(person.pose);
|
|
PersonPose smoothed = person.pose;
|
|
auto const& state = states_[person.pose.box.trackId];
|
|
for (int keypoint = 0; keypoint < 133; ++keypoint) {
|
|
Vec2 point = denormalized(state.smoothed[keypoint], person.pose.box);
|
|
smoothed.keypoints[keypoint].x = static_cast<float>(point.x);
|
|
smoothed.keypoints[keypoint].y = static_cast<float>(point.y);
|
|
}
|
|
result.smoothedPeople.push_back(smoothed);
|
|
result.v2People.push_back(std::move(smoothed));
|
|
}
|
|
for (std::size_t person = 0; person < fused.size(); ++person) {
|
|
auto& state = states_[fused[person].pose.box.trackId];
|
|
int frameGap = state.lastFrame < 0 ? 0 : std::max(frameIndex - state.lastFrame - 1, 0);
|
|
for (auto& endpoint : state.endpoints) endpoint.missedFrames += frameGap;
|
|
state.lastFrame = frameIndex;
|
|
for (int side = 0; side < 2; ++side) {
|
|
if (!fused[person].meta[side].limbConsensus || !fused[person].meta[side].distanceEligible) continue;
|
|
int shoulderIndex = side == 0 ? 5 : 6;
|
|
int elbowIndex = side == 0 ? 7 : 8;
|
|
int wristIndex = side == 0 ? 9 : 10;
|
|
Vec2 shoulder = vec(fused[person].pose.keypoints[shoulderIndex]);
|
|
Vec2 elbow = vec(fused[person].pose.keypoints[elbowIndex]);
|
|
Vec2 wrist = vec(fused[person].pose.keypoints[wristIndex]);
|
|
double diagonal = boxDiagonal(fused[person].pose.box);
|
|
double upper = norm(sub(shoulder, elbow)) / diagonal;
|
|
double forearm = norm(sub(elbow, wrist)) / diagonal;
|
|
if (upper >= 0.04 && upper <= 0.65 && forearm >= 0.04 && forearm <= 0.65) {
|
|
auto& samples = state.boneSamples[side];
|
|
samples.push_back({upper, forearm});
|
|
while (samples.size() > 120) samples.pop_front();
|
|
}
|
|
}
|
|
for (int side = 0; side < 2; ++side) {
|
|
int shoulderIndex = side == 0 ? 5 : 6;
|
|
int elbowIndex = side == 0 ? 7 : 8;
|
|
int wristIndex = side == 0 ? 9 : 10;
|
|
int handStart = side == 0 ? 91 : 112;
|
|
auto& endpoint = state.endpoints[side];
|
|
auto measurement = measurements[person][side];
|
|
// Reference temporal recovery gate.
|
|
bool const recoverableHandState = fused[person].meta[side].handState == "conflict" ||
|
|
fused[person].meta[side].handState == "inferred";
|
|
if (!measurement && endpoint.hasWrist && endpoint.hasHandShape && endpoint.missedFrames <= 30 &&
|
|
recoverableHandState && fused[person].meta[side].handValid &&
|
|
fused[person].meta[side].handScore >= 2.0 &&
|
|
fused[person].meta[side].linkToSelectedLimb <= 0.08) {
|
|
Vec2 measured = vec(fused[person].pose.keypoints[handStart]);
|
|
Vec2 prediction = add(endpoint.wrist, endpoint.velocity);
|
|
double gate = std::min(0.20, 0.05 + 0.008 * std::max(endpoint.missedFrames, 1)) * boxDiagonal(fused[person].pose.box);
|
|
if (norm(sub(measured, prediction)) <= gate) {
|
|
Measurement recovered;
|
|
for (int i = 0; i < 21; ++i) recovered.hand[i] = vec(fused[person].pose.keypoints[handStart + i]);
|
|
recovered.provisional = true; recovered.quality = fused[person].meta[side].handScore; recovered.source = "temporal_recovery";
|
|
measurement = recovered;
|
|
}
|
|
}
|
|
LimbState outputState;
|
|
std::array<Vec2, 21> outputHand{};
|
|
bool hasOutput = false;
|
|
bool returningAfterLong = measurement && endpoint.missedFrames > 12;
|
|
if (measurement) {
|
|
Vec2 measuredWrist = measurement->hand[0];
|
|
std::array<Vec2, 21> measuredShape{};
|
|
for (int i = 0; i < 21; ++i) measuredShape[i] = sub(measurement->hand[i], measuredWrist);
|
|
if (!endpoint.hasWrist || endpoint.missedFrames > 12) {
|
|
endpoint.wrist = measuredWrist; endpoint.velocity = {}; endpoint.handShape = measuredShape;
|
|
} else {
|
|
Vec2 prediction = add(endpoint.wrist, endpoint.velocity);
|
|
Vec2 residual = sub(measuredWrist, prediction);
|
|
double alpha = measurement->precise ? 0.78 : 0.88;
|
|
double beta = measurement->precise ? 0.18 : 0.24;
|
|
endpoint.wrist = add(prediction, mul(residual, alpha));
|
|
endpoint.velocity = add(mul(endpoint.velocity, 0.82), mul(residual, beta));
|
|
double shapeAlpha = measurement->precise ? 0.55 : 0.70;
|
|
for (int i = 0; i < 21; ++i) endpoint.handShape[i] = add(mul(measuredShape[i], shapeAlpha), mul(endpoint.handShape[i], 1.0 - shapeAlpha));
|
|
}
|
|
endpoint.hasWrist = endpoint.hasHandShape = true;
|
|
endpoint.lastObservedFrame = frameIndex;
|
|
endpoint.missedFrames = 0;
|
|
outputState = measurement->source == "global_reassignment" ? LimbState::Reassigned : measurement->source == "temporal_recovery" ? LimbState::Recovered : measurement->precise ? LimbState::Observed : measurement->provisional ? LimbState::Provisional : LimbState::Interpolated;
|
|
for (int i = 0; i < 21; ++i) outputHand[i] = add(endpoint.wrist, endpoint.handShape[i]);
|
|
hasOutput = true;
|
|
} else {
|
|
++endpoint.missedFrames;
|
|
if (!endpoint.hasWrist || endpoint.missedFrames > 30) outputState = LimbState::ReachableLong;
|
|
else if (endpoint.missedFrames <= 12) {
|
|
endpoint.wrist = add(endpoint.wrist, endpoint.velocity);
|
|
endpoint.velocity = mul(endpoint.velocity, 0.86);
|
|
for (int i = 0; i < 21; ++i) outputHand[i] = add(endpoint.wrist, endpoint.handShape[i]);
|
|
hasOutput = true; outputState = LimbState::PredictedShort;
|
|
} else outputState = LimbState::ReachableMedium;
|
|
}
|
|
Vec2 smoothedShoulder = denormalized(state.smoothed[shoulderIndex], fused[person].pose.box);
|
|
Vec2 smoothedElbow = denormalized(state.smoothed[elbowIndex], fused[person].pose.box);
|
|
Keypoint shoulder = fused[person].pose.keypoints[shoulderIndex];
|
|
shoulder.x = static_cast<float>(smoothedShoulder.x);
|
|
shoulder.y = static_cast<float>(smoothedShoulder.y);
|
|
Keypoint elbow = fused[person].pose.keypoints[elbowIndex];
|
|
elbow.x = static_cast<float>(smoothedElbow.x);
|
|
elbow.y = static_cast<float>(smoothedElbow.y);
|
|
Keypoint wrist = result.smoothedPeople[person].keypoints[wristIndex];
|
|
std::array<Keypoint, 21> hand{};
|
|
for (int i = 0; i < 21; ++i) {
|
|
hand[i] = result.smoothedPeople[person].keypoints[handStart + i];
|
|
}
|
|
if (hasOutput) {
|
|
auto& ownSamples = state.boneSamples[side];
|
|
auto& oppositeSamples = state.boneSamples[1 - side];
|
|
auto const& samples = ownSamples.size() < 10 && oppositeSamples.size() >= 10
|
|
? oppositeSamples : ownSamples;
|
|
double upperRatio = 0.24, forearmRatio = 0.22;
|
|
if (!samples.empty()) {
|
|
std::vector<double> upperValues, forearmValues;
|
|
upperValues.reserve(samples.size());
|
|
forearmValues.reserve(samples.size());
|
|
for (auto const& sample : samples) {
|
|
upperValues.push_back(sample[0]);
|
|
forearmValues.push_back(sample[1]);
|
|
}
|
|
upperRatio = median(std::move(upperValues));
|
|
forearmRatio = median(std::move(forearmValues));
|
|
}
|
|
double diagonal = boxDiagonal(fused[person].pose.box);
|
|
double upperLength = upperRatio * diagonal;
|
|
double forearmLength = forearmRatio * diagonal;
|
|
double totalLength = upperLength + forearmLength;
|
|
if (totalLength > 0.95 * diagonal) {
|
|
double lengthScale = 0.95 * diagonal / totalLength;
|
|
upperLength *= lengthScale;
|
|
forearmLength *= lengthScale;
|
|
}
|
|
Vec2 referenceElbow = returningAfterLong || !endpoint.hasLastElbow
|
|
? vec(fused[person].pose.keypoints[elbowIndex]) : endpoint.lastElbow;
|
|
if (!fused[person].meta[side].limbConsensus || outputState != LimbState::Observed) {
|
|
Vec2 solved = solveTwoBoneElbow(
|
|
smoothedShoulder, outputHand[0], upperLength, forearmLength, referenceElbow);
|
|
elbow.x = static_cast<float>(solved.x);
|
|
elbow.y = static_cast<float>(solved.y);
|
|
}
|
|
endpoint.lastElbow = vec(elbow);
|
|
endpoint.hasLastElbow = true;
|
|
Vec2 armDirection = sub(outputHand[0], smoothedShoulder);
|
|
if (norm(armDirection) > 1e-6) {
|
|
endpoint.lastDirection = mul(armDirection, 1.0 / norm(armDirection));
|
|
endpoint.hasLastDirection = true;
|
|
}
|
|
wrist.x = outputHand[0].x; wrist.y = outputHand[0].y;
|
|
for (int i = 0; i < 21; ++i) {
|
|
hand[i].x = outputHand[i].x; hand[i].y = outputHand[i].y;
|
|
}
|
|
}
|
|
auto& v2Pose = result.v2People[person];
|
|
v2Pose.keypoints[shoulderIndex] = shoulder;
|
|
v2Pose.keypoints[elbowIndex] = elbow;
|
|
v2Pose.keypoints[wristIndex] = wrist;
|
|
if (hasOutput) {
|
|
for (int point = 0; point < 21; ++point) v2Pose.keypoints[handStart + point] = hand[point];
|
|
}
|
|
result.limbs.push_back({
|
|
fused[person].pose.box.trackId, side == 0, outputState,
|
|
elbow, wrist, hand, fused[person].pose.box,
|
|
fused[person].meta[side].distanceEligible,
|
|
fused[person].meta[side].alarmEligible,
|
|
static_cast<float>(fused[person].meta[side].handScore)});
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
} // namespace fangjia
|