384 lines
16 KiB
C++
384 lines
16 KiB
C++
#include "viewer_debug.hpp"
|
|
#include "core.hpp"
|
|
|
|
#include <pcl/visualization/pcl_visualizer.h>
|
|
#include <pcl/filters/voxel_grid.h>
|
|
#include <pcl/segmentation/extract_clusters.h>
|
|
#include <algorithm>
|
|
#include <sstream>
|
|
#include <iomanip>
|
|
#include <chrono>
|
|
#include <deque>
|
|
#include <boost/make_shared.hpp>
|
|
|
|
#include <vtkSmartPointer.h>
|
|
#include <vtkPolyData.h>
|
|
#include <vtkPoints.h>
|
|
#include <vtkCellArray.h>
|
|
#include <vtkLine.h>
|
|
|
|
namespace {
|
|
std::string degC(reinterpret_cast<const char*>(u8"°C"));
|
|
std::string deg(reinterpret_cast<const char*>(u8"°"));
|
|
|
|
std::atomic<bool> g_run{ false };
|
|
std::thread g_thr;
|
|
|
|
std::atomic<bool> g_draw_boxes{ false };
|
|
std::atomic<bool> g_color_by_distance{ true };
|
|
std::atomic<bool> g_mode_map{ false };
|
|
std::atomic<bool> g_use_slam{ false };
|
|
std::atomic<bool> g_sensor_invertido{ false };
|
|
std::atomic<bool> g_show_axis{ false };
|
|
|
|
|
|
// colormap tipo "jet" (0..1 -> RGB)
|
|
static inline void ColorMapJet(float v, uint8_t& r, uint8_t& g, uint8_t& b) {
|
|
if (v < 0) v = 0; if (v > 1) v = 1;
|
|
float x = 4.0f * v;
|
|
float rf = std::clamp(std::min(x - 1.5f, -x + 4.5f), 0.0f, 1.0f);
|
|
float gf = std::clamp(std::min(x - 0.5f, -x + 3.5f), 0.0f, 1.0f);
|
|
float bf = std::clamp(std::min(x + 0.5f, -x + 2.5f), 0.0f, 1.0f);
|
|
r = (uint8_t)(rf * 255.f);
|
|
g = (uint8_t)(gf * 255.f);
|
|
b = (uint8_t)(bf * 255.f);
|
|
}
|
|
|
|
// recolore de acordo com o modo: distância radial (true) ou altura Z (false)
|
|
static inline void colorizePoint(pcl::PointXYZRGB& p, bool by_dist) {
|
|
uint8_t r, g, b;
|
|
if (by_dist) {
|
|
float dist = std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
|
|
ColorMapJet(dist / 5.0f, r, g, b); // 5 m = referência; ajuste se quiser
|
|
}
|
|
else {
|
|
// normaliza Z entre [-0.2, 1.5] como no código antigo
|
|
float zmin = -0.2f, zmax = 1.5f;
|
|
float v = (p.z - zmin) / std::max(1e-3f, (zmax - zmin));
|
|
ColorMapJet(v, r, g, b);
|
|
}
|
|
p.r = r; p.g = g; p.b = b;
|
|
}
|
|
|
|
// ---- helpers HUD ----
|
|
static void addHudTop(pcl::visualization::PCLVisualizer::Ptr& viewer) {
|
|
viewer->removeShape("hud_top");
|
|
auto rw = viewer->getRenderWindow();
|
|
int* size = rw->GetSize();
|
|
int win_w = size[0], win_h = size[1];
|
|
|
|
const int margin = 10;
|
|
const int font_px = 14;
|
|
const int line_h = font_px + 4;
|
|
const int x = margin;
|
|
const int y = win_h - margin - line_h;
|
|
|
|
auto temp = Core::get_temperature_c();
|
|
auto rpy = Core::get_attitude();
|
|
|
|
std::stringstream ss;
|
|
ss.setf(std::ios::fixed);
|
|
ss << std::setprecision(1);
|
|
ss << "TEMP: " << temp << degC
|
|
<< " |"
|
|
<< " R:" << Core::rad2deg(rpy.roll) << deg
|
|
<< " P:" << Core::rad2deg(rpy.pitch) << deg
|
|
<< " Y:" << Core::rad2deg(rpy.yaw) << deg;
|
|
|
|
viewer->addText(ss.str(), x, y, font_px, 1.0, 1.0, 1.0, "hud_top");
|
|
}
|
|
|
|
static void addHudBottom(pcl::visualization::PCLVisualizer::Ptr& viewer) {
|
|
viewer->removeShape("hud_bottom");
|
|
std::stringstream ss;
|
|
ss << (g_mode_map.load() ? "MODE: MAP " : "MODE: LIVE ")
|
|
<< (g_color_by_distance.load() ? "DISTANCIA " : "ALTURA ")
|
|
<< "| SLAM: " << (g_use_slam.load() ? "ON" : "OFF")
|
|
<< " | BOX: " << (g_draw_boxes.load() ? "ON" : "OFF");
|
|
viewer->addText(ss.str(), 10, 10, 14, 1.0, 1.0, 1.0, "hud_bottom");
|
|
}
|
|
|
|
// (simplificado) converte CloudRGB ? pcl
|
|
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr toPcl(const CloudRGB& c) {
|
|
auto pc = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>());
|
|
pc->points.reserve(c.pts.size());
|
|
for (auto& q : c.pts) {
|
|
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;
|
|
pc->points.push_back(p);
|
|
}
|
|
pc->width = (uint32_t)pc->points.size();
|
|
pc->height = 1;
|
|
pc->is_dense = true;
|
|
return pc;
|
|
}
|
|
|
|
// (debug) desenha “bússola” de distâncias
|
|
static void drawDistances3D(pcl::visualization::PCLVisualizer::Ptr& viewer) {
|
|
using std::string;
|
|
for (auto id : { "linha_frente","linha_tras","linha_esquerda","linha_direita","linha_cima","linha_baixo",
|
|
"texto_frente","texto_tras","texto_esquerda","texto_direita","texto_cima","texto_baixo" })
|
|
viewer->removeShape(id);
|
|
|
|
auto d = Core::get_distances();
|
|
auto draw = [&](const string& idLine, const string& idText,
|
|
float vx, float vy, float vz, float dist, float cr, float cg, float cb, const char* label) {
|
|
if (dist <= 0.001f) dist = 0.0f;
|
|
Eigen::Vector3f dst = Eigen::Vector3f(vx, vy, vz).normalized() * dist;
|
|
viewer->addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(dst.x(), dst.y(), dst.z()), cr, cg, cb, idLine);
|
|
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, idLine);
|
|
|
|
char txt[64];
|
|
if (dist > 0.0f) std::snprintf(txt, sizeof(txt), "%s: %.0f cm", label, dist * 100.f);
|
|
else std::snprintf(txt, sizeof(txt), "%s: --", label);
|
|
viewer->addText3D(txt, pcl::PointXYZ(dst.x(), dst.y(), dst.z()), 0.05, cr, cg, cb, idText);
|
|
};
|
|
|
|
draw("linha_frente", "texto_frente", 1, 0, 0, d.front, 1, 0, 0, "frente");
|
|
draw("linha_tras", "texto_tras", -1, 0, 0, d.back, 0.8, 0, 0, "tras");
|
|
draw("linha_direita", "texto_direita", 0, 1, 0, d.right, 0, 1, 0, "direita");
|
|
draw("linha_esquerda", "texto_esquerda", 0, -1, 0, d.left, 0, 0.8, 0, "esquerda");
|
|
draw("linha_cima", "texto_cima", 0, 0, 1, d.up, 0, 0, 1, "cima");
|
|
draw("linha_baixo", "texto_baixo", 0, 0, -1, d.down, 0, 0, 0.8, "baixo");
|
|
}
|
|
|
|
// compara AABB com tolerância
|
|
static inline bool approx_equal(const BBox3D& a, const BBox3D& b, float eps = 1e-4f) {
|
|
auto ae = [&](float x, float y) { return std::fabs(x - y) <= eps; };
|
|
return ae(a.min_x, b.min_x) && ae(a.min_y, b.min_y) && ae(a.min_z, b.min_z) &&
|
|
ae(a.max_x, b.max_x) && ae(a.max_y, b.max_y) && ae(a.max_z, b.max_z);
|
|
}
|
|
|
|
static const int BBOX_EDGES[12][2] = {
|
|
{0,1},{1,2},{2,3},{3,0}, {4,5},{5,6},{6,7},{7,4}, {0,4},{1,5},{2,6},{3,7}
|
|
};
|
|
|
|
static uint64_t g_last_draw_ms = 0;
|
|
static std::string g_actor_id = "bboxes_all";
|
|
static size_t g_last_n = 0;
|
|
|
|
// constrói um único polydata de linhas com TODAS as bboxes
|
|
static vtkSmartPointer<vtkPolyData> build_bbox_poly(const std::vector<BBox3D>& boxes, size_t max_boxes = SIZE_MAX) {
|
|
auto pts = vtkSmartPointer<vtkPoints>::New();
|
|
auto lines = vtkSmartPointer<vtkCellArray>::New();
|
|
|
|
const size_t N = std::min(boxes.size(), max_boxes);
|
|
pts->SetDataTypeToFloat();
|
|
pts->Allocate(8 * N);
|
|
lines->AllocateEstimate(12 * N, 2);
|
|
|
|
vtkIdType base = 0;
|
|
for (size_t i = 0; i < N; ++i) {
|
|
const auto& b = boxes[i];
|
|
const float x0 = b.min_x, y0 = b.min_y, z0 = b.min_z;
|
|
const float x1 = b.max_x, y1 = b.max_y, z1 = b.max_z;
|
|
|
|
// 8 vértices
|
|
vtkIdType id0 = pts->InsertNextPoint(x0, y0, z0);
|
|
vtkIdType id1 = pts->InsertNextPoint(x1, y0, z0);
|
|
vtkIdType id2 = pts->InsertNextPoint(x1, y1, z0);
|
|
vtkIdType id3 = pts->InsertNextPoint(x0, y1, z0);
|
|
vtkIdType id4 = pts->InsertNextPoint(x0, y0, z1);
|
|
vtkIdType id5 = pts->InsertNextPoint(x1, y0, z1);
|
|
vtkIdType id6 = pts->InsertNextPoint(x1, y1, z1);
|
|
vtkIdType id7 = pts->InsertNextPoint(x0, y1, z1);
|
|
|
|
const vtkIdType ids[8] = { id0,id1,id2,id3,id4,id5,id6,id7 };
|
|
|
|
// 12 arestas
|
|
for (int e = 0; e < 12; ++e) {
|
|
auto ln = vtkSmartPointer<vtkLine>::New();
|
|
ln->GetPointIds()->SetId(0, ids[BBOX_EDGES[e][0]]);
|
|
ln->GetPointIds()->SetId(1, ids[BBOX_EDGES[e][1]]);
|
|
lines->InsertNextCell(ln);
|
|
}
|
|
|
|
base += 8;
|
|
}
|
|
|
|
auto poly = vtkSmartPointer<vtkPolyData>::New();
|
|
poly->SetPoints(pts);
|
|
poly->SetLines(lines);
|
|
return poly;
|
|
}
|
|
|
|
// desenha/atualiza em 1 único ator
|
|
static void draw_bboxes_one_actor(pcl::visualization::PCLVisualizer::Ptr& viewer,
|
|
uint64_t now_ms,
|
|
uint64_t min_interval_ms = 120, // ~8 Hz
|
|
size_t max_boxes = 200) // cota de segurança
|
|
{
|
|
if (now_ms - g_last_draw_ms < min_interval_ms) return;
|
|
|
|
auto boxes = Core::get_bboxes();
|
|
if (boxes.empty()) {
|
|
if (g_last_n) { viewer->removeShape(g_actor_id); g_last_n = 0; }
|
|
g_last_draw_ms = now_ms;
|
|
return;
|
|
}
|
|
|
|
// (Opcional) cull simples: mantenha só as K mais próximas/do maior tamanho etc.
|
|
// std::partial_sort(...); boxes.resize(std::min(boxes.size(), max_boxes));
|
|
|
|
auto poly = build_bbox_poly(boxes, max_boxes);
|
|
|
|
// remove e recria SÓ UM ator
|
|
viewer->removeShape(g_actor_id);
|
|
viewer->addModelFromPolyData(poly, g_actor_id);
|
|
|
|
// propriedades visuais leves
|
|
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, g_actor_id);
|
|
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 1.5, g_actor_id);
|
|
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, g_actor_id);
|
|
// desliga “efeitos”
|
|
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_SHADING, 0.0, g_actor_id);
|
|
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
|
|
pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, g_actor_id);
|
|
|
|
g_last_n = std::min(boxes.size(), max_boxes);
|
|
g_last_draw_ms = now_ms;
|
|
}
|
|
|
|
void KeyboardCallback(const pcl::visualization::KeyboardEvent& event, void*) {
|
|
if (!event.keyDown()) return;
|
|
const std::string k = event.getKeySym();
|
|
|
|
auto toggle = [](std::atomic<bool>& f) {
|
|
// troca atômica: inverte o bit de forma thread-safe
|
|
bool expected = f.load(std::memory_order_relaxed);
|
|
while (!f.compare_exchange_weak(expected, !expected, std::memory_order_relaxed)) {}
|
|
};
|
|
|
|
if (k == "m" || k == "M") {
|
|
toggle(g_mode_map);
|
|
std::cout << "\n[KEY] Modo: " << (g_mode_map.load() ? "MAP" : "LIVE") << std::endl;
|
|
}
|
|
else if (k == "s" || k == "S") {
|
|
toggle(g_use_slam);
|
|
std::cout << "\n[KEY] SLAM: " << (g_use_slam.load() ? "ATIVADO (ICP ON)" : "DESATIVADO") << std::endl;
|
|
}
|
|
else if (k == "d" || k == "D") {
|
|
toggle(g_color_by_distance);
|
|
std::cout << "\n[KEY] Color: " << (g_color_by_distance.load() ? "DISTANCIA" : "ALTURA") << std::endl;
|
|
}
|
|
else if (k == "b" || k == "B") {
|
|
toggle(g_draw_boxes);
|
|
std::cout << "\n[KEY] Caixas: " << (g_draw_boxes.load() ? "ON" : "OFF") << std::endl;
|
|
}
|
|
else if (k == "i" || k == "I") {
|
|
toggle(g_sensor_invertido);
|
|
std::cout << "\n[KEY] Sensor invertido: " << (g_sensor_invertido.load() ? "ON" : "OFF") << std::endl;
|
|
}
|
|
else if (k == "a" || k == "A") {
|
|
toggle(g_show_axis);
|
|
std::cout << "\n[KEY] Mostrar eixos: " << (g_show_axis.load() ? "ON" : "OFF") << std::endl;
|
|
}
|
|
else if (k == "c" || k == "C") {
|
|
Core::ResetGlobalMap();
|
|
std::cout << "\n[KEY] MAP: limpo\n";
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
auto viewer = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer("Mid-360 Debug Viewer"));
|
|
viewer->setBackgroundColor(0, 0, 0);
|
|
viewer->addCoordinateSystem(1.0);
|
|
viewer->initCameraParameters();
|
|
|
|
auto interactor = viewer->getRenderWindow()->GetInteractor();
|
|
|
|
// === Keyboard ===
|
|
viewer->registerKeyboardCallback(&KeyboardCallback, nullptr);
|
|
|
|
// nuvem “fonte” (live/map) — aqui vamos desenhar a “live”
|
|
auto live = Core::get_cloud_live();
|
|
auto cloud_pcl = toPcl(live);
|
|
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_pcl);
|
|
viewer->addPointCloud<pcl::PointXYZRGB>(cloud_pcl, rgb, "cloud");
|
|
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
|
|
|
|
while (g_run.load() && !viewer->wasStopped()) {
|
|
// 1) pega a nuvem já acumulada (engine)
|
|
CloudRGB src = g_mode_map.load() ? Core::get_cloud_map() : Core::get_cloud_live();
|
|
|
|
// 2) converte para PCL
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_now(new pcl::PointCloud<pcl::PointXYZRGB>);
|
|
pc_now->points.reserve(src.pts.size());
|
|
for (const auto& q : src.pts) {
|
|
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; // vamos recolorir abaixo
|
|
pc_now->points.push_back(p);
|
|
}
|
|
pc_now->width = (uint32_t)pc_now->points.size();
|
|
pc_now->height = 1;
|
|
pc_now->is_dense = true;
|
|
|
|
// 3) colorir por distância (default) ou por altura (tecla D alterna)
|
|
bool by_dist = g_color_by_distance.load();
|
|
for (auto& p : pc_now->points) {
|
|
colorizePoint(p, by_dist); // mesma função já existente aí
|
|
}
|
|
|
|
// 4) atualiza no viewer
|
|
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb2(pc_now);
|
|
viewer->updatePointCloud<pcl::PointXYZRGB>(pc_now, rgb2, "cloud");
|
|
|
|
addHudTop(viewer);
|
|
addHudBottom(viewer);
|
|
|
|
if (g_show_axis.load()) {
|
|
drawDistances3D(viewer);
|
|
}
|
|
else {
|
|
// remove tudo se estiver OFF
|
|
for (auto id : {
|
|
"linha_frente","linha_tras","linha_esquerda","linha_direita","linha_cima","linha_baixo",
|
|
"texto_frente","texto_tras","texto_esquerda","texto_direita","texto_cima","texto_baixo"
|
|
})
|
|
viewer->removeShape(id);
|
|
}
|
|
|
|
if (g_draw_boxes.load()) {
|
|
uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
|
|
draw_bboxes_one_actor(viewer, now_ms, /*min_interval_ms=*/10, /*max_boxes=*/20);
|
|
}
|
|
else {
|
|
viewer->removeShape(g_actor_id);
|
|
g_last_n = 0;
|
|
}
|
|
|
|
viewer->spinOnce(15);
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(15));
|
|
}
|
|
}
|
|
|
|
} // anon
|
|
|
|
namespace ViewerDebug {
|
|
bool start(bool usar_slam_inicial) {
|
|
if (g_run.exchange(true)) return true;
|
|
g_use_slam = usar_slam_inicial;
|
|
try {
|
|
g_thr = std::thread(loop);
|
|
return true;
|
|
}
|
|
catch (...) {
|
|
g_run.store(false);
|
|
return false;
|
|
}
|
|
}
|
|
void stop() {
|
|
if (!g_run.exchange(false)) return;
|
|
if (g_thr.joinable()) g_thr.join();
|
|
}
|
|
|
|
void set_draw_boxes(bool on) { g_draw_boxes = on; }
|
|
void set_color_by_distance(bool on) { g_color_by_distance = on; }
|
|
void set_mode_map(bool on) { g_mode_map = on; }
|
|
void set_use_slam(bool on) { g_use_slam = on; }
|
|
bool get_mode_map() { return g_mode_map.load(); }
|
|
|
|
} // namespace ViewerDebug
|