Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
agrobot_base/AgroBase/livox_visual_debugger/metrics.cpp

537 lines
21 KiB
C++
Raw Normal View History

integracao resultados da operacao
2025-11-05 20:03:46 +00:00
#include "metrics.hpp"
#include <mutex>
#include <atomic>
#include <thread>
#include <optional>
#include <cmath>
#include <cfloat>
// PCL
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include "voxel_lite.cpp"
#include <pcl/filters/crop_box.h>
#include <Eigen/Dense>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include "status_snapshot.hpp"
using namespace std::chrono;
namespace {
std::mutex g_mtx_params;
MetricsParams gP;
// ---- fila latest-only para nuvens ----
std::mutex g_mtx_queue;
std::optional<CloudRGB> g_last_cloud;
uint64_t g_last_cloud_ms = 0;
std::atomic<bool> g_run{ false };
std::thread g_thr;
pcl::PointCloud<pcl::PointXYZ>::Ptr toPclXYZ(const CloudRGB& merged) {
auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
out->points.reserve(merged.pts.size());
for (const auto& q : merged.pts) {
// Se tiver NaN/Inf, pule
if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z)) continue;
out->points.push_back(pcl::PointXYZ{ q.x, q.y, q.z });
}
out->width = static_cast<uint32_t>(out->points.size());
out->height = 1; // unorganized
out->is_dense = false; // conservador
return out;
}
inline Distance6 computeDistances6(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& pc, const MetricsParams& P, uint64_t t_ms) {
Distance6 d{}; d.t_ms = t_ms;
auto upd_min = [](float v, float& cur) { if (cur == 0.f || v < cur) cur = v; };
for (const auto& p : pc->points) {
if (p.x > 0) upd_min(p.x, d.front); else if (p.x < 0) upd_min(-p.x, d.back);
if (p.y > 0) upd_min(p.y, d.left); else if (p.y < 0) upd_min(-p.y, d.right);
if (p.z > 0) upd_min(p.z, d.up); else if (p.z < 0) upd_min(-p.z, d.down);
}
if (d.front > 0 && d.front > P.x_max) d.front = 0;
if (d.back > 0 && d.back > std::abs(P.x_min)) d.back = 0;
if (d.left > 0 && d.left > P.y_max) d.left = 0;
if (d.right > 0 && d.right > std::abs(P.y_min)) d.right = 0;
if (d.up > 0 && d.up > P.z_max) d.up = 0;
if (d.down > 0 && d.down > std::abs(P.z_min)) d.down = 0;
return d;
}
// pipeline síncrona (usada pela thread)
static void update_from_cloud_sync(const CloudRGB& merged, uint64_t now_ms) {
MetricsParams P; { std::lock_guard<std::mutex> lk(g_mtx_params); P = gP; }
// 1) ROI (ping-pong, sem in-place)
auto A = toPclXYZ(merged);
if (A->empty()) { Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return; }
// Sanitize: remova NaNs e ajuste header (evita PassThrough iterar fora do vetor)
{
std::vector<int> idxmap;
pcl::removeNaNFromPointCloud(*A, *A, idxmap);
A->width = static_cast<uint32_t>(A->points.size());
A->height = 1;
A->is_dense = false;
if (A->empty()) { Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return; }
}
/*auto B = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PassThrough<pcl::PointXYZ> pass;
auto pass_axis = [&](const char* axis, float lo, float hi) {
pass.setInputCloud(A);
pass.setFilterFieldName(axis);
pass.setFilterLimits(lo, hi);
// RESET COMPLETO do buffer de saída (clear() não zera header)
B->points.clear();
B->width = 0; B->height = 1; B->is_dense = false;
pass.filter(*B);
// corrige header após o filtro
B->width = static_cast<uint32_t>(B->points.size());
B->height = 1;
B->is_dense = false;
A.swap(B);
};
try {
pass_axis("x", P.x_min, P.x_max);
pass_axis("y", P.y_min, P.y_max);
pass_axis("z", P.z_min, P.z_max);
}
catch (const std::exception& e) {
std::cerr << "[ROI] PassThrough exception: " << e.what() << std::endl;
Core::set_distances(Distance6{ .t_ms = now_ms });
Core::set_bboxes({});
return;
}
catch (...) {
std::cerr << "[ROI] PassThrough unknown exception" << std::endl;
Core::set_distances(Distance6{ .t_ms = now_ms });
Core::set_bboxes({});
return;
}*/
/*auto roi_manual = [&](const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& in,
float xmin, float xmax, float ymin, float ymax, float zmin, float zmax)
-> pcl::PointCloud<pcl::PointXYZ>::Ptr
{
auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
out->points.reserve(in->points.size());
for (const auto& p : in->points) {
if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;
if (p.x < xmin || p.x > xmax) continue;
if (p.y < ymin || p.y > ymax) continue;
if (p.z < zmin || p.z > zmax) continue;
out->points.push_back(p);
}
out->width = static_cast<uint32_t>(out->points.size());
out->height = 1;
out->is_dense = false;
return out;
};
// uso no teu código (substitui os 3 PassThrough):
A = roi_manual(A, P.x_min, P.x_max, P.y_min, P.y_max, P.z_min, P.z_max);*/
/*pcl::CropBox<pcl::PointXYZ> cb;
cb.setMin(Eigen::Vector4f(P.x_min, P.y_min, P.z_min, 1.0f));
cb.setMax(Eigen::Vector4f(P.x_max, P.y_max, P.z_max, 1.0f));
cb.setInputCloud(A);
auto B = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
cb.filter(*B);
B->width = static_cast<uint32_t>(B->points.size());
B->height = 1;
B->is_dense = false;
A.swap(B);*/
/*auto filter_axis = [&](const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& in,
const char* axis, float lo, float hi)
-> pcl::PointCloud<pcl::PointXYZ>::Ptr
{
std::vector<int> keep;
keep.reserve(in->points.size());
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(in);
pass.setFilterFieldName(axis);
pass.setFilterLimits(lo, hi);
pass.filter(keep); // ← pega só os índices válidos
auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
out->points.reserve(keep.size());
pcl::copyPointCloud(*in, keep, *out);
out->width = static_cast<uint32_t>(out->points.size());
out->height = 1;
out->is_dense = false;
return out;
};
// encadeia os 3 eixos:
A = filter_axis(A, "x", P.x_min, P.x_max);
A = filter_axis(A, "y", P.y_min, P.y_max);
A = filter_axis(A, "z", P.z_min, P.z_max);*/
/*struct VoxelKey {
int ix, iy, iz;
bool operator==(const VoxelKey& o) const {
return ix == o.ix && iy == o.iy && iz == o.iz;
}
};
struct VoxelKeyHash {
std::size_t operator()(const VoxelKey& k) const noexcept {
// hash combinando 3 ints
// isso não precisa ser perfeito, só razoável
std::size_t h1 = std::hash<int>()(k.ix);
std::size_t h2 = std::hash<int>()(k.iy);
std::size_t h3 = std::hash<int>()(k.iz);
return h1 ^ (h2 << 1) ^ (h3 << 2);
}
};
auto _DownsampleVoxelLike = [&](const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& in, float leaf)
{
auto out = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
out->points.reserve(in->points.size());
std::unordered_map<VoxelKey, pcl::PointXYZRGB, VoxelKeyHash> voxels;
voxels.reserve(in->points.size());
const float inv_leaf = 1.0f / leaf;
for (const auto& p : in->points) {
if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z))
continue;
VoxelKey key{
(int)std::floor(p.x * inv_leaf),
(int)std::floor(p.y * inv_leaf),
(int)std::floor(p.z * inv_leaf)
};
// mantém o primeiro ponto daquele voxel
if (voxels.find(key) == voxels.end()) {
voxels[key] = p;
}
}
out->points.reserve(voxels.size());
for (const auto& kv : voxels) {
out->points.push_back(kv.second);
}
out->width = static_cast<uint32_t>(out->points.size());
out->height = 1;
out->is_dense = false;
return out;
};
auto CastXYZRGBtoXYZ = [](const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& in) {
auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
if (!in) return out;
out->points.reserve(in->points.size());
for (const auto& p : in->points) out->points.emplace_back(p.x, p.y, p.z);
out->width = static_cast<uint32_t>(out->points.size());
out->height = 1; out->is_dense = false;
return out;
};
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud_in->points.reserve(merged.pts.size());
for (const auto& q : merged.pts) {
// supondo que CloudRGB usa metros em x/y/z
if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z)) continue;
pcl::PointXYZRGB p;
p.x = q.x; p.y = q.y; p.z = q.z;
p.r = q.r; p.g = q.g; p.b = q.b; // se não tiver RGB, deixe 0
cloud_in->points.push_back(p);
}
cloud_in->width = static_cast<uint32_t>(cloud_in->points.size());
cloud_in->height = 1;
cloud_in->is_dense = false;
if (cloud_in->points.size() < 20) {
Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr roi(new pcl::PointCloud<pcl::PointXYZRGB>);
roi->reserve(cloud_in->points.size());
for (const auto& p : cloud_in->points) {
float dist2 = p.x * p.x + p.y * p.y + p.z * p.z;
if (dist2 > 9.0f) continue;
if (p.z < -0.2f) continue;
if (p.z > 2.0f) continue;
roi->points.push_back(p);
}
if (roi->points.size() < 20) {
// quase nada perto -> nada pra detectar
return;
}
roi->width = static_cast<uint32_t>(roi->points.size());
roi->height = 1;
roi->is_dense = true;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr coarse = _DownsampleVoxelLike(roi, 0.05f);
if (coarse->points.size() < 20) {
return;
}
auto pc_vx = CastXYZRGBtoXYZ(coarse);*/
if (A->empty()) { Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return; }
// 2) Voxel (guard de leaf)
/*auto pc_vx = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::VoxelGrid<pcl::PointXYZ> vg; vg.setInputCloud(A);
const float leaf = (P.voxel_leaf > 1e-5f) ? P.voxel_leaf : 0.05f;
vg.setLeafSize(leaf, leaf, leaf);
vg.filter(*pc_vx);*/
// 2) Downsample voxel-like (sem PCL::VoxelGrid)
auto pc_vx = VoxelLite::DownsampleVoxelLikeXYZ(A, P.voxel_leaf);
// 3) Distâncias
auto d6 = computeDistances6(pc_vx, P, now_ms);
Core::set_distances(d6);
// 4) Clusters
if (pc_vx->size() < (size_t)P.min_cluster) {
Core::set_bboxes({});
return;
}
struct VKey { int x, y, z; };
struct VKeyHash {
size_t operator()(const VKey& k) const noexcept {
// hash 3D simples
uint64_t h = (uint64_t)(uint32_t)k.x * 73856093u
^ (uint64_t)(uint32_t)k.y * 19349663u
^ (uint64_t)(uint32_t)k.z * 83492791u;
return (size_t)h;
}
};
struct VKeyEq {
bool operator()(const VKey& a, const VKey& b) const noexcept {
return a.x == b.x && a.y == b.y && a.z == b.z;
}
};
// vizinhança 26-conexa
static const int NBR[26][3] = {
{-1,-1,-1},{-1,-1,0},{-1,-1,1},{-1,0,-1},{-1,0,0},{-1,0,1},{-1,1,-1},{-1,1,0},{-1,1,1},
{0,-1,-1},{0,-1,0},{0,-1,1},{0,0,-1}, {0,0,1},{0,1,-1},{0,1,0},{0,1,1},
{1,-1,-1},{1,-1,0},{1,-1,1},{1,0,-1},{1,0,0},{1,0,1},{1,1,-1},{1,1,0},{1,1,1}
};
if (!pc_vx || pc_vx->empty()) {
Core::set_bboxes({});
return;
}
const float vox = std::max(0.01f, P.voxel_leaf); // segurança
// 1) marca voxels ocupados (usando a nuvem já voxelizada pc_vx)
std::unordered_set<VKey, VKeyHash, VKeyEq> occ;
occ.reserve(pc_vx->size());
// ROI rápida (aproveita seus limites já definidos)
for (const auto& p : pc_vx->points) {
if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;
if (p.x < P.x_min || p.x > P.x_max ||
p.y < P.y_min || p.y > P.y_max ||
p.z < P.z_min || p.z > P.z_max) continue;
VKey k{ (int)std::floor(p.x / vox), (int)std::floor(p.y / vox), (int)std::floor(p.z / vox) };
occ.insert(k);
}
if (occ.empty()) {
Core::set_bboxes({});
return;
}
// 2) componentes conexos em voxels ocupados
std::unordered_map<VKey, uint8_t, VKeyHash, VKeyEq> vis; // 0/1 visitado
vis.reserve(occ.size());
std::vector<VKey> stack; stack.reserve(4096);
std::vector<BBox3D> boxes; boxes.reserve(256);
// limite opcional de saída (evita overlays gigantes)
constexpr size_t MAX_BOXES_OUT = 200;
for (const auto& k0 : occ) {
if (vis[k0]) continue;
vis[k0] = 1;
stack.clear();
stack.push_back(k0);
// AABB em coord reais
float min_x = FLT_MAX, min_y = FLT_MAX, min_z = FLT_MAX;
float max_x = -FLT_MAX, max_y = -FLT_MAX, max_z = -FLT_MAX;
size_t vox_count = 0;
while (!stack.empty()) {
VKey k = stack.back(); stack.pop_back();
++vox_count;
// bounds do voxel → atualiza AABB
const float x0 = k.x * vox, x1 = (k.x + 1) * vox;
const float y0 = k.y * vox, y1 = (k.y + 1) * vox;
const float z0 = k.z * vox, z1 = (k.z + 1) * vox;
if (x0 < min_x) min_x = x0; if (x1 > max_x) max_x = x1;
if (y0 < min_y) min_y = y0; if (y1 > max_y) max_y = y1;
if (z0 < min_z) min_z = z0; if (z1 > max_z) max_z = z1;
// expande 26-vizinhos
for (int n = 0; n < 26; ++n) {
VKey nb{ k.x + NBR[n][0], k.y + NBR[n][1], k.z + NBR[n][2] };
if (occ.find(nb) == occ.end()) continue;
auto it = vis.find(nb);
if (it != vis.end() && it->second) continue;
vis[nb] = 1;
stack.push_back(nb);
}
}
// 3) filtros de tamanho com SEUS campos já existentes
// usamos min/max em "conta de voxels", reaproveitando P.min_cluster / P.max_cluster
if (vox_count >= (size_t)std::max(1, P.min_cluster) &&
vox_count <= (size_t)std::max(P.min_cluster, P.max_cluster)) {
// centro e dimensões
const float cx = 0.5f * (min_x + max_x);
const float cy = 0.5f * (min_y + max_y);
const float cz = 0.5f * (min_z + max_z);
const float w = std::fabs(max_y - min_y);
const float h = std::fabs(max_z - min_z);
const float d = std::fabs(max_x - min_x);
// distâncias
const float dist_m = std::sqrt(cx * cx + cy * cy + cz * cz); // 3D
const float dist_xy_m = std::sqrt(cx * cx + cy * cy); // plano chão
const float frente_m = cx; // eixo X é "frente"
BBox3D out{
min_x, min_y, min_z,
max_x, max_y, max_z,
now_ms
};
out.voxels = static_cast<uint32_t>(vox_count);
out.cx = cx; out.cy = cy; out.cz = cz;
out.w = w; out.h = h; out.d = d;
out.dist_m = dist_m;
out.dist_xy_m = dist_xy_m;
out.frente_m = frente_m;
boxes.push_back(std::move(out));
if (boxes.size() >= MAX_BOXES_OUT) break;
}
}
Core::set_bboxes(boxes);
}
void worker_loop() {
using namespace std::chrono;
const uint64_t intervalo_ms = 100; // processa a cada 100 ms (ajuste conforme quiser)
uint64_t ultimo_processamento = 0;
while (g_run.load()) {
CloudRGB c;
uint64_t t = 0;
{
std::lock_guard<std::mutex> lk(g_mtx_queue);
if (g_last_cloud.has_value()) {
c = std::move(*g_last_cloud);
t = g_last_cloud_ms;
g_last_cloud.reset();
}
}
// tempo atual
uint64_t agora_ms = duration_cast<milliseconds>(
steady_clock::now().time_since_epoch()).count();
if (t != 0 && (agora_ms - ultimo_processamento >= intervalo_ms)) {
ultimo_processamento = agora_ms;
update_from_cloud_sync(c, t);
}
else {
// dorme curto pra não travar a thread
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
}
}
} // anon
namespace Metrics {
void init(const MetricsParams& p) { std::lock_guard<std::mutex> lk(g_mtx_params); gP = p; }
void set_params(const MetricsParams& p) { std::lock_guard<std::mutex> lk(g_mtx_params); gP = p; }
MetricsParams get_params() { std::lock_guard<std::mutex> lk(g_mtx_params); return gP; }
void start_worker() {
if (g_run.exchange(true)) return;
g_thr = std::thread(worker_loop);
}
void stop_worker() {
if (!g_run.exchange(false)) return;
if (g_thr.joinable()) g_thr.join();
}
void enqueue_cloud(const CloudRGB& merged, uint64_t now_ms) {
// guarda só a mais recente
std::lock_guard<std::mutex> lk(g_mtx_queue);
g_last_cloud = merged;
g_last_cloud_ms = now_ms;
}
void update_from_imu(float ax, float ay, float az, float gx, float gy, float gz, uint64_t t_ms) {
auto timestamp_ms_0 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
Core::set_last_imu(ImuRaw{ ax, ay, az, gx, gy, gz, t_ms });
static uint64_t last_ms = 0;
auto rpy = Core::get_attitude();
if (last_ms == 0) { last_ms = t_ms; return; }
float dt = (t_ms - last_ms) / 1000.0f; last_ms = t_ms;
float roll_gyro = rpy.roll + gx * dt;
float pitch_gyro = rpy.pitch + gy * dt;
float yaw_gyro = rpy.yaw + gz * dt;
float denom = std::sqrt(ay * ay + az * az); if (denom < 1e-6f) denom = 1e-6f;
float roll_acc = std::atan2(ay, az);
float pitch_acc = std::atan2(-ax, denom);
const float a = get_params().alpha_complementar;
AttitudeRPY fused;
fused.timestamp = timestamp_ms_0;
fused.roll = Core::wrapPi(a * roll_gyro + (1.f - a) * roll_acc);
fused.pitch = Core::wrapPi(a * pitch_gyro + (1.f - a) * pitch_acc);
fused.yaw = Core::wrapPi(yaw_gyro);
auto timestamp_ms_1 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
fused.latencia = timestamp_ms_1 - timestamp_ms_0;
fused.frequencia = 1.0f / dt;
Core::set_attitude(fused);
}
} // namespace Metrics