We zijn nog niet klaar met point clouds

Als je maar lang genoeg wacht om een probleem op te lossen, lost iemand anders het misschien wel voor je op. Dat is in ieder geval wat ik mezelf vertel over de afwas in mijn gootsteen.

Tijdens een conferentie in Wenen stuitte ik op een groep onderzoekers die precies dat voor mij hebben gedaan: ze namen werk dat ik twee jaar geleden had gepubliceerd, bouwden daarop voort en versloegen me op vrijwel elke benchmark. Ik schrijf dit artikel om aandacht te vragen voor hun werk en, een beetje egoïstisch, om te vertellen over de dingen die ik heb geleerd bij het opnieuw implementeren van hun methode.

Kort gezegd hebben zij een datastructuur gemaakt voor botsingsdetectie (collision-checking) tegen point clouds (puntenwolken) die zeer snel werkt en bovendien extreem efficiënt is in termen van geheugengebruik en constructietijd. Voor wie niet geïnteresseerd is in de details: u kunt direct naar het paper of de originele C++-implementatie gaan. Ik heb daarnaast een Rust-implementatie gepubliceerd met eigen optimalisaties, waarvan de broncode op GitHub staat en het pakket op crates.io beschikbaar is.

De basis van bewegingsplanning

Ik besteed veel tijd aan het nadenken over motion planning: het vinden van manieren voor robots om botsingsvrije bewegingen te maken van een starttoestand naar een doeltoestand. Er zijn talloze manieren om bewegingsplanningsproblemen op te lossen, maar zodra je genoeg papers hebt gelezen, lijken ze allemaal op elkaar: je samplet enkele configuraties, test of deze geldig zijn en voert een uitgebreide padzoekopdracht uit over alle mogelijke configuraties.

Elk van deze algoritmen vereist configuratievalidatie: gegeven de configuratie van een robot moet worden bepaald of een robot in die positie botst met de geometrie van de wereld. Aangezien robots vaak werken in waargenomen omgevingen, wordt die wereldgeometrie meestal aangeleverd als een point cloud.

Als we de geometrie van onze robot vereenvoudigen tot een verzameling sferen (bollen), kunnen we het probleem verder reduceren tot spherical collision checking: controleer voor elke configuratie simpelweg of een van de sferen op de robot botst met de waargenomen point cloud.

Probleemstelling: Gegeven een lijst van $N$ punten $P$ en een set sferen $S$, bepaal in minimale tijd of een willekeurige sfeer in $S$ botst met $P$.

Een paar jaar geleden stelde ik een datastructuur voor genaamd de CAPT, die is ontworpen om configuratievalidatie tegen point clouds zeer snel te maken. In feite is het een botsingsdetector tussen sferen en point clouds. Het is een nearest-neighbor search-structuur, vergelijkbaar met een $k$-d tree, maar we doen extra werk tijdens de constructie om het terugkeren (backtracking) door de zoekboom te voorkomen. Het resultaat is een $k$-d tree met een batch-parallel zoekalgoritme dat SIMD-versnelde branchless queries ondersteunt.

Het grote probleem met CAPT's was echter de constructietijd: dichte point clouds vereisen veel gedupliceerde data om backtracking te voorkomen. Zodra point clouds een bepaalde dichtheid bereiken, schaalt de constructie van CAPT naar $O(N^2)$, wat rampzalig is voor gebruikers die planning op control-loop frequenties willen realiseren. De datalayout van CAPT's vereist dat elk blad van de zoekboom (dat een bepaald gebied in de ruimte vertegenwoordigt) duplicate kopieën opslaat van veel punten uit de point cloud. Deze duplicaten gaan domineren in het geheugenbeslag, wat op zijn beurt de constructietijd enorm doet toenemen.

Denken vanuit voxels

De onderzoekers Ching Chen en Tsung-Tai Yeh besloten deze problemen met CAPT's voor zichzelf op te lossen. Om dat te doen, stapten ze volledig af van nearest-neighbor search trees. In plaats van een ruimteverdelende boom, verdelen zij de ruimte in een raster van voxels, waarbij elke voxel een lijst opslaat van de punten die zich daarin bevinden.

Dit biedt twee voordelen:

1. Met eenvoudige rekenkunde kun je bepalen in welke voxel een query-sfeer zich bevindt.
2. Er hoeven geen punten gedupliceerd te worden, aangezien het vinden van aangrenzende voxels triviaal is.

Het naïef opslaan van elke voxel in de werkruimte werkt echter niet. Als de werkruimte honderd voxels lang is in elke dimensie, zou je informatie voor een miljoen voxels moeten opslaan om één point cloud te registreren die, na filtering, misschien maar een paar duizend punten bevat.

Om dit beheersbaar te houden, slaan Chen en Yeh alleen bezette voxels sparse op in een drie-laagse sparse tree, waarbij elke laag is gesegmenteerd per dimensie. Samen met een aantal axis-aligned bounding box (AABB) tests vormt dit de multilevel voxel table, of MVT. Net als de CAPT kunnen MVTs geparallelliseerd worden met single-instruction, multiple-data (SIMD). Voor elke gegeven voxel kan de botsingsdetector een grote batchcontrole uitvoeren voor alle punten in die voxel, wat zorgt voor een gratis snelheidsverbetering.

Verbeteringen in de implementatie

De originele implementatie van MVT's bevatte enkele complexe C++-constructies: de voxel-tabellen maakten gebruik van een kluwen aan pointers naar elke rij tabellen. Dit maakte het geheugenbeheer lastig en was niet efficiënt qua grootte. Bovendien gebruikte de originele C++-implementatie handmatig poolbeheer, wat leidde tot crashes zodra point clouds te groot werden.

struct MVT {
    // pointers naar voxel indices
    using ZLevelTable = uint32_t*;
    // pointers naar z-level tabellen
    using YLevelTable = uint32_t**;
    // pointers naar y-level tabellen
    using XLevelTable = uint32_t***;
    XLevelTable x_level_table;
}

Om de implementatie in Rust te vereenvoudigen, heb ik dit aangepast: alles wordt nu ondersteund door een Box<[]>.

struct Mvt {
    /// vertelt ons waar we voxel-data kunnen vinden vanuit een grid-index
    tables: Box<[u32]>,
    /// vertelt ons waar we puntlijsten in `points` kunnen vinden
    voxels: Box<[u32]>,
    /// afgeplatte SoA shared buffer van alle puntdata
    points: [Box<[f32]>; 3]
    // andere velden...
}

struct Voxel {
    /// index van het eerste punt dat in deze voxel is opgeslagen in `points`
    offset: u32,
    /// aantal punten in de voxel
    count: u32,
    /// andere velden...
}

De zoeklogica wordt hierdoor zeer eenvoudig: gebruik de tabellen om te bepalen bij welke voxel je hoort, zoek deze op in voxels, en gebruik vervolgens die voxel om het bereik van punten te vinden waartegen een brute-force controle moet worden uitgevoerd.

Mutable MVTs

Naast het vereenvoudigen van de zoeklogica, maakt deze nieuwe structuur het triviaal om MVT's aanpasbaar (mutable) te maken door elke Voxel zijn eigen points-veld te geven in plaats van één grote gedeelde buffer.

// `points` is verwijderd uit `Mvt`
struct Voxel {
    /// index van het eerste punt dat in deze voxel is opgeslagen in `points`
    offset: u32,
    /// aantal punten in de voxel
    count: u32,
    /// SoA buffer van punten in deze voxel
    points: [Vec<[f32]>; 3]
    // andere velden...
}

Het toevoegen van mutabiliteit kost ongeveer een factor 2 in grootte en een factor 1,5 in constructietijd, maar het is een waardevolle functie. Voor gebruikers die MVT's als een eenmalige structuur gebruiken, heb ik de implementatie gesplitst in een standaard onveranderlijke Mvt en een MutableMvt.

De keuze van de voxelgrootte

Om een MVT te bouwen, moet je bepalen hoe groot de voxels moeten zijn. Als voxels te groot zijn, verspilt de botsingsdetectie teveel tijd aan het doorzoeken van ver weg gelegen punten. Zijn ze echter te klein, dan moet de query filteren tegen tientallen kleine voxels.

Er zijn een paar logische kandidaten voor de breedte:

• $r_{max}$: de grootste sfeer van de robotgeometrie.
• $r_{link}$: alleen de bewegende schakels van de robot (zonder de grote sferen van de basis).
• $r_{BVH}$: de grootte van de grootste sfeer die ooit wordt gebruikt in een botsingscontrole binnen de bounding-volume hierarchy.

Het originele MVT-paper adviseerde om $r_{max}$ te gebruiken, voornamelijk op basis van de claim dat de prestaties daarmee "goed genoeg" waren. Ik wilde echter een preciezer antwoord en heb dit empirisch onderzocht via een parameter sweep over de voxelbreedte voor verschillende robots (Fetch, Panda, UR5 en Baxter).

Voor Fetch, Panda en UR5 is $r{max}$ inderdaad een redelijke keuze, maar niet optimaal. Voor de Baxter-robot bleek het gebruik van $r{max}$ echter extreem traag; de querytijden waren twintig keer trager dan bij een optimale selectie. Ik vermoed dat de originele auteurs nooit benchmarks hebben uitgevoerd met Baxter.

Verrassend genoeg ligt de optimale voxelbreedte voor alle robots altijd tussen de 10 en 20 cm. Ik vermoed dat dit een gevolg is van het filterproces van de point cloud: bij een bepaalde dichtheid is er een optimaal voxelformaat om verspilde arbeid te minimaliseren.

Prestatiebenchmarks

Om te bepalen of de code echt snel is, heb ik benchmarks opgezet waarbij ik bewegingsplanningsproblemen oploste, alle gemaakte botsingscontroles registreerde en deze vervolgens opnieuw afspeelde om de doorvoer (throughput) te meten. Ik vergeleek mijn Rust-MVT, de originele C++ MVT, mijn oude CAPT-implementatie en een zeer snelle $k$-d tree (genaamd 'kiddo').

Constructietijd

De grootste winst zit in de constructietijd. CAPT's waren altijd traag om te bouwen, zeker in de Rust-implementatie; dit was vaak de langzaamste stap in de gehele pipeline. Omdat MVT's veel minder administratie vereisen tijdens de constructie, scoren ze hier zeer goed. Bovendien heeft de MVT een lineaire geheugenschaal, waardoor de constructietijd $O(N)$ is bij point cloud grootte $N$, in tegenstelling tot de exponentiële schaling van ruimteverdelende bomen. Zelfs de mutable MVT's blijven relatief goedkoop om te bouwen.

Query-doorvoer

MVTs hebben ook een uitstekende query-doorvoer. Waar ik voorheen trots was op de doorvoer in de orde van tien nanoseconden voor CAPT's, doen MVTs het nog beter. Opvallend genoeg lijken mutable MVT's marginaal betere prestaties te leveren dan immutable versies, waarschijnlijk door een eigenaardigheid van het cachegeheugen: het opslaan van punten in aparte allocaties lijkt te helpen.

End-to-end planning

Bij het oplossen van echte planningsproblemen zorgen MVT's over de hele linie voor een aanzienlijke versnelling. In bijna elk scenario is planning met de MVT point cloud representatie sneller dan met CAPT, en vaak zelfs sneller dan wanneer er gebruik wordt gemaakt van de exacte geometrie (ground-truth primitive geometry).

De onopgeloste problemen

MVT's zijn fantastische datastructuren: ze zijn snel, efficiënt en eenvoudig te beheren. Ze lossen echter slechts één van de grote problemen met CAPT's op, namelijk de constructietijd.

Point cloud data is noodzakelijkerwijs imperfect; een camera kan immers altijd maar één kant van een object zien. Bij planning voor echte robots moeten we hier rekening mee houden, meestal door aan te nemen dat onzichtbare ruimte ook ongeldig (bezet) is. Eerdere werken over perceptiedata, zoals Octomaps, kunnen dit doen, maar ik had in het CAPT-werk occlusion-handling opgeofferd op het altaar van prestaties.

Gebruikers staan nu voor een harde keuze: veilig zijn of snel zijn. Octomaps zijn extreem traag, maar tot nu toe de enige die occlusion correct behandelen. In bredere zin denk ik dat de klassieke formulering van bewegingsplanning — waarbij een robot opereert in een prachtig, stilstaand schilderij van de wereld — simpelweg incorrect is. Zelfs de beste perceptie voor robots is nooit beter dan "matig", en onze planningsbenaderingen zouden moeten omgaan met het feit dat we altijd plannen tegen een benadering van de echte wereld.

Dus, net als mijn afwas, blijft planning vanuit perceptie nog onopgelost.