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/livox_visual_debugger.cpp

1165 lines
40 KiB
C++
Raw Normal View History

iniciado integracao com o lidar
2025-10-29 13:22:39 +00:00
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <thread>
#include <chrono>
#include <mutex>
#include <atomic>
#include <iostream>
#include <iomanip> // <- pra formatar tabela bonitinha
#include <filesystem>
#include <cstring>
#include <deque>
#include <chrono>
#include <cmath> // sqrt etc.
#include <Eigen/Dense>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h> // downsample pra ajudar ICP
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include "livox_lidar_api.h"
#include "livox_lidar_def.h"
namespace fs = std::filesystem;
// =====================================================
// ESTRUTURAS / GLOBAIS
// =====================================================
struct TimedPoint {
pcl::PointXYZRGB p;
uint64_t t_ms;
};
static std::deque<TimedPoint> g_buffer_temporal;
static const uint64_t WINDOW_MS = 100; // janela vis<69>vel: <20>ltimos 300 ms
// nuvem atual pra render
static std::mutex g_cloud_mutex;
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
static std::atomic<bool> g_running(true);
// escolher modo de cor (true = dist<73>ncia radial, false = altura Z)
static bool ver_distancia = true;
static bool modo_map = false;
static bool usar_slam = false;
static bool desenhar_caixas = true;
static bool sensor_invertido = true; // true = montado de cabe<62>a pra baixo
// nuvem global acumulada (map<61>o)
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_global_map(new pcl::PointCloud<pcl::PointXYZRGB>);
// ---- IMU globals ----
// vamos guardar o <20>ltimo pacote de IMU recebido
static std::mutex g_imu_mutex;
// vamos manter valores "mais recentes"
static float g_imu_acc_x = 0.0f;
static float g_imu_acc_y = 0.0f;
static float g_imu_acc_z = 0.0f;
static float g_imu_gyro_x = 0.0f;
static float g_imu_gyro_y = 0.0f;
static float g_imu_gyro_z = 0.0f;
static uint64_t g_imu_last_ms = 0; // timestamp local que recebemos
// Pose estimada do LiDAR no mundo (por enquanto s<> rota<74><61>o).
static std::mutex g_pose_mutex;
static float g_roll = 0.0f; // rota<74><61>o em torno de X
static float g_pitch = 0.0f; // rota<74><61>o em torno de Y
static float g_yaw = 0.0f; // rota<74><61>o em torno de Z
static uint64_t g_last_imu_ms = 0;
// posi<73><69>o global estimada (em metros)
static float g_pos_x = 0.0f;
static float g_pos_y = 0.0f;
static float g_pos_z = 0.0f;
// velocidade global estimada (m/s)
static float g_vel_x = 0.0f;
static float g_vel_y = 0.0f;
static float g_vel_z = 0.0f;
// Pose global estimada via ICP+IMU
// OBS: agora quem manda em posi<73><69>o <20> ICP, e n<>o mais a integra<72><61>o da acelera<72><61>o.
// A gente N<>O vai mais usar g_pos_x/g_pos_y/g_pos_z que vinha da IMU que driftava infinito.
// Em vez disso, vamos manter uma matriz 4x4 acumulada que representa a pose do LiDAR no "mapa global".
static Eigen::Matrix4f g_pose_Twm = Eigen::Matrix4f::Identity();
// Twm = Transform from sensor(local frame "m") to world "w".
// (world <20> o mapa global onde estamos acumulando pontos)
// nuvem 'keyframe' anterior para ICP
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_prev_cloud_raw(new pcl::PointCloud<pcl::PointXYZRGB>);
// flag pra saber se j<> temos prev_cloud inicializada
static bool g_has_prev_frame = false;
// acumula pontos estabilizados (rotacionados pela IMU) at<61> formar um "keyframe"
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_keyframe_accum(new pcl::PointCloud<pcl::PointXYZRGB>);
// timestamp de quando come<6D>amos a acumular esse keyframe
static uint64_t g_keyframe_start_ms = 0;
static const uint64_t KEYFRAME_WINDOW_MS = 150; // ~150ms de varredura por keyframe
static const size_t KEYFRAME_MIN_POINTS = 2000; // se ainda n<>o bateu tempo mas j<> tem bastante ponto
static const size_t KEYFRAME_MAX_POINTS = 20000; // se passou disso tamb<6D>m fecha logo
static uint64_t g_last_detection_ms = 0;
static const uint64_t DETECTION_PERIOD_MS = 100; // roda cluster a cada ~300ms
// value em [0,1] -> cor estilo "jet"
static inline void ColorMapJet(float value, uint8_t& r, uint8_t& g, uint8_t& b) {
if (value < 0.0f) value = 0.0f;
if (value > 1.0f) value = 1.0f;
float fourValue = 4.0f * value;
float r_f = std::min(std::max(std::min(fourValue - 1.5f, -fourValue + 4.5f), 0.0f), 1.0f);
float g_f = std::min(std::max(std::min(fourValue - 0.5f, -fourValue + 3.5f), 0.0f), 1.0f);
float b_f = std::min(std::max(std::min(fourValue + 0.5f, -fourValue + 2.5f), 0.0f), 1.0f);
r = static_cast<uint8_t>(r_f * 255.0f);
g = static_cast<uint8_t>(g_f * 255.0f);
b = static_cast<uint8_t>(b_f * 255.0f);
}
static Eigen::Matrix3f R_from_rpy(float roll, float pitch, float yaw) {
float cr = std::cos(roll);
float sr = std::sin(roll);
float cp = std::cos(pitch);
float sp = std::sin(pitch);
float cy = std::cos(yaw);
float sy = std::sin(yaw);
Eigen::Matrix3f R;
R << cy * cp, cy* sp* sr - sy * cr, cy* sp* cr + sy * sr,
sy* cp, sy* sp* sr + cy * cr, sy* sp* cr - cy * sr,
-sp, cp* sr, cp* cr;
return R;
}
struct VoxelKey {
int ix, iy, iz;
bool operator==(const VoxelKey& o) const {
return ix == o.ix && iy == o.iy && iz == o.iz;
}
};
// hash pra usar VoxelKey como chave em unordered_map
struct VoxelKeyHash {
std::size_t operator()(const VoxelKey& k) const noexcept {
// hash combinando 3 ints
// isso n<>o precisa ser perfeito, s<> razo<7A>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);
}
};
pcl::PointCloud<pcl::PointXYZRGB>::Ptr 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<6E>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;
}
void DesenharBussolaDistancias(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
// --- limpa linhas antigas (mant<6E>m s<> as novas por frame)
const std::vector<std::string> nomes = {
"linha_frente", "linha_tras", "linha_esquerda", "linha_direita",
"linha_cima", "linha_baixo",
"texto_frente", "texto_tras", "texto_esquerda", "texto_direita",
"texto_cima", "texto_baixo"
};
for (auto& n : nomes) viewer->removeShape(n);
auto distancia_direcao = [&](auto seletor) {
std::vector<float> dists;
dists.reserve(cloud->points.size());
for (const auto& p : cloud->points) {
if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;
if (seletor(p)) {
float dist = std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
dists.push_back(dist);
}
}
if (dists.size() < 5) return 9999.f; // muito poucos pontos = inv<6E>lido
std::sort(dists.begin(), dists.end());
// m<>dia dos 10 menores (ou menos se tiver poucos)
int n = std::min<int>(10, dists.size());
float soma = 0.f;
for (int i = 0; i < n; ++i) soma += dists[i];
return soma / n;
};
float frente = distancia_direcao([](const auto& p) { return p.y > 0 && std::fabs(p.x) < 0.5; });
float tras = distancia_direcao([](const auto& p) { return p.y < 0 && std::fabs(p.x) < 0.5; });
float direita = distancia_direcao([](const auto& p) { return p.x > 0 && std::fabs(p.y) < 0.5; });
float esquerda = distancia_direcao([](const auto& p) { return p.x < 0 && std::fabs(p.y) < 0.5; });
float cima = distancia_direcao([](const auto& p) { return p.z > 0; });
float baixo = distancia_direcao([](const auto& p) { return p.z < 0; });
// --- fun<75><6E>o utilit<69>ria
auto desenhar = [&](const std::string& nome, const Eigen::Vector3f& dir, float dist, const Eigen::Vector3f& cor, const std::string& label) {
if (dist >= 9990.f) dist = 0.0f; // sem obst<73>culo
Eigen::Vector3f origem(0, 0, 0);
Eigen::Vector3f destino = dir.normalized() * dist;
viewer->addLine(
pcl::PointXYZ(origem.x(), origem.y(), origem.z()),
pcl::PointXYZ(destino.x(), destino.y(), destino.z()),
cor.x(), cor.y(), cor.z(),
nome);
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, nome);
// adiciona o texto no final da linha
char txt[64];
if (dist > 0.001f)
std::snprintf(txt, sizeof(txt), "%s: %.0f cm", label.c_str(), dist * 100.f);
else
std::snprintf(txt, sizeof(txt), "%s: --", label.c_str());
pcl::PointXYZ pos(destino.x(), destino.y(), destino.z());
viewer->addText3D(txt, pos, 0.05, cor.x(), cor.y(), cor.z(), "texto_" + label);
};
// --- desenha as seis dire<72><65>es
desenhar("linha_frente", { 1,0,0 }, frente, { 1,0,0 }, "frente");
desenhar("linha_tras", { -1,0,0 }, tras, { 0.8,0,0 }, "tras");
desenhar("linha_direita", { 0,1,0 }, direita, { 0,1,0 }, "direita");
desenhar("linha_esquerda", { 0,-1,0 }, esquerda, { 0,0.8,0 }, "esquerda");
desenhar("linha_cima", { 0,0,1 }, cima, { 0,0,1 }, "cima");
desenhar("linha_baixo", { 0,0,-1 }, baixo, { 0,0,0.8 }, "baixo");
}
void DetectarClustersEAdicionarCaixas(pcl::visualization::PCLVisualizer::Ptr& viewer, pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in)
{
if (!desenhar_caixas) {
// Se n<>o vamos desenhar caixas, limpa overlay e sai
for (int i = 0; i < 100; ++i) {
std::string box_id = "bbox_" + std::to_string(i);
viewer->removeShape(box_id);
}
return;
}
// Limpa overlay anterior (caixas antigas)
for (int i = 0; i < 100; ++i) {
std::string box_id = "bbox_" + std::to_string(i);
viewer->removeShape(box_id);
}
if (!cloud_in || cloud_in->empty()) {
return;
}
// ============================================================
// 1. ROI (Regi<67>o de Interesse)
// - S<> objetos at<61> ~5m de raio
// - Ignorar muito baixo (abaixo do ch<63>o) / muito alto
// IMPORTANTE: ajustar limites de z depois que voc<6F> montar o sensor.
// ============================================================
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;
// at<61> 5m -> 5 * 5 = 25
if (dist2 > 9.0f) continue;
// corta lixo MUITO baixo ou MUITO alto
// ajuste isso conforme a montagem do LiDAR:
// Exemplo:
// se o LiDAR est<73> a ~0.5m do ch<63>o e Z cresce pra cima,
// voc<6F> pode querer ignorar z < -0.2 e z > 2m
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;
// ============================================================
// 2. Downsample (voxel grid)
// - A gente n<>o quer cada pontinho, quer blocos de ~7cm
// - Isso mata ru<72>do fino tipo perna de trip<69> fina / cabo fino
// ============================================================
//pcl::VoxelGrid<pcl::PointXYZRGB> vg;
//vg.setInputCloud(roi);
//vg.setLeafSize(0.05f, 0.05f, 0.05f); // ~7cm
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr coarse(new pcl::PointCloud<pcl::PointXYZRGB>);
//vg.filter(*coarse);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr coarse = DownsampleVoxelLike(roi, 0.05f);
if (coarse->points.size() < 20) {
return;
}
DesenharBussolaDistancias(viewer, coarse);
// ============================================================
// 3. Remover "ch<63>o" r<>pido via limiar de altura
//
// Aqui N<>O estamos fazendo RANSAC (caro).
// Supondo que o sensor fica a uma altura razoavelmente fixa e o eixo Z
// est<73> alinhado com "vertical".
//
// Ideia: tudo muito baixo (tipo at<61> 10cm do ch<63>o local) <20> ch<63>o.
// E o que est<73> acima disso (0.10m, 0.15m...) <20> obst<73>culo.
//
// IMPORTANTE: voc<6F> precisa ajustar esse limite quando souber
// a altura do sensor no rob<6F>. Aqui vou deixar 0.10 m.
// ============================================================
const float ALTURA_SENSOR = 0.70f; // metros
const float TOLERANCIA = 0.00f; // metros
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtrada(new pcl::PointCloud<pcl::PointXYZRGB>);
filtrada->reserve(coarse->points.size());
for (const auto& p : coarse->points) {
if (!sensor_invertido) {
// sensor normal: remove ch<63>o
float limite_chao = -ALTURA_SENSOR + TOLERANCIA;
if (p.z < limite_chao)
continue;
}
else {
// sensor invertido: remove teto
float limite_teto = ALTURA_SENSOR - TOLERANCIA;
if (p.z > limite_teto)
continue;
}
filtrada->points.push_back(p);
}
filtrada->width = static_cast<uint32_t>(filtrada->points.size());
filtrada->height = 1;
filtrada->is_dense = false;
// ============================================================
// 4. Clusteriza<7A><61>o Euclidiana
// - Agora rodamos cluster s<> em "acima do ch<63>o"
// - Com muito menos pontos, bem mais leve
// ============================================================
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(filtrada);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance(0.20f); // 20cm de vizinhan<61>a pra juntar pontos
ec.setMinClusterSize(50); // ignora micro-coisas
ec.setMaxClusterSize(100000); // bem grande
ec.setSearchMethod(tree);
ec.setInputCloud(filtrada);
ec.extract(cluster_indices);
// ============================================================
// 5. Criar bounding boxes por cluster
// - S<> desenha clusters grandes o suficiente
// - Voc<6F> falou: queremos ignorar coisa < ~30cm.
// Vamos aplicar esse filtro aqui.
// ============================================================
const float MIN_SIZE_FOR_BOX = 0.20f; // 30cm m<>nimo num dos eixos
int cluster_id = 0;
for (const auto& indices : cluster_indices) {
if (indices.indices.size() < 30)
continue; // redund<6E>ncia com MinClusterSize
float min_x = std::numeric_limits<float>::max();
float max_x = -std::numeric_limits<float>::max();
float min_y = std::numeric_limits<float>::max();
float max_y = -std::numeric_limits<float>::max();
float min_z = std::numeric_limits<float>::max();
float max_z = -std::numeric_limits<float>::max();
for (int idx : indices.indices) {
const auto& p = (*filtrada)[idx];
if (p.x < min_x) min_x = p.x;
if (p.x > max_x) max_x = p.x;
if (p.y < min_y) min_y = p.y;
if (p.y > max_y) max_y = p.y;
if (p.z < min_z) min_z = p.z;
if (p.z > max_z) max_z = p.z;
}
float size_x = max_x - min_x;
float size_y = max_y - min_y;
float size_z = max_z - min_z;
// filtra clusters muito pequenos
if (size_x < MIN_SIZE_FOR_BOX &&
size_y < MIN_SIZE_FOR_BOX &&
size_z < MIN_SIZE_FOR_BOX) {
continue;
}
float cx = (min_x + max_x) * 0.5f;
float cy = (min_y + max_y) * 0.5f;
float cz = (min_z + max_z) * 0.5f;
std::string box_id = "bbox_" + std::to_string(cluster_id);
viewer->addCube(
cx - size_x * 0.5f, cx + size_x * 0.5f,
cy - size_y * 0.5f, cy + size_y * 0.5f,
cz - size_z * 0.5f, cz + size_z * 0.5f,
1.0, 0.0, 0.0, // vermelho (voc<6F> pode mudar depois)
box_id
);
viewer->setShapeRenderingProperties(
pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME,
box_id);
viewer->setShapeRenderingProperties(
pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,
2.0,
box_id);
cluster_id++;
if (cluster_id >= 100) break; // seguran<61>a pra n<>o floodar
}
}
// =====================================================
// CALLBACK DE NUVEM DE PONTOS
// =====================================================
void LidarPointCloudCallback(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* packet, void* /*client_data*/) {
using namespace std::chrono;
uint64_t now_ms = duration_cast<milliseconds>(
steady_clock::now().time_since_epoch()
).count();
std::lock_guard<std::mutex> lock(g_cloud_mutex);
// par<61>metros do nosso color map
const float z_min = -0.2f;
const float z_max = 1.5f;
const float z_range = (z_max - z_min);
// vamos guardar os pontos "novos" desse pacote numa lista tempor<6F>ria,
// pra poder reutilizar tanto no modo Live quanto no modo Map
std::vector<TimedPoint> new_points;
new_points.reserve(packet->dot_num);
auto make_point = [&](float x, float y, float z) {
TimedPoint tp;
tp.p.x = x;
tp.p.y = y;
tp.p.z = z;
uint8_t rr, gg, bb;
if (ver_distancia) {
float dist = std::sqrt(x * x + y * y + z * z); // metros
float norm = dist / 5.0f; // normaliza em ~5m
ColorMapJet(norm, rr, gg, bb);
}
else {
float norm = (z - z_min) / (z_range > 0.0001f ? z_range : 1.0f);
ColorMapJet(norm, rr, gg, bb);
}
tp.p.r = rr;
tp.p.g = gg;
tp.p.b = bb;
tp.t_ms = now_ms;
new_points.push_back(tp);
};
// 1. pegar pontos do pacote e montar new_points
if (packet->data_type == kLivoxLidarCartesianCoordinateHighData) {
auto* pts = reinterpret_cast<LivoxLidarCartesianHighRawPoint*>(packet->data);
for (uint16_t i = 0; i < packet->dot_num; ++i) {
float x = pts[i].x / 1000.0f;
float y = pts[i].y / 1000.0f;
float z = pts[i].z / 1000.0f;
make_point(x, y, z);
}
}
else if (packet->data_type == kLivoxLidarCartesianCoordinateLowData) {
auto* pts = reinterpret_cast<LivoxLidarCartesianLowRawPoint*>(packet->data);
for (uint16_t i = 0; i < packet->dot_num; ++i) {
float x = pts[i].x / 100.0f;
float y = pts[i].y / 100.0f;
float z = pts[i].z / 100.0f;
make_point(x, y, z);
}
}
else {
// outros formatos ainda n<>o tratados
}
// 2. dependendo do modo, tratamos diferente
if (!modo_map) {
// ------ MODO LIVE ------
// empurrar os pontos novos pro buffer temporal
for (const auto& tp : new_points) {
g_buffer_temporal.push_back(tp);
}
// remover pontos velhos > WINDOW_MS
const uint64_t cutoff = now_ms - WINDOW_MS;
while (!g_buffer_temporal.empty() &&
g_buffer_temporal.front().t_ms < cutoff) {
g_buffer_temporal.pop_front();
}
// reconstruir g_cloud a partir da janela temporal
g_cloud->points.clear();
g_cloud->points.reserve(g_buffer_temporal.size());
for (const auto& tp : g_buffer_temporal) {
g_cloud->points.push_back(tp.p);
}
g_cloud->width = static_cast<uint32_t>(g_cloud->points.size());
g_cloud->height = 1;
g_cloud->is_dense = true;
}
else {
// ------ MODO MAP COM ICP ROBUSTO ------
using namespace std::chrono;
// se o SLAM estiver desativado, apenas acumula os pontos diretamente
if (!usar_slam) {
Eigen::Matrix3f R_imu = R_from_rpy(g_roll, g_pitch, g_yaw);
for (const auto& tp : new_points) {
Eigen::Vector3f v_local(tp.p.x, tp.p.y, tp.p.z);
Eigen::Vector3f v_rot = R_imu * v_local;
pcl::PointXYZRGB p_out;
p_out.x = v_rot.x();
p_out.y = v_rot.y();
p_out.z = v_rot.z();
p_out.r = tp.p.r;
p_out.g = tp.p.g;
p_out.b = tp.p.b;
g_global_map->points.push_back(p_out);
}
g_global_map->width = (uint32_t)g_global_map->points.size();
g_global_map->height = 1;
g_global_map->is_dense = true;
*g_cloud = *g_global_map;
return;
}
// 1. pega atitude atual
float roll, pitch, yaw;
{
std::lock_guard<std::mutex> lock_pose(g_pose_mutex);
roll = g_roll;
pitch = g_pitch;
yaw = g_yaw;
}
Eigen::Matrix3f R_imu = R_from_rpy(roll, pitch, yaw);
// 2. converte os pontos desse pacote para o frame estabilizado (IMU)
// e acumula em g_keyframe_accum
uint64_t now_ms = duration_cast<milliseconds>(
steady_clock::now().time_since_epoch()
).count();
if (g_keyframe_start_ms == 0) {
g_keyframe_start_ms = now_ms;
}
for (const auto& tp : new_points) {
Eigen::Vector3f v_local(tp.p.x, tp.p.y, tp.p.z);
Eigen::Vector3f v_rot = R_imu * v_local;
pcl::PointXYZRGB p_out;
p_out.x = v_rot.x();
p_out.y = v_rot.y();
p_out.z = v_rot.z();
p_out.r = tp.p.r;
p_out.g = tp.p.g;
p_out.b = tp.p.b;
g_keyframe_accum->points.push_back(p_out);
}
// 3. decidiu fechar keyframe?
bool close_keyframe = false;
uint64_t elapsed = now_ms - g_keyframe_start_ms;
size_t npts = g_keyframe_accum->points.size();
if (elapsed >= KEYFRAME_WINDOW_MS && npts >= KEYFRAME_MIN_POINTS) {
close_keyframe = true;
}
if (npts >= KEYFRAME_MAX_POINTS) {
close_keyframe = true;
}
if (!close_keyframe) {
// ainda n<>o formou um keyframe completo
// atualiza visualiza<7A><61>o s<> pra voc<6F> continuar vendo algo (opcional)
// aqui a gente s<> mostra o mapa atual sem adicionar nada novo
*g_cloud = *g_global_map;
return;
}
// ------ FECHANDO KEYFRAME ------
// monta um ptr pra esse keyframe atual completo
pcl::PointCloud<pcl::PointXYZRGB>::Ptr curr_keyframe(
new pcl::PointCloud<pcl::PointXYZRGB>);
*curr_keyframe = *g_keyframe_accum;
curr_keyframe->width = (uint32_t)curr_keyframe->points.size();
curr_keyframe->height = 1;
curr_keyframe->is_dense = true;
// limpa o acumulador pro pr<70>ximo ciclo
g_keyframe_accum->points.clear();
g_keyframe_accum->width = 0;
g_keyframe_accum->height = 1;
g_keyframe_accum->is_dense = true;
g_keyframe_start_ms = now_ms;
// se esse <20> o primeiro keyframe, s<> inicializa estado
if (!g_has_prev_frame) {
*g_prev_cloud_raw = *curr_keyframe;
g_has_prev_frame = true;
// injeta esse primeiro keyframe no mapa global com a pose inicial
for (const auto& p_in : curr_keyframe->points) {
Eigen::Vector4f v_local(p_in.x, p_in.y, p_in.z, 1.0f);
Eigen::Vector4f v_world = g_pose_Twm * v_local;
pcl::PointXYZRGB p_out;
p_out.x = v_world.x();
p_out.y = v_world.y();
p_out.z = v_world.z();
p_out.r = p_in.r;
p_out.g = p_in.g;
p_out.b = p_in.b;
g_global_map->points.push_back(p_out);
}
g_global_map->width = (uint32_t)g_global_map->points.size();
g_global_map->height = 1;
g_global_map->is_dense = true;
*g_cloud = *g_global_map;
return;
}
// ------ j<> temos prev frame: ICP entre keyframes ------
// downsample prev e curr pra robustez
pcl::VoxelGrid<pcl::PointXYZRGB> voxel;
voxel.setLeafSize(0.03f, 0.03f, 0.03f);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr curr_ds(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr prev_ds(new pcl::PointCloud<pcl::PointXYZRGB>);
voxel.setInputCloud(curr_keyframe);
voxel.filter(*curr_ds);
voxel.setInputCloud(g_prev_cloud_raw);
voxel.filter(*prev_ds);
// roda ICP
pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setInputSource(curr_ds); // atual
icp.setInputTarget(prev_ds); // anterior
icp.setMaximumIterations(40);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setMaxCorrespondenceDistance(0.30f); // ~30 cm, porque agora <20> keyframe vs keyframe
pcl::PointCloud<pcl::PointXYZRGB> icp_aligned;
icp.align(icp_aligned);
bool accept_icp = false;
Eigen::Matrix4f T_prev_to_curr = Eigen::Matrix4f::Identity();
if (icp.hasConverged()) {
double score = icp.getFitnessScore();
// std::cout << "[ICP KF] fitness=" << score << " points=" << curr_ds->points.size() << "\n";
if (score < 0.15) {
// final transformation: curr -> prev
Eigen::Matrix4f T_curr_to_prev = icp.getFinalTransformation();
T_prev_to_curr = T_curr_to_prev.inverse();
accept_icp = true;
}
}
if (accept_icp) {
g_pose_Twm = g_pose_Twm * T_prev_to_curr;
}
else {
// se n<>o aceitamos o ICP, n<>o mexe a pose (evita teleporte)
// isso tamb<6D>m evita mapear coisa aleat<61>ria
}
// agora insere o keyframe atual no mapa global usando a pose global atual
for (const auto& p_in : curr_keyframe->points) {
Eigen::Vector4f v_local(p_in.x, p_in.y, p_in.z, 1.0f);
Eigen::Vector4f v_world = g_pose_Twm * v_local;
pcl::PointXYZRGB p_out;
p_out.x = v_world.x();
p_out.y = v_world.y();
p_out.z = v_world.z();
p_out.r = p_in.r;
p_out.g = p_in.g;
p_out.b = p_in.b;
g_global_map->points.push_back(p_out);
}
g_global_map->width = (uint32_t)g_global_map->points.size();
g_global_map->height = 1;
g_global_map->is_dense = true;
// prepara pro pr<70>ximo ICP
*g_prev_cloud_raw = *curr_keyframe;
// atualiza o viewer
*g_cloud = *g_global_map;
}
}
// =====================================================
// CALLBACK DE IMU
// =====================================================
void LidarImuDataCallback(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* packet, void* /*client_data*/) {
using namespace std::chrono;
uint64_t now_ms = duration_cast<milliseconds>(
steady_clock::now().time_since_epoch()
).count();
if (!packet || packet->dot_num == 0) {
return;
}
// pega a <20>ltima amostra do pacote de IMU
auto* imu_points = reinterpret_cast<LivoxLidarImuRawPoint*>(packet->data);
uint16_t n = packet->dot_num;
LivoxLidarImuRawPoint imu = imu_points[n - 1];
// -------------------------
// 1. guardar IMU pro HUD
// -------------------------
{
std::lock_guard<std::mutex> lock(g_imu_mutex);
g_imu_gyro_x = imu.gyro_x;
g_imu_gyro_y = imu.gyro_y;
g_imu_gyro_z = imu.gyro_z;
g_imu_acc_x = imu.acc_x;
g_imu_acc_y = imu.acc_y;
g_imu_acc_z = imu.acc_z;
g_imu_last_ms = now_ms;
}
// -------------------------------------------------
// 2. integrar atitude (roll/pitch/yaw)
// + filtro complementar com gravidade
// -------------------------------------------------
// assumindo gyro_* em rad/s
float gx = imu.gyro_x;
float gy = imu.gyro_y;
float gz = imu.gyro_z;
// assumindo acc_* em "g" (1.0 ~= 9.81 m/s^2),
// vamos converter pra m/s^2
// se j<> vier em m/s^2 no seu SDK, coloque scale_acc = 1.0f
const float g_m_s2 = 9.81f;
float ax_raw = imu.acc_x * g_m_s2;
float ay_raw = imu.acc_y * g_m_s2;
float az_raw = imu.acc_z * g_m_s2;
std::lock_guard<std::mutex> pose_lock(g_pose_mutex);
if (g_last_imu_ms == 0) {
// primeira leitura: s<> inicializa timestamp
g_last_imu_ms = now_ms;
return;
}
float dt = (now_ms - g_last_imu_ms) / 1000.0f; // segundos
g_last_imu_ms = now_ms;
// integra<72><61>o dos gyros (atitude "livre")
float roll_gyro = g_roll + gx * dt;
float pitch_gyro = g_pitch + gy * dt;
float yaw_gyro = g_yaw + gz * dt;
// estima roll/pitch absoluto a partir da gravidade
// assumindo eixos IMU:
// X: frente
// Y: esquerda
// Z: cima
// pitch = inclina<6E><61>o pra frente/tr<74>s
// roll = inclina<6E><61>o pros lados
float denom = std::sqrt(ay_raw * ay_raw + az_raw * az_raw);
if (denom < 1e-6f) denom = 1e-6f;
float roll_acc = std::atan2(ay_raw, az_raw);
float pitch_acc = std::atan2(-ax_raw, denom);
// filtro complementar
const float alpha = 0.98f;
float fused_roll = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
float fused_pitch = alpha * pitch_gyro + (1.0f - alpha) * pitch_acc;
float fused_yaw = yaw_gyro; // yaw ainda s<> do gyro
// atualiza atitude global
g_roll = fused_roll;
g_pitch = fused_pitch;
g_yaw = fused_yaw;
// -------------------------------------------------
// 3. transformar acelera<72><61>o pro frame global
// -------------------------------------------------
// A IMU mede acelera<72><61>o no frame do sensor, incluindo gravidade.
// A gente quer acelera<72><61>o linear do sensor no frame global.
//
// Passo 1: construir rota<74><61>o corpo->mundo (mesma R que usamos no mapper)
// R = Rz(yaw)*Ry(pitch)*Rx(roll)
/*float cr = std::cos(g_roll);
float sr = std::sin(g_roll);
float cp = std::cos(g_pitch);
float sp = std::sin(g_pitch);
float cy = std::cos(g_yaw);
float sy = std::sin(g_yaw);
float R00 = cy * cp;
float R01 = cy * sp * sr - sy * cr;
float R02 = cy * sp * cr + sy * sr;
float R10 = sy * cp;
float R11 = sy * sp * sr + cy * cr;
float R12 = sy * sp * cr - cy * sr;
float R20 = -sp;
float R21 = cp * sr;
float R22 = cp * cr;
// acelera<72><61>o medida no frame do sensor (m/s^2)
// (ax_raw, ay_raw, az_raw)
// projetar para mundo:
float acc_world_x = R00 * ax_raw + R01 * ay_raw + R02 * az_raw;
float acc_world_y = R10 * ax_raw + R11 * ay_raw + R12 * az_raw;
float acc_world_z = R20 * ax_raw + R21 * ay_raw + R22 * az_raw;
// Passo 2: remover gravidade.
// No mundo, gravidade <20> aproximadamente (0,0,-9.81)
acc_world_z -= (-g_m_s2 * -1.0f);
// cuidado: vamos deixar isso claro:
// Se o eixo Z_global aponta "pra cima", gravidade global <20> (0,0,-9.81).
// Ent<6E>o basta:
acc_world_z -= (-g_m_s2); // ou seja: acc_world_z = acc_world_z + 9.81
// vamos reescrever limpo pra n<>o confundir:
acc_world_z = acc_world_z + g_m_s2; // substitui a linha anterior inteira
// (Depois do ajuste acima, acc_world_* tenta ser acelera<72><61>o linear sem gravidade)
// -------------------------------------------------
// 4. integrar acelera<72><61>o -> velocidade -> posi<73><69>o
// -------------------------------------------------
g_vel_x += acc_world_x * dt;
g_vel_y += acc_world_y * dt;
g_vel_z += acc_world_z * dt;
g_pos_x += g_vel_x * dt;
g_pos_y += g_vel_y * dt;
g_pos_z += g_vel_z * dt;*/
}
// =====================================================
// CALLBACK DE INFO / ESTADO DO LIDAR
// =====================================================
void LidarInfoChangeCallback(const uint32_t handle, const LivoxLidarInfo* info, void* /*client_data*/) {
std::cout << "[INFO] LidarInfoChangeCb CHAMADO!\n";
if (!info) {
std::cout << "[INFO] info == NULL\n";
return;
}
std::cout << "[INFO] handle=" << handle
<< " dev_type=" << (int)info->dev_type
<< " sn=" << info->sn
<< " ip=" << info->lidar_ip
<< std::endl;
// coloca em modo normal
livox_status wm = SetLivoxLidarWorkMode(
handle,
kLivoxLidarNormal,
nullptr,
nullptr);
std::cout << "[INFO] SetLivoxLidarWorkMode ret=" << wm << std::endl;
// habilita envio de pontos
livox_status en = EnableLivoxLidarPointSend(
handle,
nullptr,
nullptr);
std::cout << "[INFO] EnableLivoxLidarPointSend ret=" << en << std::endl;
// habilita envio de imu (dependendo do SDK isso pode ser autom<6F>tico,
// mas alguns modelos precisam de algo tipo EnableLivoxLidarImuDataSend())
// Se existir essa fun<75><6E>o no seu SDK, chama aqui parecido:
// livox_status en_imu = EnableLivoxLidarImuData(handle, nullptr, nullptr);
// std::cout << "[INFO] EnableLivoxLidarImuData ret=" << en_imu << std::endl;
}
// =====================================================
// CALLBACK DE COMANDOS DO TECLADO
// =====================================================
void KeyboardCallback(const pcl::visualization::KeyboardEvent& event, void*) {
if (event.keyDown()) {
if (event.getKeySym() == "m" || event.getKeySym() == "M") {
modo_map = !modo_map;
std::cout << "\n[KEY] Modo alterado para: "
<< (modo_map ? "MAP" : "LIVE") << std::endl;
}
if (event.getKeySym() == "s" || event.getKeySym() == "S") {
usar_slam = !usar_slam;
std::cout << "\n[KEY] SLAM: "
<< (usar_slam ? "ATIVADO (ICP ON)" : "DESATIVADO (bruto)") << std::endl;
}
if (event.getKeySym() == "d" || event.getKeySym() == "D") {
ver_distancia = !ver_distancia;
std::cout << "\n[KEY] Modo alterado para: "
<< (ver_distancia ? "DISTANCIA" : "ALTURA") << std::endl;
}
if (event.getKeySym() == "b" || event.getKeySym() == "B") {
desenhar_caixas = !desenhar_caixas;
std::cout << "\n[KEY] Caixas de obstaculo: "
<< (desenhar_caixas ? "ON" : "OFF") << std::endl;
}
if (event.getKeySym() == "i" || event.getKeySym() == "I") {
sensor_invertido = !sensor_invertido;
}
}
}
// =====================================================
// MAIN
// =====================================================
int main(int argc, char** argv) {
// monta caminho do config.json na mesma pasta do exe
fs::path exe_dir = fs::current_path();
fs::path cfg = exe_dir / "config.json";
std::cout << "[MAIN] usando config: " << cfg.string() << std::endl;
// init SDK
if (!LivoxLidarSdkInit(cfg.string().c_str())) {
std::cout << "[ERR] LivoxLidarSdkInit falhou\n";
LivoxLidarSdkUninit();
return -1;
}
std::cout << "[MAIN] LivoxLidarSdkInit OK\n";
// registra callbacks principais
SetLivoxLidarPointCloudCallBack(LidarPointCloudCallback, nullptr);
std::cout << "[MAIN] SetLivoxLidarPointCloudCallBack OK\n";
SetLivoxLidarInfoChangeCallback(LidarInfoChangeCallback, nullptr);
std::cout << "[MAIN] SetLivoxLidarInfoChangeCallback OK\n";
// >>> novo: callback da IMU <<<
// Essa fun<75><6E>o pode ter nome ligeiramente diferente dependendo da vers<72>o do SDK:
// Ex.: SetLivoxLidarImuDataCallback, SetLivoxLidarImuCallBack, etc.
// Use o nome que existir no seu livox_lidar_api.h.
SetLivoxLidarImuDataCallback(LidarImuDataCallback, nullptr);
std::cout << "[MAIN] SetLivoxLidarImuDataCallback OK\n";
// inicia sdk
if (!LivoxLidarSdkStart()) {
std::cout << "[ERR] LivoxLidarSdkStart falhou\n";
LivoxLidarSdkUninit();
return -1;
}
std::cout << "[MAIN] LivoxLidarSdkStart OK\n";
// cria viewer PCL
pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer("Mid-360 Live"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
viewer->registerKeyboardCallback(KeyboardCallback);
{
std::lock_guard<std::mutex> lock(g_cloud_mutex);
g_cloud->clear();
g_cloud->width = 1;
g_cloud->height = 1;
g_cloud->is_dense = true;
}
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(g_cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(g_cloud, rgb, "livox_cloud");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
2,
"livox_cloud");
std::cout << "[MAIN] Viewer + SDK rodando. Fecha a janela pra sair.\n";
// loop principal
while (!viewer->wasStopped() && g_running.load()) {
// snapshot nuvem atual que vamos desenhar/usar pra percep<65><70>o
pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_copy(new pcl::PointCloud<pcl::PointXYZRGB>);
{
std::lock_guard<std::mutex> lock(g_cloud_mutex);
*local_copy = *g_cloud;
}
// atualiza visualiza<7A><61>o da nuvem
{
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb2(local_copy);
viewer->updatePointCloud<pcl::PointXYZRGB>(local_copy, rgb2, "livox_cloud");
}
// HUD texto
viewer->removeShape("hud_text");
{
std::stringstream ss;
ss << (modo_map ? "MODE: MAP " : "MODE: LIVE ")
<< (ver_distancia ? "DISTANCIA " : "ALTURA ")
<< "| SLAM: " << (usar_slam ? "ON" : "OFF")
<< " | BOX: " << (desenhar_caixas ? "ON" : "OFF");
viewer->addText(ss.str(), 10, 10, 14, 1.0, 1.0, 1.0, "hud_text");
}
// detec<65><63>o de obst<73>culos a cada DETECTION_PERIOD_MS
{
using namespace std::chrono;
uint64_t now_ms = duration_cast<milliseconds>(
steady_clock::now().time_since_epoch()
).count();
if (now_ms - g_last_detection_ms >= DETECTION_PERIOD_MS) {
g_last_detection_ms = now_ms;
DetectarClustersEAdicionarCaixas(viewer, local_copy);
}
}
viewer->spinOnce(15);
std::this_thread::sleep_for(std::chrono::milliseconds(15));
// 7. Imprime IMU no console, etc, como voc<6F> j<> fazia...
/*{
std::lock_guard<std::mutex> lock(g_imu_mutex);
float acc_norm = std::sqrt(
g_imu_acc_x * g_imu_acc_x +
g_imu_acc_y * g_imu_acc_y +
g_imu_acc_z * g_imu_acc_z
);
std::cout << "\r"
<< std::fixed << std::setprecision(3)
<< "[IMU] gyro: "
<< " gx=" << std::setw(7) << g_imu_gyro_x
<< " gy=" << std::setw(7) << g_imu_gyro_y
<< " gz=" << std::setw(7) << g_imu_gyro_z
<< " | acc(g?): "
<< " ax=" << std::setw(7) << g_imu_acc_x
<< " ay=" << std::setw(7) << g_imu_acc_y
<< " az=" << std::setw(7) << g_imu_acc_z
<< " | |acc|=" << std::setw(6) << acc_norm
<< " | t_ms=" << g_imu_last_ms
<< " "
<< std::flush;
}*/
}
g_running.store(false);
LivoxLidarSdkUninit();
std::cout << "\n[MAIN] Encerrado.\n";
return 0;
}