SHA256
116 lines
3.4 KiB
C++
116 lines
3.4 KiB
C++
#pragma once
|
|
|
|
#include <algorithm>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
#include <unistd.h>
|
|
|
|
namespace r1_vision {
|
|
|
|
struct Detection {
|
|
bool valid = false;
|
|
std::string class_name;
|
|
double timestamp = 0.0;
|
|
double confidence = 0.0;
|
|
double u = 0.0;
|
|
double v = 0.0;
|
|
};
|
|
|
|
inline bool ExtractNumber(const std::string& json, const std::string& key, double& value) {
|
|
const std::string search = "\"" + key + "\"";
|
|
const size_t p = json.find(search);
|
|
if (p == std::string::npos) return false;
|
|
const size_t colon = json.find(':', p);
|
|
if (colon == std::string::npos) return false;
|
|
try {
|
|
value = std::stod(json.substr(colon + 1));
|
|
return true;
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
inline bool ExtractString(const std::string& json, const std::string& key, std::string& value) {
|
|
const std::string search = "\"" + key + "\"";
|
|
const size_t p = json.find(search);
|
|
if (p == std::string::npos) return false;
|
|
const size_t colon = json.find(':', p);
|
|
if (colon == std::string::npos) return false;
|
|
const size_t q1 = json.find('"', colon + 1);
|
|
if (q1 == std::string::npos) return false;
|
|
const size_t q2 = json.find('"', q1 + 1);
|
|
if (q2 == std::string::npos) return false;
|
|
value = json.substr(q1 + 1, q2 - q1 - 1);
|
|
return true;
|
|
}
|
|
|
|
inline bool ReadDetection(const std::string& path, Detection& d) {
|
|
std::ifstream f(path);
|
|
if (!f) return false;
|
|
std::stringstream buffer;
|
|
buffer << f.rdbuf();
|
|
const std::string json = buffer.str();
|
|
if (json.empty()) return false;
|
|
|
|
if (!ExtractNumber(json, "timestamp", d.timestamp)) return false;
|
|
if (!ExtractNumber(json, "confidence", d.confidence)) return false;
|
|
if (!ExtractNumber(json, "u", d.u)) return false;
|
|
if (!ExtractNumber(json, "v", d.v)) return false;
|
|
ExtractString(json, "class", d.class_name);
|
|
d.valid = true;
|
|
return true;
|
|
}
|
|
|
|
inline bool IsFresh(const Detection& d, double max_age_sec = 0.5) {
|
|
const double now = std::chrono::duration<double>(
|
|
std::chrono::system_clock::now().time_since_epoch()).count();
|
|
return std::isfinite(d.timestamp) && std::abs(now - d.timestamp) <= max_age_sec;
|
|
}
|
|
|
|
inline bool GetStableBottleCenter(
|
|
const std::string& path,
|
|
double& u,
|
|
double& v,
|
|
int samples = 8,
|
|
double min_confidence = 0.7,
|
|
double timeout_sec = 5.0)
|
|
{
|
|
std::vector<double> us;
|
|
std::vector<double> vs;
|
|
const auto start = std::chrono::steady_clock::now();
|
|
|
|
while (static_cast<int>(us.size()) < samples) {
|
|
Detection d;
|
|
if (ReadDetection(path, d) && d.valid &&
|
|
(d.class_name.empty() || d.class_name == "bottle") &&
|
|
d.confidence >= min_confidence && IsFresh(d)) {
|
|
us.push_back(d.u);
|
|
vs.push_back(d.v);
|
|
}
|
|
|
|
const double elapsed = std::chrono::duration<double>(
|
|
std::chrono::steady_clock::now() - start).count();
|
|
if (elapsed > timeout_sec) return false;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(25));
|
|
}
|
|
|
|
std::sort(us.begin(), us.end());
|
|
std::sort(vs.begin(), vs.end());
|
|
const auto median = [](const std::vector<double>& x) {
|
|
const size_t n = x.size();
|
|
if (n & 1u) return x[n / 2];
|
|
return 0.5 * (x[n / 2 - 1] + x[n / 2]);
|
|
};
|
|
|
|
u = median(us);
|
|
v = median(vs);
|
|
return true;
|
|
}
|
|
|
|
} // namespace r1_vision
|