private List <LidarDetectedObject> DetectionObjetsProches(List <PolarPointRssi> ptList, double distanceMin, double distanceMax, double tailleSegmentationObjet)
{
List <LidarDetectedObject> ObjetsProchesList = new List <LidarDetectedObject>();
LidarDetectedObject currentObject = new LidarDetectedObject();;
//Détection des objets proches
var selectedPoints = ptList.Where(p => (p.Distance < distanceMax) && (p.Distance > distanceMin));
List <PolarPointRssi> objetsProchesPointsList = (List <PolarPointRssi>)selectedPoints.ToList();
//On segmente la liste de points sélectionnée en objets par la distance
if (objetsProchesPointsList.Count() > 0)
{
currentObject = new LidarDetectedObject();
currentObject.PtList.Add(objetsProchesPointsList[0]);
double angleInitialObjet = objetsProchesPointsList[0].Angle;
for (int i = 1; i < objetsProchesPointsList.Count; i++)
{
if ((Math.Abs(objetsProchesPointsList[i].Angle - objetsProchesPointsList[i - 1].Angle) < Toolbox.DegToRad(2)) &&
(Math.Abs(objetsProchesPointsList[i].Distance - objetsProchesPointsList[i - 1].Distance) < 0.1) &&
(Math.Abs(objetsProchesPointsList[i].Angle - angleInitialObjet) * objetsProchesPointsList[i].Distance < tailleSegmentationObjet))
//Si les pts successifs sont distants de moins de x degrés et de moins de y mètres
//et que l'objet fait moins de tailleMax en largeur
{
//Le point est cohérent avec l'objet en cours, on ajoute le point à l'objet courant
currentObject.PtList.Add(objetsProchesPointsList[i]);
}
else
{
currentObject.ExtractObjectAttributes();
ObjetsProchesList.Add(currentObject);
currentObject = new LidarDetectedObject();
currentObject.PtList.Add(objetsProchesPointsList[i]);
angleInitialObjet = objetsProchesPointsList[i].Angle;
}
}
currentObject.ExtractObjectAttributes();
ObjetsProchesList.Add(currentObject);
}
return(ObjetsProchesList);
}
private List <LidarDetectedObject> DetectionBalisesCatadioptriques(List <PolarPointRssi> ptList, double distanceMax)
{
List <LidarDetectedObject> BalisesCatadioptriquesList = new List <LidarDetectedObject>();
LidarDetectedObject currentObject = new LidarDetectedObject();;
//Détection des objets ayant un RSSI dans un intervalle correspondant aux catadioptres utilisés et proches
//double maxRssiCatadioptre = 90;
double minRssiCatadioptre = 50;
var selectedPoints = ptList.Where(p => (p.Rssi >= minRssiCatadioptre) && (p.Distance < distanceMax));
List <PolarPointRssi> balisesPointsList = (List <PolarPointRssi>)selectedPoints.ToList();
//On segmente la liste de points sélectionnée en objets par la distance
if (balisesPointsList.Count() > 0)
{
currentObject = new LidarDetectedObject();
currentObject.PtList.Add(balisesPointsList[0]);
for (int i = 1; i < balisesPointsList.Count; i++)
{
if (Math.Abs(balisesPointsList[i].Angle - balisesPointsList[i - 1].Angle) < Toolbox.DegToRad(2)) //Si les pts successifs sont distants de moins de 1 degré
{
//Le point est cohérent avec l'objet en cours, on ajoute le point à l'objet courant
currentObject.PtList.Add(balisesPointsList[i]);
}
else
{
currentObject.ExtractObjectAttributes();
BalisesCatadioptriquesList.Add(currentObject);
currentObject = new LidarDetectedObject();
currentObject.PtList.Add(balisesPointsList[i]);
}
}
currentObject.ExtractObjectAttributes();
BalisesCatadioptriquesList.Add(currentObject);
}
return(BalisesCatadioptriquesList);
}
private List <LidarDetectedObject> DetectionBalisesCatadioptriquesParRssiEtTaille(List <PolarPointRssi> ptList, double distanceMax)
{
double minRssiBalise = 60;
List <LidarDetectedObject> BalisesCatadioptriquesList = new List <LidarDetectedObject>();
LidarDetectedObject currentObject = new LidarDetectedObject();;
List <PolarPointRssi> BalisePointList = new List <PolarPointRssi>();
////Détection des objets ayant un RSSI dans un intervalle correspondant aux catadioptres utilisés et proches
////double maxRssiCatadioptre = 90;
//double minRssiCatadioptre = 60;
//var selectedPoints = ptList.Where(p => (p.Rssi >= minRssiCatadioptre) && (p.Distance < distanceMax));
//List<PolarPointRssi> balisesPointsList = (List<PolarPointRssi>)selectedPoints.ToList();
//On détecte les max de RSSI supérieurs à un minRssiBalise
List <int> maxRssiIndexList = new List <int>();
if (ptList.Count() > 0)
{
for (int i = 1; i < ptList.Count - 1; i++)
{
if (ptList[i].Rssi >= ptList[i - 1].Rssi && ptList[i].Rssi > ptList[i + 1].Rssi && ptList[i].Rssi > minRssiBalise)
{
maxRssiIndexList.Add(i);
}
}
//Gestion des cas de bord de tableau pour ne pas avoir de zone morte
if (ptList[0].Rssi >= ptList[ptList.Count - 1].Rssi && ptList[0].Rssi > ptList[1].Rssi && ptList[0].Rssi > minRssiBalise)
{
maxRssiIndexList.Add(0);
}
if (ptList[ptList.Count - 1].Rssi >= ptList[ptList.Count - 2].Rssi && ptList[ptList.Count - 1].Rssi > ptList[0].Rssi && ptList[ptList.Count - 1].Rssi > minRssiBalise)
{
maxRssiIndexList.Add(ptList.Count - 1);
}
}
//ON génère la liste des points des balises pour affichage de debug uniquement
BalisePointList = new List <PolarPointRssi>();
for (int i = 0; i < ptList.Count; i++)
{
PolarPointRssi pt = new PolarPointRssi(ptList[i].Angle, 0, ptList[i].Rssi);
BalisePointList.Add(pt);
}
//On regarde la taille des objets autour des max de Rssi
double seuilSaillance = 0.2;
foreach (int indexPicRssi in maxRssiIndexList)
{
//On cherche à détecter les fronts montants de distance (distance qui augmente brutalement) autour des pics pour déterminer la taille des objets
//On définit une taille max de recherche correspondant à 5 fois la taille d'une balise
double tailleAngulaireBalisePotentielle = 0.15 / ptList[indexPicRssi].Distance;
double incrementAngleLidar = ptList[1].Angle - ptList[0].Angle;
//Défini la fenetre de recherche
int indexSearchWindow = (int)(tailleAngulaireBalisePotentielle / incrementAngleLidar);
int indexFrontMontantAngleSup = -1;
int indexFrontMontantAngleInf = -1;
//Détection des fronts montants coté des angles inférieurs à l'angle central
int index = indexPicRssi;
int indexShift = 0; //Pour gérer les balises situées à l'angle 0
while (index > indexPicRssi - indexSearchWindow)
{
if (index - 1 < 0)
{
indexShift = ptList.Count(); //Gestion des bords de tableau
}
BalisePointList[index + indexShift - 1].Distance = 5; //For debug display
if (Math.Abs(ptList[index + indexShift - 1].Distance - ptList[indexPicRssi].Distance) > seuilSaillance)
{
//On a un front montant coté des angles inférieurs à l'angle central
indexFrontMontantAngleInf = index + indexShift - 1;
break;
}
index--;
}
//Détection des fronts montants coté des angles supérieurs à l'angle central
index = indexPicRssi;
indexShift = 0; //Pour gérer les balises situées à l'angle 0
while (index < indexPicRssi + indexSearchWindow)
{
if (index + 1 >= ptList.Count)
{
indexShift = -ptList.Count(); //Gestion des bords de tableau
}
BalisePointList[index + indexShift + 1].Distance = 6; //For debug display
if (Math.Abs(ptList[index + indexShift + 1].Distance - ptList[indexPicRssi].Distance) > seuilSaillance)
{
//On a un front montant coté des angles supérieurs à l'angle central
indexFrontMontantAngleSup = index + indexShift + 1;
break;
}
index++;
}
if (indexFrontMontantAngleInf >= 0 && indexFrontMontantAngleSup >= 0)
{
//On a deux fronts montants de part et d'autre du pic, on regarde la taille de l'objet
double tailleObjet = (ptList[indexFrontMontantAngleSup].Angle - ptList[indexFrontMontantAngleInf].Angle)
* (ptList[indexFrontMontantAngleSup].Distance + ptList[indexFrontMontantAngleInf].Distance + ptList[indexPicRssi].Distance) / 3;
//if(tailleObjet>0.05&& tailleObjet<0.25)
{
//On a probablement un catadioptre de type balise Eurobot !
currentObject = new LidarDetectedObject();
if (indexFrontMontantAngleInf < indexFrontMontantAngleSup)
{
for (int i = indexFrontMontantAngleInf + 1; i < indexFrontMontantAngleSup - 1; i++) //Décalages pour éviter de mettre une distance erronnée dans les amas de points
{
currentObject.PtList.Add(ptList[i]);
BalisePointList[i].Distance = 10; //For debug display
}
currentObject.ExtractObjectAttributes();
BalisesCatadioptriquesList.Add(currentObject);
}
else //Gestion des objets coupés en deux par l'angle 0
{
for (int i = indexFrontMontantAngleInf + 1; i < ptList.Count; i++) //Décalages pour éviter de mettre une distance erronnée dans les amas de points
{
currentObject.PtList.Add(ptList[i]);
BalisePointList[i].Distance = 10; //For debug display
}
for (int i = 0; i < indexFrontMontantAngleSup - 1; i++) //Décalages pour éviter de mettre une distance erronnée dans les amas de points
{
currentObject.PtList.Add(new PolarPointRssi(ptList[i].Angle + 2 * Math.PI, ptList[i].Distance, ptList[i].Rssi));
BalisePointList[i].Distance = 10; //For debug display
}
currentObject.ExtractObjectAttributes();
BalisesCatadioptriquesList.Add(currentObject);
}
}
}
}
OnLidarBalisePointListForDebug(robotId, BalisePointList);
return(BalisesCatadioptriquesList);
}