1126 lines
46 KiB
C#
1126 lines
46 KiB
C#
using AgroBase.Models;
|
|
using Microsoft.Kinect;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Drawing.Imaging;
|
|
using System.Linq;
|
|
using System.Runtime.InteropServices;
|
|
using System.Threading.Tasks;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
using Newtonsoft.Json;
|
|
using System.IO;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class KinectService
|
|
{
|
|
public static bool Iniciado
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
if (kinectSensor != null && kinectSensor.Status == KinectStatus.Connected)
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
public static KinectModel Leitura { get; set; } = new KinectModel() { Obstaculos = new List<Obstaculo>() };
|
|
|
|
private static KinectSensor kinectSensor;
|
|
private static double dt = 0.5;
|
|
private static double[] velocidade = new double[3];
|
|
private static double[] aceleracaoAnterior = new double[3];
|
|
public static DepthImagePixel[] depthPixels;
|
|
private static byte[] colorPixels;
|
|
public static Bitmap bitmapRGB;
|
|
public static Bitmap bitmapHeat;
|
|
public static Bitmap bitmapMap;
|
|
private static AsyncTaskTimerModel tmrLeitura;
|
|
private static DataGridView gridDados;
|
|
public static PictureBox picRGB;
|
|
public static PictureBox picProfundidade;
|
|
public static TrackBar trackBarAngle;
|
|
public static int[,] Clusters;
|
|
public static List<ClusterModel> ClustersData = new List<ClusterModel>();
|
|
|
|
// Variáveis globais para subdivisões
|
|
//private static List<Subdivisao> subdivisoesInferior;
|
|
//private static List<Subdivisao> subdivisoesSuperior;
|
|
|
|
public static readonly object _lockBitmapRGB = new object();
|
|
public static readonly object _lockBitmapHeat = new object();
|
|
public static readonly object _lockBitmapMap = new object();
|
|
private static readonly object _lockDepthPixels = new object();
|
|
|
|
public static KinectParametrosModel Parametros = new KinectParametrosModel();
|
|
public static bool MostrarGrade { get; set; } = false;
|
|
public static bool MostrarTexto { get; set; } = false;
|
|
|
|
public static void IniciarRotinas()
|
|
{
|
|
PararRotinas();
|
|
|
|
tmrLeitura = new AsyncTaskTimerModel("tmrLeitura", tmrLeitura_Tick, 200);
|
|
tmrLeitura.Start();
|
|
}
|
|
|
|
public static void PararRotinas()
|
|
{
|
|
tmrLeitura?.Dispose();
|
|
}
|
|
|
|
public static bool InicializarKinect(DataGridView dg = null, PictureBox pcRgb = null, PictureBox pcPf = null, TrackBar tkAng = null)
|
|
{
|
|
gridDados = dg;
|
|
picRGB = pcRgb;
|
|
picProfundidade = pcPf;
|
|
AtualizarTrackBar(tkAng);
|
|
|
|
// Inicialize o Kinect Sensor
|
|
kinectSensor = KinectSensor.KinectSensors.FirstOrDefault(x => x.Status == KinectStatus.Connected);
|
|
|
|
if (kinectSensor != null)
|
|
{
|
|
Parametros = CarregarParametros();
|
|
|
|
AtualizarAngulo(Parametros.Angulo);
|
|
|
|
// Habilitar a câmera RGB
|
|
kinectSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
|
|
kinectSensor.ColorFrameReady -= KinectSensor_ColorFrameReady;
|
|
kinectSensor.ColorFrameReady += KinectSensor_ColorFrameReady;
|
|
|
|
// Habilitar a câmera de profundidade
|
|
kinectSensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
|
|
kinectSensor.DepthFrameReady -= KinectSensor_DepthFrameReady;
|
|
kinectSensor.DepthFrameReady += KinectSensor_DepthFrameReady;
|
|
|
|
// Iniciar o sensor
|
|
if (!kinectSensor.IsRunning)
|
|
{
|
|
kinectSensor.Start();
|
|
}
|
|
|
|
colorPixels = new byte[kinectSensor.ColorStream.FramePixelDataLength];
|
|
bitmapRGB = new Bitmap(kinectSensor.ColorStream.FrameWidth, kinectSensor.ColorStream.FrameHeight, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
|
|
|
|
if (gridDados != null)
|
|
{
|
|
gridDados.Columns.Clear();
|
|
gridDados.Rows.Clear();
|
|
gridDados.Columns.Add("clDado", "Dado");
|
|
gridDados.Columns.Add("clValor", "Valor");
|
|
gridDados.Rows.Add(new string[] { "Leitura", "" });
|
|
gridDados.Rows.Add(new string[] { "Elevação", "0" });
|
|
gridDados.Rows.Add(new string[] { "Acelerômetro X", "0" });
|
|
gridDados.Rows.Add(new string[] { "Acelerômetro Y", "0" });
|
|
gridDados.Rows.Add(new string[] { "Acelerômetro Z", "0" });
|
|
gridDados.Rows.Add(new string[] { "Inclinação Frontal", "0" });
|
|
gridDados.Rows.Add(new string[] { "Inclinação Lateral", "0" });
|
|
gridDados.Rows.Add(new string[] { "Giro Lateral", "0" });
|
|
gridDados.Rows.Add(new string[] { "Velocidade X", "0" });
|
|
gridDados.Rows.Add(new string[] { "Velocidade Y", "0" });
|
|
gridDados.Rows.Add(new string[] { "Velocidade Z", "0" });
|
|
gridDados.Rows.Add(new string[] { "Colisão", "Não" });
|
|
gridDados.Rows.Add(new string[] { "Obstáculos", "0" });
|
|
gridDados.Rows.Add(new string[] { "Obstáculo Próximo", "" });
|
|
gridDados.Rows.Add(new string[] { "Obstáculo Distante", "" });
|
|
gridDados.Rows.Add(new string[] { "Distância Esquerda", "0" });
|
|
gridDados.Rows.Add(new string[] { "Distância Direita", "0" });
|
|
gridDados.Rows.Add(new string[] { "Desvio Necessário", "Não" });
|
|
gridDados.Rows.Add(new string[] { "Direção Desvio", "Frente" });
|
|
gridDados.Rows.Add(new string[] { "Angulo Desvio", "0" });
|
|
}
|
|
|
|
DefinirDispositivo();
|
|
|
|
if (picRGB != null)
|
|
{
|
|
picRGB.DoubleClick -= picRgb_Click;
|
|
picRGB.DoubleClick += picRgb_Click;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public static void AtualizarTrackBar(TrackBar tkAng)
|
|
{
|
|
trackBarAngle = tkAng;
|
|
|
|
if (trackBarAngle != null)
|
|
{
|
|
trackBarAngle.Maximum = kinectSensor.MaxElevationAngle;
|
|
trackBarAngle.Minimum = kinectSensor.MinElevationAngle;
|
|
trackBarAngle.Value = kinectSensor.ElevationAngle;
|
|
trackBarAngle.ValueChanged += TrackBarAngle_ValueChanged;
|
|
}
|
|
}
|
|
|
|
private static void TrackBarAngle_ValueChanged(object sender, EventArgs e)
|
|
{
|
|
AtualizarAngulo(trackBarAngle.Value);
|
|
}
|
|
|
|
public static void AtualizarAngulo(int Angulo)
|
|
{
|
|
try
|
|
{
|
|
if (kinectSensor != null && kinectSensor.IsRunning)
|
|
{
|
|
kinectSensor.ElevationAngle = Angulo;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine("Erro ao atualziar angulo do Kinect: " + ex.Message);
|
|
}
|
|
}
|
|
|
|
private static void DefinirDispositivo()
|
|
{
|
|
if (Iniciado && !SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Knt))
|
|
{
|
|
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
|
{
|
|
Endereco = "USB",
|
|
Dispositivo = T_Code.Knt,
|
|
Erro = !Iniciado,
|
|
Versao = "1",
|
|
});
|
|
|
|
IniciarRotinas();
|
|
}
|
|
}
|
|
|
|
private static void AtualizarLeitura()
|
|
{
|
|
Leitura.Momento = DateTime.Now;
|
|
|
|
if (!Iniciado)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var Acelerometro = kinectSensor.AccelerometerGetCurrentReading();
|
|
Leitura.AccX = Acelerometro.X;
|
|
Leitura.AccY = Acelerometro.Y;
|
|
Leitura.AccZ = Acelerometro.Z;
|
|
Leitura.AccW = Acelerometro.W;
|
|
|
|
aceleracaoAnterior[0] = Acelerometro.X;
|
|
aceleracaoAnterior[1] = Acelerometro.Y;
|
|
aceleracaoAnterior[2] = Acelerometro.Z;
|
|
|
|
double pitch = Math.Atan2(Acelerometro.Y, Math.Sqrt(Math.Pow(Acelerometro.X, 2) + Math.Pow(Acelerometro.Z, 2))) * (180 / Math.PI);
|
|
double roll = Math.Atan2(Acelerometro.X, Math.Sqrt(Math.Pow(Acelerometro.Y, 2) + Math.Pow(Acelerometro.Z, 2))) * (180 / Math.PI);
|
|
double yaw = Math.Atan2(Acelerometro.Z, Math.Sqrt(Math.Pow(Acelerometro.X, 2) + Math.Pow(Acelerometro.Y, 2))) * (180 / Math.PI);
|
|
Leitura.Pitch = pitch;
|
|
Leitura.Roll = roll;
|
|
Leitura.Yaw = yaw;
|
|
|
|
double totalAcceleration = Math.Sqrt(Math.Pow(Leitura.AccX, 2) + Math.Pow(Leitura.AccY, 2) + Math.Pow(Leitura.AccZ, 2));
|
|
double threshold = 1.2;
|
|
Leitura.Colisao = totalAcceleration > threshold;
|
|
|
|
CalcularDistanciasLaterais();
|
|
|
|
if (Leitura.UltimoCalculo.AddSeconds(dt) < DateTime.Now && kinectSensor.Status == KinectStatus.Connected)
|
|
{
|
|
Leitura.UltimoCalculo = DateTime.Now;
|
|
|
|
// Loop para calcular a velocidade em cada eixo
|
|
velocidade[0] += (Acelerometro.X + aceleracaoAnterior[0]) / 2 * dt;
|
|
velocidade[1] += (Acelerometro.Y + aceleracaoAnterior[1]) / 2 * dt;
|
|
velocidade[2] += (Acelerometro.Z + aceleracaoAnterior[2]) / 2 * dt;
|
|
Leitura.VelX = velocidade[0];
|
|
Leitura.VelY = velocidade[1];
|
|
Leitura.VelZ = velocidade[2];
|
|
}
|
|
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect = Leitura;
|
|
}
|
|
|
|
private static KinectParametrosModel CarregarParametros()
|
|
{
|
|
//string nomeArquivo = Variaveis.CaminhoParametros + "parameters" + Enum.GetName(typeof(T_Code), T_Code.Knt) + ".par";
|
|
|
|
var newConfig = new KinectParametrosModel()
|
|
{
|
|
SonarAtivado = false,
|
|
Angulo = 0,
|
|
focalLength = 580f,
|
|
sensorWidthMM = 70f,
|
|
maxDepth = 4000,
|
|
minDepth = 500,
|
|
distProjetarTrajetoriaMm = 2000,
|
|
depthFrameHeight = Iniciado ? kinectSensor.DepthStream.FrameHeight : 480,
|
|
depthFrameWidth = Iniciado ? kinectSensor.DepthStream.FrameWidth : 640,
|
|
subdivisoesX = 10,
|
|
subdivisoesY = 10,
|
|
Subdivisoes = new List<Subdivisao>(),
|
|
toleranciaDeteccaoTrajetoria = 100,
|
|
toleranciaDeteccaoSuperior = 200,
|
|
};
|
|
|
|
VersaoArquivoModel arquivo = VersionamentoService.ArquivoParametros(T_Code.Knt);
|
|
if (arquivo != null)
|
|
{
|
|
string nomeArquivo = arquivo.CaminhoCompleto;
|
|
if (!File.Exists(nomeArquivo))
|
|
{
|
|
File.WriteAllText(nomeArquivo, JsonConvert.SerializeObject(newConfig));
|
|
}
|
|
var configText = File.ReadAllText(nomeArquivo);
|
|
var Parametros = JsonConvert.DeserializeObject<KinectParametrosModel>(configText);
|
|
|
|
return Parametros;
|
|
}
|
|
else
|
|
{
|
|
return newConfig;
|
|
}
|
|
}
|
|
|
|
public static void SalvarParametros(KinectParametrosModel newConfig = null)
|
|
{
|
|
if (newConfig == null)
|
|
{
|
|
newConfig = Parametros;
|
|
}
|
|
|
|
VersaoArquivoModel arquivo = VersionamentoService.ArquivoParametros(T_Code.Knt);
|
|
if (arquivo != null)
|
|
{
|
|
string nomeArquivo = arquivo.CaminhoCompleto;
|
|
File.WriteAllText(nomeArquivo, JsonConvert.SerializeObject(newConfig));
|
|
}
|
|
}
|
|
|
|
private static async Task tmrLeitura_Tick()
|
|
{
|
|
bool usoNaoNecessario = !Iniciado || (picRGB == null && !Variaveis.OperacaoEmAndamento.Iniciado);
|
|
tmrLeitura.SetInterval(usoNaoNecessario ? 1000 : 200);
|
|
|
|
if (usoNaoNecessario)
|
|
{
|
|
return;
|
|
}
|
|
|
|
Leitura.Obstaculos = IdentificarObstaculos();
|
|
Leitura.AtualizarUltimoStatusDeteccao();
|
|
|
|
lock (_lockBitmapMap)
|
|
{
|
|
if (bitmapRGB != null)
|
|
{
|
|
bitmapMap = (Bitmap)ExibirObstaculos(bitmapRGB, Parametros.Subdivisoes, Leitura.Obstaculos, MostrarTexto).Clone();
|
|
}
|
|
}
|
|
|
|
AtualizarLeitura();
|
|
|
|
AtualizarGrid();
|
|
|
|
AtualizarPictureBoxes();
|
|
|
|
if (Parametros.SonarAtivado && Leitura.DesvioNecessario && Leitura.DirecaoDesvio == Direcao.Parado && Variaveis.OperacaoEmAndamento.DispMvd != null && Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Any(x => x.MovMotor.BLD_Inicializado))
|
|
{
|
|
GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov);
|
|
}
|
|
}
|
|
|
|
private static void KinectSensor_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
|
|
{
|
|
if (colorFrame != null)
|
|
{
|
|
colorFrame.CopyPixelDataTo(colorPixels);
|
|
|
|
// Usar lock para garantir que apenas uma thread acesse o bitmapRGB por vez
|
|
lock (_lockBitmapRGB)
|
|
{
|
|
// Criar um novo Bitmap para cada frame, evitando reutilização do bitmapRGB anterior
|
|
using (Bitmap newBitmap = new Bitmap(kinectSensor.ColorStream.FrameWidth, kinectSensor.ColorStream.FrameHeight, PixelFormat.Format32bppRgb))
|
|
{
|
|
BitmapData bitmapData = newBitmap.LockBits(new Rectangle(0, 0, newBitmap.Width, newBitmap.Height), ImageLockMode.WriteOnly, newBitmap.PixelFormat);
|
|
IntPtr ptr = bitmapData.Scan0;
|
|
|
|
// Copiar os dados dos pixels de cor para o novo bitmap
|
|
Marshal.Copy(colorPixels, 0, ptr, colorPixels.Length);
|
|
|
|
// Desbloquear o bitmap para permitir o uso posterior
|
|
newBitmap.UnlockBits(bitmapData);
|
|
|
|
// Espelhar a imagem e atribuí-la ao bitmapRGB
|
|
bitmapRGB = EspelharImagemRGB(newBitmap);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine("Erro ao receber dados RGB do Kinect: " + ex.Message);
|
|
}
|
|
}
|
|
|
|
private static Bitmap EspelharImagemRGB(Bitmap originalBitmap)
|
|
{
|
|
Bitmap espelhada = (Bitmap)originalBitmap.Clone();
|
|
espelhada.RotateFlip(RotateFlipType.RotateNoneFlipX);
|
|
return espelhada;
|
|
}
|
|
|
|
private static void KinectSensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
|
|
{
|
|
if (depthFrame != null)
|
|
{
|
|
lock (_lockDepthPixels)
|
|
{
|
|
depthPixels = new DepthImagePixel[depthFrame.PixelDataLength];
|
|
depthFrame.CopyDepthImagePixelDataTo(depthPixels);
|
|
}
|
|
|
|
lock (_lockBitmapHeat)
|
|
{
|
|
EspelharDepthPixels(depthPixels, Parametros.depthFrameWidth, Parametros.depthFrameHeight);
|
|
bitmapHeat = GenerateHeatmapBitmap(depthPixels);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine("Erro ao receber dados de profundidade do Kinect: " + ex.Message);
|
|
}
|
|
}
|
|
|
|
private static void EspelharDepthPixels(DepthImagePixel[] depthPixels, int width, int height)
|
|
{
|
|
for (int y = 0; y < height; y++)
|
|
{
|
|
int startIndex = y * width;
|
|
int endIndex = (y + 1) * width - 1;
|
|
|
|
while (startIndex < endIndex)
|
|
{
|
|
// Trocar os pixels na linha atual
|
|
DepthImagePixel temp = depthPixels[startIndex];
|
|
depthPixels[startIndex] = depthPixels[endIndex];
|
|
depthPixels[endIndex] = temp;
|
|
|
|
startIndex++;
|
|
endIndex--;
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void AtualizarGrid()
|
|
{
|
|
if (gridDados != null && gridDados.Rows.Count > 0)
|
|
{
|
|
var ObstaculoMaisProximo = Leitura.Obstaculos.OrderBy(x => x.DistanciaMedia_mm).FirstOrDefault();
|
|
var ObstaculoMaisDistante = Leitura.Obstaculos.OrderByDescending(x => x.DistanciaMedia_mm).FirstOrDefault();
|
|
|
|
gridDados.Rows[0].Cells[1].Value = Leitura.Momento.ToString("dd/MM/yyyy HH:mm:ss.fff");
|
|
gridDados.Rows[1].Cells[1].Value = Leitura.AnguloElevacao;
|
|
gridDados.Rows[2].Cells[1].Value = Leitura.AccX;
|
|
gridDados.Rows[3].Cells[1].Value = Leitura.AccY;
|
|
gridDados.Rows[4].Cells[1].Value = Leitura.AccZ;
|
|
gridDados.Rows[5].Cells[1].Value = Leitura.Pitch;
|
|
gridDados.Rows[6].Cells[1].Value = Leitura.Roll;
|
|
gridDados.Rows[7].Cells[1].Value = Leitura.Yaw;
|
|
gridDados.Rows[8].Cells[1].Value = Leitura.VelX;
|
|
gridDados.Rows[9].Cells[1].Value = Leitura.VelY;
|
|
gridDados.Rows[10].Cells[1].Value = Leitura.VelZ;
|
|
gridDados.Rows[11].Cells[1].Value = Leitura.Colisao ? "Sim" : "Não";
|
|
gridDados.Rows[12].Cells[1].Value = Leitura.Obstaculos.Count().ToString();
|
|
gridDados.Rows[13].Cells[1].Value = ObstaculoMaisProximo != null ? $"D={ObstaculoMaisProximo.DistanciaMedia_mm.ToString("0.00")}, L={ObstaculoMaisProximo.Largura_mm.ToString("0.00")}, A={ObstaculoMaisProximo.Altura_mm.ToString("0.00")}" : "";
|
|
gridDados.Rows[14].Cells[1].Value = ObstaculoMaisDistante != null ? $"D={ObstaculoMaisDistante.DistanciaMedia_mm.ToString("0.00")}, L={ObstaculoMaisDistante.Largura_mm.ToString("0.00")}, A={ObstaculoMaisDistante.Altura_mm.ToString("0.00")}" : "";
|
|
gridDados.Rows[15].Cells[1].Value = Leitura.EspacoLivreEsquerda.ToString("0.00");
|
|
gridDados.Rows[16].Cells[1].Value = Leitura.EspacoLivreDireita.ToString("0.00");
|
|
gridDados.Rows[17].Cells[1].Value = Leitura.DesvioNecessario ? "Sim" : "Não";
|
|
gridDados.Rows[18].Cells[1].Value = Leitura.DirecaoDesvio.ToString();
|
|
gridDados.Rows[19].Cells[1].Value = Leitura.AnguloDesvio.ToString("0.00");
|
|
}
|
|
}
|
|
|
|
private static void AtualizarPictureBoxes()
|
|
{
|
|
lock (_lockBitmapHeat)
|
|
{
|
|
if (picProfundidade != null && bitmapHeat != null)
|
|
{
|
|
picProfundidade.Image = (Bitmap)bitmapHeat.Clone();
|
|
}
|
|
}
|
|
|
|
if (MostrarGrade)
|
|
{
|
|
lock (_lockBitmapMap)
|
|
{
|
|
if (picRGB != null && bitmapMap != null)
|
|
{
|
|
picRGB.Image = (Bitmap)bitmapMap.Clone();
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
lock (_lockBitmapRGB)
|
|
{
|
|
if (picRGB != null && bitmapRGB != null)
|
|
{
|
|
picRGB.Image = (Bitmap)bitmapRGB.Clone();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void CalcularDistanciasLaterais()
|
|
{
|
|
if (depthPixels == null || depthPixels.Length == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
int width = Parametros.depthFrameWidth;
|
|
int height = Parametros.depthFrameHeight;
|
|
|
|
int faixaVertical = (int)(height * 0.2); // 20% da altura
|
|
int faixaHorizontal = (int)(width * 0.2); // 20% da largura
|
|
|
|
List<int> distanciasEsquerda = new List<int>();
|
|
List<int> distanciasDireita = new List<int>();
|
|
|
|
// Percorrer a faixa na coluna mais à esquerda
|
|
for (int y = height - faixaVertical; y < height; y++)
|
|
{
|
|
for (int x = 0; x < faixaHorizontal; x++)
|
|
{
|
|
int depthEsquerda = depthPixels[y * width + x].Depth;
|
|
if (depthEsquerda > Parametros.minDepth && depthEsquerda < Parametros.maxDepth)
|
|
{
|
|
distanciasEsquerda.Add(depthEsquerda);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Percorrer a faixa na coluna mais à direita
|
|
for (int y = height - faixaVertical; y < height; y++)
|
|
{
|
|
for (int x = width - faixaHorizontal; x < width; x++)
|
|
{
|
|
int depthDireita = depthPixels[y * width + x].Depth;
|
|
if (depthDireita > Parametros.minDepth && depthDireita < Parametros.maxDepth)
|
|
{
|
|
distanciasDireita.Add(depthDireita);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Calcular a distância mínima na faixa
|
|
double distanciaMinimaEsquerda = distanciasEsquerda.Count > 0 ? distanciasEsquerda.Min() : Parametros.maxDepth;
|
|
double distanciaMinimaDireita = distanciasDireita.Count > 0 ? distanciasDireita.Min() : Parametros.maxDepth;
|
|
|
|
// Atualizar o modelo
|
|
Leitura.DistanciaDaEsquerda = distanciaMinimaEsquerda;
|
|
Leitura.DistanciaDaDireita = distanciaMinimaDireita;
|
|
}
|
|
|
|
private static Bitmap GenerateHeatmapBitmap(DepthImagePixel[] depthPixels)
|
|
{
|
|
if (depthPixels == null || depthPixels.Length == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
int width = Parametros.depthFrameWidth;
|
|
int height = Parametros.depthFrameHeight;
|
|
Bitmap heatmapBitmap = new Bitmap(width, height, PixelFormat.Format24bppRgb);
|
|
|
|
BitmapData bitmapData = heatmapBitmap.LockBits(
|
|
new Rectangle(0, 0, width, height),
|
|
ImageLockMode.WriteOnly,
|
|
heatmapBitmap.PixelFormat);
|
|
|
|
int bytesPerPixel = Image.GetPixelFormatSize(heatmapBitmap.PixelFormat) / 8;
|
|
int byteCount = bitmapData.Stride * height;
|
|
byte[] pixels = new byte[byteCount];
|
|
|
|
Parallel.For(0, height, y =>
|
|
{
|
|
for (int x = 0; x < width; x++)
|
|
{
|
|
int index = x + y * width;
|
|
int depth = depthPixels[index].Depth;
|
|
|
|
Color color = GetHeatmapColor(depth);
|
|
int pixelIndex = (y * bitmapData.Stride) + (x * bytesPerPixel);
|
|
|
|
pixels[pixelIndex] = color.B;
|
|
pixels[pixelIndex + 1] = color.G;
|
|
pixels[pixelIndex + 2] = color.R;
|
|
}
|
|
});
|
|
|
|
System.Runtime.InteropServices.Marshal.Copy(pixels, 0, bitmapData.Scan0, pixels.Length);
|
|
heatmapBitmap.UnlockBits(bitmapData);
|
|
|
|
return heatmapBitmap;
|
|
}
|
|
|
|
private static Color GetHeatmapColor(int depth)
|
|
{
|
|
if (depth > 0 && depth < Parametros.minDepth)
|
|
{
|
|
Parametros.minDepth = depth;
|
|
}
|
|
if (depth > Parametros.maxDepth && depth < 8000)
|
|
{
|
|
Parametros.maxDepth = depth;
|
|
}
|
|
if (depth == 0 || depth < Parametros.minDepth || depth > Parametros.maxDepth)
|
|
{
|
|
return Color.Gray;
|
|
}
|
|
|
|
double ratio = (depth - Parametros.minDepth) / (double)(Parametros.maxDepth - Parametros.minDepth);
|
|
int red = (int)(255 * Math.Min(1, Math.Max(0, (ratio - 0.5) * 2)));
|
|
int blue = (int)(255 * Math.Min(1, Math.Max(0, (0.5 - ratio) * 2)));
|
|
int green = 255 - red - blue;
|
|
|
|
return Color.FromArgb(red, green, blue);
|
|
}
|
|
|
|
public static float CalculatePixelToMMRatio(float distance)
|
|
{
|
|
// Exemplo de cálculo simplificado da relação pixels para milímetros
|
|
// A relação exata pode variar e pode precisar de ajustes
|
|
// Aqui assumimos uma relação linear simplificada
|
|
|
|
float pixelToMM = (distance / Parametros.focalLength) * (Parametros.sensorWidthMM / Parametros.depthFrameWidth) * 10;
|
|
return pixelToMM;
|
|
}
|
|
|
|
public static Bitmap ExibirObstaculos(Bitmap bitmap, List<Subdivisao> subdivisoes, List<Obstaculo> obstaculos, bool ExibirTexto = false)
|
|
{
|
|
if (bitmap == null)
|
|
{
|
|
return bitmap;
|
|
}
|
|
|
|
try
|
|
{
|
|
var _bitmapMap = (Bitmap)bitmap.Clone();
|
|
|
|
using (Graphics g = Graphics.FromImage(_bitmapMap))
|
|
{
|
|
// Desenhar a trajetória do robô
|
|
DesenharTrajetoriaRobo(g, subdivisoes);
|
|
|
|
foreach (var obstaculo in obstaculos)
|
|
{
|
|
Pen Cor = (obstaculo.Tipo == TipoObstaculo.Ressalto ? Pens.Red : obstaculo.Tipo == TipoObstaculo.Rebaixo ? Pens.Blue : Pens.Black);
|
|
Rectangle rect = new Rectangle(obstaculo.X, obstaculo.Y, obstaculo.Width, obstaculo.Height);
|
|
g.DrawRectangle(Cor, rect);
|
|
|
|
if (ExibirTexto)
|
|
{
|
|
string distanceText = $"D: {obstaculo.DistanciaMedia_mm:F1} mm, L: {obstaculo.Largura_mm:F1} mm, A: {obstaculo.Altura_mm:F1} mm";
|
|
Font font = new Font("Arial", 12);
|
|
SizeF textSize = g.MeasureString(distanceText, font);
|
|
PointF textLocation = new PointF(obstaculo.X + (obstaculo.Width - textSize.Width) / 2, obstaculo.Y + (obstaculo.Height - textSize.Height) / 2);
|
|
g.DrawString(distanceText, font, Brushes.Red, textLocation);
|
|
}
|
|
}
|
|
|
|
if (Leitura.DesvioNecessario && Leitura.DirecaoDesvio != Direcao.Parado)
|
|
{
|
|
// Desenhar a seta
|
|
DesenharSeta(g, _bitmapMap.Width, _bitmapMap.Height, 90 - (float)Leitura.AnguloDesvio);
|
|
}
|
|
}
|
|
|
|
return _bitmapMap;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
|
|
}
|
|
|
|
return bitmap;
|
|
}
|
|
|
|
private static void DesenharSeta(Graphics g, int largura, int altura, float angulo)
|
|
{
|
|
// Ponto de início (base da seta)
|
|
PointF pontoInicio = new PointF(largura / 2, altura);
|
|
|
|
// Comprimento da seta (2/3 da altura da tela)
|
|
float comprimentoSeta = altura * 1 / 3;
|
|
|
|
// Cálculo do ponto final da seta com base no ângulo
|
|
float anguloRad = (float)(Math.PI * angulo / 180.0);
|
|
PointF pontoFinal = new PointF(
|
|
pontoInicio.X + comprimentoSeta * (float)Math.Cos(anguloRad),
|
|
pontoInicio.Y - comprimentoSeta * (float)Math.Sin(anguloRad)
|
|
);
|
|
|
|
// Desenhar a linha da seta
|
|
Pen penSeta = new Pen(Color.Orange, 4);
|
|
g.DrawLine(penSeta, pontoInicio, pontoFinal);
|
|
|
|
// Desenhar as pontas da seta
|
|
float tamanhoPonta = 20;
|
|
PointF ponta1 = new PointF(
|
|
pontoFinal.X + tamanhoPonta * (float)Math.Cos(anguloRad + Math.PI * 3 / 4),
|
|
pontoFinal.Y - tamanhoPonta * (float)Math.Sin(anguloRad + Math.PI * 3 / 4)
|
|
);
|
|
PointF ponta2 = new PointF(
|
|
pontoFinal.X + tamanhoPonta * (float)Math.Cos(anguloRad - Math.PI * 3 / 4),
|
|
pontoFinal.Y - tamanhoPonta * (float)Math.Sin(anguloRad - Math.PI * 3 / 4)
|
|
);
|
|
|
|
g.DrawLine(penSeta, pontoFinal, ponta1);
|
|
g.DrawLine(penSeta, pontoFinal, ponta2);
|
|
}
|
|
|
|
|
|
|
|
private static void picRgb_Click(object sender, EventArgs e)
|
|
{
|
|
PictureBox pnl = (PictureBox)sender;
|
|
double fX = 640.0 / pnl.Width;
|
|
double fY = 480.0 / pnl.Height;
|
|
int x = Convert.ToInt32(((MouseEventArgs)e).X * fX);
|
|
int y = Convert.ToInt32(((MouseEventArgs)e).Y * fY);
|
|
int p = depthPixels[y * 640 + x].Depth;
|
|
var s = Parametros.Subdivisoes.FirstOrDefault(a => x >= a.X && x <= a.X + a.Largura && y >= a.Y && y <= a.Y + a.Altura);
|
|
string t = s != null ? s.IsZonaSuperior ? "Superior " : "Trajetória " : "";
|
|
string tp = s != null && s.ObstaculoDetectado ? $"\r\nTipo: {s.Tipo.ToString()}" : "";
|
|
string zona = s != null ? $"{t}(X: {s.idxX}, Y: {s.idxY})\r\nProfundidade Zona: {s.ProfundidadeEsperada}\r\nProfundidade Média: {s.ProfundidadeMedia}\r\nDetecção: {s.ObstaculoDetectado}{tp}" : "Fora";
|
|
MessageBox.Show($"X: {x}\r\nY: {y}\r\nProfundidade Pixel: {p}\r\nZona: {zona}");
|
|
}
|
|
|
|
public static int EncontrarLinhaMaisProxima(DepthImagePixel[] depthPixels, int width, int height, int distanceMm)
|
|
{
|
|
if (depthPixels == null || depthPixels.Length == 0)
|
|
{
|
|
return height / 2;
|
|
}
|
|
|
|
int linhaMaisProxima = -1;
|
|
int diferencaMinima = int.MaxValue;
|
|
|
|
for (int y = 0; y < height; y++)
|
|
{
|
|
int somaDiferenca = 0;
|
|
int contagemValida = 0;
|
|
|
|
for (int x = 0; x < width; x++)
|
|
{
|
|
int profundidade = depthPixels[y * width + x].Depth;
|
|
if (profundidade > 0)
|
|
{
|
|
int diferenca = Math.Abs(profundidade - distanceMm);
|
|
somaDiferenca += diferenca;
|
|
contagemValida++;
|
|
}
|
|
}
|
|
|
|
if (contagemValida > 0)
|
|
{
|
|
int diferencaMedia = somaDiferenca / contagemValida;
|
|
if (diferencaMedia <= diferencaMinima) // Note the <= instead of <
|
|
{
|
|
diferencaMinima = diferencaMedia;
|
|
linhaMaisProxima = y;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (linhaMaisProxima == -1)
|
|
{
|
|
linhaMaisProxima = height / 2;
|
|
}
|
|
|
|
return linhaMaisProxima;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
public static void RecalibrarSubdivisoes()
|
|
{
|
|
Parametros.Subdivisoes.Clear();
|
|
|
|
CalcularProfundidadeMedia(depthPixels, Parametros.Subdivisoes, Parametros.depthFrameWidth, Parametros.minDepth, Parametros.maxDepth);
|
|
}
|
|
|
|
private static void DesenharTrajetoriaRobo(Graphics g, List<Subdivisao> subdivisoes)
|
|
{
|
|
var subdivisoesTrajetoria = subdivisoes.Where(x => !x.IsZonaSuperior).ToList();
|
|
DesenharSubdivisoes(g, subdivisoesTrajetoria, subdivisoesTrajetoria.FirstOrDefault().Tolerancia, Color.Green, Color.Green, false);
|
|
var subdivisoesSuperior = subdivisoes.Where(x => x.IsZonaSuperior).ToList();
|
|
DesenharSubdivisoes(g, subdivisoesSuperior, subdivisoesSuperior.FirstOrDefault().Tolerancia, Color.Red, Color.Red, true);
|
|
}
|
|
|
|
// Método para criar subdivisões dentro da trajetória projetada
|
|
private static List<Subdivisao> CriarSubdivisoes(int width, int height, int subdivisoesHorizontais, int subdivisoesVerticais, int alturaInicio, int alturaFim, bool isZonaSuperior)
|
|
{
|
|
List<Subdivisao> subdivisoes = new List<Subdivisao>();
|
|
|
|
if (isZonaSuperior)
|
|
{
|
|
int larguraSubdivisao = width / subdivisoesHorizontais;
|
|
int alturaSubdivisao = (alturaFim - alturaInicio) / subdivisoesVerticais;
|
|
|
|
for (int i = 0; i < subdivisoesHorizontais; i++)
|
|
{
|
|
for (int j = 0; j < subdivisoesVerticais; j++)
|
|
{
|
|
subdivisoes.Add(new Subdivisao
|
|
{
|
|
idxX = i,
|
|
idxY = j,
|
|
X = i * larguraSubdivisao,
|
|
Y = alturaInicio + j * alturaSubdivisao,
|
|
Largura = larguraSubdivisao,
|
|
Altura = alturaSubdivisao,
|
|
IsZonaSuperior = isZonaSuperior,
|
|
Tolerancia = Parametros.toleranciaDeteccaoSuperior,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
int larguraTopo = width * 8 / 10; // Largura no topo (80% da largura da imagem)
|
|
double PxToMm = CalculatePixelToMMRatio(Parametros.distProjetarTrajetoriaMm);
|
|
int larguraBase = (int)(VariaveisEquipamento.LarguraEquipamentoMm / PxToMm); // Base da projeção é 1 metro projetado a 2 metros de distância
|
|
|
|
for (int i = 0; i < subdivisoesHorizontais; i++)
|
|
{
|
|
for (int j = 0; j < subdivisoesVerticais; j++)
|
|
{
|
|
int larguraAtual = larguraBase + (larguraTopo - larguraBase) * j / subdivisoesVerticais; // Largura da subdivisão considerando a perspectiva
|
|
int x = (width - larguraAtual) / 2 + i * larguraAtual / subdivisoesHorizontais;
|
|
int y = alturaInicio + j * (alturaFim - alturaInicio) / subdivisoesVerticais;
|
|
int largura = larguraAtual / subdivisoesHorizontais;
|
|
int altura = (alturaFim - alturaInicio) / subdivisoesVerticais;
|
|
|
|
subdivisoes.Add(new Subdivisao
|
|
{
|
|
idxX = i,
|
|
idxY = j,
|
|
X = x,
|
|
Y = y,
|
|
Largura = largura,
|
|
Altura = altura,
|
|
IsZonaSuperior = isZonaSuperior,
|
|
Tolerancia = Parametros.toleranciaDeteccaoTrajetoria
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
return subdivisoes;
|
|
}
|
|
|
|
// Método para calcular a profundidade média em cada subdivisão
|
|
private static void CalcularProfundidadeMedia(DepthImagePixel[] depthPixels, List<Subdivisao> subdivisoes, int width, int minDepth, int maxDepth)
|
|
{
|
|
if (depthPixels == null || depthPixels.Length == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (subdivisoes == null || !subdivisoes.Any())
|
|
{
|
|
CalibrarProfundidadesEsperadas();
|
|
return;
|
|
}
|
|
|
|
foreach (var subdivisao in subdivisoes)
|
|
{
|
|
int somaProfundidade = 0;
|
|
int contagemValida = 0;
|
|
|
|
for (int x = subdivisao.X; x < subdivisao.X + subdivisao.Largura; x++)
|
|
{
|
|
for (int y = subdivisao.Y; y < subdivisao.Y + subdivisao.Altura; y++)
|
|
{
|
|
int profundidade = depthPixels[y * width + x].Depth;
|
|
if (profundidade > minDepth && profundidade < maxDepth)
|
|
{
|
|
somaProfundidade += profundidade;
|
|
contagemValida++;
|
|
}
|
|
}
|
|
}
|
|
|
|
subdivisao.ProfundidadeMedia = contagemValida > 0 ? somaProfundidade / contagemValida : 0;
|
|
}
|
|
}
|
|
|
|
// Método para desenhar subdivisões e indicar obstáculos
|
|
private static void DesenharSubdivisoes(Graphics g, List<Subdivisao> subdivisoes, int tolerancia, Color corContorno, Color corPreenchimento, bool isZonaSuperior)
|
|
{
|
|
foreach (var subdivisao in subdivisoes)
|
|
{
|
|
Color cor = subdivisao.ObstaculoDetectado ? Color.FromArgb(128, corPreenchimento) : Color.Transparent;
|
|
|
|
Brush brush = new SolidBrush(cor);
|
|
Pen pen = new Pen(corContorno);
|
|
|
|
Rectangle rect = new Rectangle(subdivisao.X, subdivisao.Y, subdivisao.Largura, subdivisao.Altura);
|
|
g.FillRectangle(brush, rect);
|
|
g.DrawRectangle(pen, rect);
|
|
}
|
|
}
|
|
|
|
// Método para calibrar subdivisões
|
|
private static void CalibrarSubdivisoes(DepthImagePixel[] depthPixels, List<Subdivisao> subdivisoes, int width, int alturaSuperiorFim)
|
|
{
|
|
foreach (var subdivisao in subdivisoes)
|
|
{
|
|
int somaProfundidade = 0;
|
|
int contagemValida = 0;
|
|
|
|
for (int x = subdivisao.X; x < subdivisao.X + subdivisao.Largura; x++)
|
|
{
|
|
for (int y = subdivisao.Y; y < subdivisao.Y + subdivisao.Altura; y++)
|
|
{
|
|
int profundidade = depthPixels[y * width + x].Depth;
|
|
if (profundidade > Parametros.minDepth && profundidade < Parametros.maxDepth)
|
|
{
|
|
somaProfundidade += profundidade;
|
|
contagemValida++;
|
|
}
|
|
}
|
|
}
|
|
|
|
subdivisao.ProfundidadeEsperada = contagemValida > 0 ? somaProfundidade / contagemValida : 0;
|
|
|
|
// Para a área superior, definimos uma profundidade esperada padrão
|
|
if (subdivisao.Y + subdivisao.Altura <= alturaSuperiorFim)
|
|
{
|
|
subdivisao.ProfundidadeEsperada = Parametros.distProjetarTrajetoriaMm;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Método para calibrar profundidades esperadas
|
|
public static void CalibrarProfundidadesEsperadas()
|
|
{
|
|
Parametros.depthFrameHeight = kinectSensor.DepthStream.FrameHeight;
|
|
Parametros.depthFrameWidth = kinectSensor.DepthStream.FrameWidth;
|
|
|
|
int width = Parametros.depthFrameWidth;
|
|
int height = Parametros.depthFrameHeight;
|
|
int subdivisoesHorizontais = Parametros.subdivisoesX;
|
|
int subdivisoesVerticais = Parametros.subdivisoesY;
|
|
|
|
int linhaProxima2m = EncontrarLinhaMaisProxima(depthPixels, Parametros.depthFrameWidth, Parametros.depthFrameHeight, Parametros.distProjetarTrajetoriaMm);
|
|
//int linhaProxima2m = height * 2 / 3;
|
|
|
|
// Calibrar área inferior (trajetória do robô)
|
|
int alturaInferiorInicio = linhaProxima2m;
|
|
int alturaInferiorFim = height;
|
|
var subdivisoesInferior = CriarSubdivisoes(width, height, subdivisoesHorizontais, subdivisoesVerticais, alturaInferiorInicio, alturaInferiorFim, false);
|
|
CalibrarSubdivisoes(depthPixels, subdivisoesInferior, width, alturaInferiorInicio);
|
|
|
|
// Calibrar área superior
|
|
int alturaSuperiorInicio = 0;
|
|
int alturaSuperiorFim = linhaProxima2m;
|
|
var subdivisoesSuperior = CriarSubdivisoes(width, height, subdivisoesHorizontais / 2, subdivisoesVerticais / 2, alturaSuperiorInicio, alturaSuperiorFim, true);
|
|
CalibrarSubdivisoes(depthPixels, subdivisoesSuperior, width, alturaSuperiorFim);
|
|
|
|
Parametros.Subdivisoes = new List<Subdivisao>();
|
|
Parametros.Subdivisoes.AddRange(subdivisoesInferior);
|
|
Parametros.Subdivisoes.AddRange(subdivisoesSuperior);
|
|
|
|
SalvarParametros();
|
|
}
|
|
|
|
private static List<Obstaculo> IdentificarObstaculos()
|
|
{
|
|
if (!Parametros.SonarAtivado || Parametros.Subdivisoes == null)
|
|
{
|
|
return new List<Obstaculo>();
|
|
}
|
|
|
|
CalcularProfundidadeMedia(depthPixels, Parametros.Subdivisoes, Parametros.depthFrameWidth, Parametros.minDepth, Parametros.maxDepth);
|
|
|
|
List<Subdivisao> subdivisoes = Parametros.Subdivisoes;
|
|
|
|
|
|
double tolerancia = 100.0;
|
|
|
|
List<List<Subdivisao>> AgrupamentoPorProfundidade = new List<List<Subdivisao>>();
|
|
foreach (var sub in subdivisoes.Where(x => x.ObstaculoDetectado))
|
|
{
|
|
bool add = false;
|
|
foreach (var grupo in AgrupamentoPorProfundidade)
|
|
{
|
|
if (grupo.Any(x => Math.Abs(x.ProfundidadeMedia - sub.ProfundidadeMedia) <= tolerancia))
|
|
{
|
|
grupo.Add(sub);
|
|
add = true;
|
|
continue;
|
|
}
|
|
}
|
|
if (!add)
|
|
{
|
|
AgrupamentoPorProfundidade.Add(new List<Subdivisao>() { sub });
|
|
}
|
|
}
|
|
|
|
List<List<Subdivisao>> AgrupamentoPorAdjacencia = new List<List<Subdivisao>>();
|
|
foreach (var grupo in AgrupamentoPorProfundidade)
|
|
{
|
|
while (grupo.Any())
|
|
{
|
|
var subgrupo = new List<Subdivisao> { grupo[0] };
|
|
grupo.RemoveAt(0);
|
|
|
|
bool adicionou = true;
|
|
while (adicionou)
|
|
{
|
|
adicionou = false;
|
|
var adjacentes = grupo.Where(s =>
|
|
subgrupo.Any(x =>
|
|
Math.Abs(x.X - s.X) <= Math.Max(x.Largura, s.Largura) &&
|
|
(x.IsZonaSuperior != s.IsZonaSuperior ?
|
|
VerificarContinuidade(x, s) :
|
|
Math.Abs(x.Y - s.Y) <= (Math.Max(x.Altura, s.Altura) + 1)))).ToList();
|
|
|
|
if (adjacentes.Any())
|
|
{
|
|
subgrupo.AddRange(adjacentes);
|
|
foreach (var adj in adjacentes)
|
|
{
|
|
grupo.Remove(adj);
|
|
}
|
|
adicionou = true;
|
|
}
|
|
}
|
|
AgrupamentoPorAdjacencia.Add(subgrupo);
|
|
}
|
|
}
|
|
|
|
List<Obstaculo> obstaculos = new List<Obstaculo>();
|
|
foreach (var grupoSubdivisoes in AgrupamentoPorAdjacencia)
|
|
{
|
|
var obstaculo = CriarObstaculo(grupoSubdivisoes);
|
|
if (obstaculo.DistanciaMedia_mm > 0 && obstaculo.DesvioNecessario)
|
|
{
|
|
obstaculos.Add(obstaculo);
|
|
}
|
|
}
|
|
|
|
return obstaculos;
|
|
}
|
|
|
|
// Método auxiliar para verificar continuidade entre zonas superior e inferior
|
|
private static bool VerificarContinuidade(Subdivisao sub1, Subdivisao sub2)
|
|
{
|
|
// Se as subdivisões estão na mesma zona, são adjacentes se tocarem fisicamente
|
|
if (sub1.IsZonaSuperior == sub2.IsZonaSuperior)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Se uma subdivisão está na zona superior e a outra na inferior, verifique se estão alinhadas verticalmente
|
|
// Isso considera a adjacência na transição entre as zonas
|
|
if (sub1.IsZonaSuperior && !sub2.IsZonaSuperior)
|
|
{
|
|
return sub1.idxY == sub2.idxY - 1;
|
|
}
|
|
|
|
if (!sub1.IsZonaSuperior && sub2.IsZonaSuperior)
|
|
{
|
|
return sub1.idxY - 1 == sub2.idxY;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
private static Obstaculo CriarObstaculo(List<Subdivisao> grupoObstaculo)
|
|
{
|
|
int minX = grupoObstaculo.Min(s => s.X);
|
|
int minY = grupoObstaculo.Min(s => s.Y);
|
|
int maxX = grupoObstaculo.Max(s => s.X + s.Largura);
|
|
int maxY = grupoObstaculo.Max(s => s.Y + s.Altura);
|
|
|
|
int larguraPixels = maxX - minX;
|
|
int alturaPixels = maxY - minY;
|
|
|
|
// Calcular a distância média do obstáculo
|
|
float distanciaMedia = (float)grupoObstaculo.Average(s => s.ProfundidadeMedia);
|
|
|
|
// Calcular a relação pixels para milímetros
|
|
float pixelToMM = CalculatePixelToMMRatio(distanciaMedia);
|
|
|
|
// Converter dimensões de pixels para milímetros
|
|
float larguraMM = larguraPixels * pixelToMM;
|
|
float alturaMM = alturaPixels * pixelToMM;
|
|
|
|
Obstaculo obstaculo = new Obstaculo
|
|
{
|
|
X = minX,
|
|
Y = minY,
|
|
Width = larguraPixels,
|
|
Height = alturaPixels,
|
|
DistanciaMedia_mm = distanciaMedia,
|
|
Largura_mm = larguraMM,
|
|
Altura_mm = alturaMM,
|
|
ObstaculoSuperior = grupoObstaculo.Any(x => x.IsZonaSuperior),
|
|
ObstaculoTrajetoria = grupoObstaculo.Any(x => !x.IsZonaSuperior),
|
|
Tipo = grupoObstaculo.Count(x => x.Tipo == TipoObstaculo.Ressalto) > grupoObstaculo.Count(x => x.Tipo == TipoObstaculo.Rebaixo) ? TipoObstaculo.Ressalto : TipoObstaculo.Rebaixo,
|
|
};
|
|
|
|
return obstaculo;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
public class ClusterModel
|
|
{
|
|
public int clusterId { get; set; }
|
|
public int minY { get; set; }
|
|
public int maxY { get; set; }
|
|
public double depth { get; set; }
|
|
}
|
|
} |