Landmarks

Om meer controle te hebben over welke data er gebruikt wordt voor de positiebepaling is gekozen om landmarks te detecteren in de data uit de LIDAR scanners. Uit analyse van de LIDAR data bleek dat vooral de muren van de kuilwanden en stallen erg goed zichtbaar zijn in de data. Deze goed- gedefinieerde objecten zijn duidelijk herkenbaar in de data; de meetpunten liggen hier op een rij.

Figuur 4.5: Localisatie faalt in deze situatie omdat deze gedaan moet worden zonder sterk gedefinieerde features.

Concept

Van tevoren wordt er een ‘kaart’ gemaakt van de boerderij, deze kaart bestaat uit de locatie van verschil- lende landmarks. Voor de positiebepaling hoeven enkel deze landmarks gedetecteerd te worden. Onder de aanname dat de positie van het voertuig redelijk klopt, kunnen we aannemen dat we redelijk goed weten waar de landmark zich bevind. Hierdoor kunnen we een bounding box creeëren om de landmark heen, vervolgens hoeven we enkel in de pointcloud binnen deze box de landmark te detecteren. Deze pre-processing stap reduceerd het formaat van de pointcloud waarin de landmark gezocht wordt. Omdat we per bounding box één landmark zoeken is de registratie stap eenvoudig, want we weten direct welke gedetecteerde landmark bij welke map landmark hoort. Vervolgens kan er met de bekende positie van de landmark en een berekende positie van de LIDAR data een correctie bepaald worden die de huidige positie corrigeerd dusdanig dat de gemeten landmark overeen komt met de kaart.

Voor het decteren van de landmarks wordt er gebruik gemaakt van Point Cloud Library[72], dit is een library gemaakt om te werken met point clouds, dus in feitte veel data punten in een 3D ruimte. Deze library is uitermate geschikt voor het verwerken van LIDAR data of informatie van RGBD-camera’s zoals een Kinect. Deze library maakt het (relatief ) eenvoudig om delen van de point cloud eruit te knippen (de bounding box), en methoden voor registratie en feature detectie zijn beschikbaar. Het gebruik van deze library maakt het mogelijk om met relatief weinig code complexe algoritmen te gebruiken. Indien een algoritme niet beschikbaar is in de library is het mogelijk om deze op eenvoudige wijze zelf te implementeren, eenPointCloudis immers niets anders dan eenstd::vectorvanPoints.

Om op een gebruikersviendelijke manier de landmarks te kunnen plaatsen en manipuleren is er gekozen om gebruik te maken van hetInteractiveMarkersysteem in ROS. Dit maakt het mogelijk om markers zoals kubbussen, lijnen en spheres inrvizweer te geven. Voor deze markers kunnen vrijheidsgraden vrijgegeven worden en bij verplaatsing of clicks of andere user interaction wordt er een feedback callback aangeroepen. Bij verplaatsing bijvoorbeeld kan deze callback de interne staat updaten om zodoende de verplaatsing inrvizweer te representeren.

Figuur 4.6: Inclusion graph of landmark.h, it shows the various dependencies of parts of

pcl_landmark.

Architecture & implementation

Aangezien het landmark systeem waarschijnlijk uigebreid zal worden in de toekomst is het van belang dat dit een flexibele en uitbreidbare software opzet heeft. Een poging tot een dusdanig systeem is gedaan, hierbij zijn de volgende overwegingen in acht genomen:

• Het systeem moet uitbreidbaar zijn voor andere typen landmarks. • GUI voor het plaatsen / modificeren van de landmarks.

• De landmark detectie zelf mag geen dependency hebben op de GUI.

• Het landmark systeem moet een scheiding hebben tussen detectie en de positiecorrectie. • Landmarks moeten indien beschikbaar een pose met covariance opleveren.

Met deze overwegingen komt men bij gebruik van object-oriented-programming al snel tot de volgende klassen:

• Voor elk type landmark een aparte class die de functionaliteit van het detecteren omvat. • Base class van het typeLandmarkdie de gedeelde functies bevat.

• EenLandmarkManagerom meerdereLandmarks bij te houden en ze hun detectie-stap te laten uitvoeren.

• Een GUI klasse voor zowel deLandmarkals deLandmarkManager.

Om geen dependency te hebben vanuit het landmark systeem op de GUI zal op sommige plekken gebruik gemaakt moeten worden van een callback in plaats van een referentie naar het object. In Figure 4.6 is de inclusion graph te zien vanlandmark.h, deze if afkomstig uit de automatisch gegene- reerde documentatie[88].

Wat volgt is een korte beschrijving van de verschillende onderdelen van dePCL_landmarkpackage. Landmark Is de abstract base-class van deLandmark’s. Elke landmark heeft een type, frame en unique identifier.

Figuur 4.7: Editing GUI of the landmark, showing the bounding box and movement / rotation markers.

LandmarkLine Representeerd een line landmark, gebruikt PCL’s RANSAC methoden om een lijn te vinden, voor de RANSAC wordt de pointcloud gereduceerd door alleen het stuk in de bounding box te selecteren. Door gebruik te maken van depcl::SampleConsensusModelParallelLineis ook de richting van de lijn binnen een bepaalde hoek vastgelegd. Indien er meer dan x punten voldoen aan het RANSAC model wordt de landmark als ‘gedetecteerd’ beschouwd. De interne representatie van de landmark is eenEigen::ParametrizedLine, dus een lijn uit een oorsprong met een bepaalde richting.

LandmarkManager Deze klasse is in feitte de ROS node, in de initialisatie worden de topics gesub- scribed en de parameters opgehaald. Deze klasse gebruikt de service van deLaserAssemblerom een pointcloud op te halen en roept op elk van deLandmarkobjecten in zijn lijst dedetectmethode aan. Vervolgens wordt voor elke landmark dietruereturnde op dedetectmethode een message gepublished op het detectiontopic.

LandmarkManagerGUI PublishedMarkerArraymessages voor deLandmarks, daarnaast hookt deze zich aan de post-detection callback van deLandmarkManagerdie vervolgens de GUI van elke losseLandmarkde update methode aanroept.

LandmarkGUILine Handelt de visualisatie en modificatie van eenLandmarkLineklasse af. Dit staat manipulatie van de positie / orientatie en bounding box corners toe, deze GUI is te zien in Figure 4.7.

landmark_manager_gui_node Dit is de ROS node die zowel deLandmarkManageren de

LandmarkManagerGUIinstantieerd. Verder laad deze node een bestand wat landmarks kan bevatten, voegt hij optioneel landmarks toe aan dit bestand en bij het sluiten wordt het bestand bijgewerkt.

Map creation

Het plaatsen van de landmarks kan het makkelijkst door een kaart die gemaakt is met OctoMap in ROS te laden en weer te geven inrviz, vervolgens kunnen de Landmarks op de juiste plek geplaatst worden.

Door de GUI node te gebruiken worden er realtime markers geplot en is het ook mogelijk om de point cloud uit de bounding box te visualiseren. Het correct positioneren van deLandmarksis momenteel echter handwerk, gezien het geringe aantal en de eenvoud waarmee dit gedaan wordt is deze taak momenteel te overzien.

Op termijn is het uiteraard wenselijk om dit te automatiseren en ook de landmarks zelf automatisch te detecteren. In de toekomst zal er toch ook nog een redelijke hoeveelheid met de hand ingetekend moeten worden, bijvoorbeeld om aan te geven waar de kuilen te vinden zijn, of waar de INNOVADOzich moet positioneren om additieven van een silo in te nemen. De geschrevenInteractiveMarkercode kan hiervoor ook gebruikt worden.

Voor het opslaan van de kaart metLandmarks wordt er gebruik gemaakt van Boost’s Serialization module[4]. Deboost::serializationis in staat om een vector pointers naarLandmarkobjecten te serializen en deserializen2. Het mooie hieraan is dat de serialization methode in elkeLandmarkclass zelf gedefinieerd is en zodoende er geen aparte serialization functies nodig zijn die alle setters en getters aanroepen. Ook gaat deze library graceful om met verschillendeLandmarktype; ook als serialization aangeroepen wordt op eenvectorvanLandmark*worden deze als het juiste type ge-serialized en de-serialized.

Quickstart

Voor het gemak is hier een korte beschrijving bijgeleverd hoe het huidige localisatiesysteem in ROS opgestart kan worden en de resultaten verkregen zijn.

1 roscore

2 # start a new terminal in which we will launch the localization.

3 rosparam set use_sim_time true # ensure we use a simulated time.

4 cd test_pcl_landmark/workspace/ # navigate to the workspace

5 catkin build # build all modules.

6 source devel/setup.bash # ensure buid result is in path.

7 roslaunch pcl_landmark start_landmark_localization.launch # start localization

8 # start a new terminal to launch rviz in.

9 cd test_pcl_landmark/workspace/src/pcl_landmark/rviz

10 rviz −d display_pcl.rviz # start rviz with sensible visualisation.

11 # start a new terminal and play a bagfile.

12 rosbag play ₋₋clock 2016₋02₋04₋10₋59₋35.bag # play a recording at Vrielink

Voor uitgebreidere informatie hoe de verschillende functionaliteit onder verdeeld is in de ROS launchfi- les en dergelijke is het handigst te verwijzen naar deze launchfile, deze include namelijk verscheidene andere launchfiles waardoor er een duidelijke scheiding van functionaliteit is.