4th Tensilica Day23st and 24nd of September 2019

Ego Motion Estimation

Christoph Heinrichs, Martin Köhler, Meinolf Blawat

Dream Chip Technologies GmbH

Dream Chip Technologies GmbHConfidential – For Internal Use Only

Usage of Ego Motion Estimation: iFUSE Project

Input

DataiFusion

Processing

Sensor

Data 1

ProcessingSensor

Data 2

Processing

Sensor Data 2

Output

Data

Lidar

Radar

Camera

Video data pre processing: Stereo depth maps, …, monocular Ego-Motion calculation, …

iFUSE Project GoalAutonomous driving through a test crossroad.

Monocular ego motion calculation is used to enhance camera data confidence

SDIP SoC (Things2Do Project)

ARM A53 IVPTensilicaVP6

Overview – Ego Motion Estimation

ego motion

structure

motion

Structure from motion

motion

structure

Ground truth Reference model

Kitti dataset

Image t Image t+1

iFUSE Hardware Block-DiagramiFUSESensoren

Bosch

Radar

2x DCT

Atom

Kameras

Synchron. &

Preproc.

Radar

Synchron. & Preproc. Kamera

dSPACE AD PC (Ubuntu, ROS 1)

SfM,

Disparitäts-

schätzung

iFUSE

Low

Level

Fusion

Umfeld-/

Szenen-

modell

Situations-

analyse &

Handlungs-

planung

dSPACE Autobox (Matlab Simulink, Stateflow, Embedded Matlab, C)

iFUSE Fahrfunktion

Ibeo Lux

Laser-

scanner

Synchron. & Preproc.

Laser

Trajektorien-

planung

Längs/

Quer-

Regelung

Lenkung

Triebstrang

Bremse

3rd Party Sensoren

GenesysADMA DGPS

VW CAN-Bus

(ESP…)

wavebee

C2X

Ego Motion

Estimation

iFUSE Perzeptionssystem

Objektbildung

& Tracking

Gridmap &

Freiraumerk.

Lokalisierung/ SLAM

Licht

Demonstrator mit Aktoren

Ego Motion Estimation

• Reference Algorithm

• Evaluation of various egomotion algorithms

• ORB (Oriented FAST and Rotated BRIEF) feature extraction

• Optical flow added to enhance feature extraction

• Hierarchical decomposition of frames forscaling invariance of features

• Matching of extracted features in secutive frames

• Ego motion vector calculation using „feature matching vectors“

• Essential Matrix and create translation and rotation matrix

Feature detector

Image n Descriptors

Matching

Feature extration

Image n+1 Descriptors

create tranlation

and rotation matrix

3D ego-motion vector

FAST (Features from Accelerated Segment Test)

BRIEF (Binary Robust Independent Elementary Features)

Ego Motion Estimation

Brief 8b = 10010100

• Corner detector• Faster then Sift and Surf• Compare p with 16 pixel around

• If more then 12 grayscale values brighter corner• If more then 12 grayscale values darker corner

• Compute hamming distance of the descriptors from two following images

• Find minimal match Greedy algorithm

• Motion vectors

• Find essential matrix 5-Point algorithm• Find rotation and tranlation matrix

Ego motion vector

SDIP

TensilicaVision P6- L1:

512kB - L2: 2MB

TensilicaVision P6- L1:

512kB - L2: 2MB

TensilicaVision P6- L1:

512kB - L2: 2MB

TensilicaVision P6- L1:

512kB - L2: 2MB

Display -motionvectors

AtomOne

Memory(DDR)

MVDUVIU

ARM Cortex

A53

ARM Cortex

A53

ARM Cortex

A53

ARM Cortex

A53

SDIP SoC (Things2Do Project)

• VIU: Video input unit• MVDU: Multistream video display unit

• Arm Cortex A53

• Tesnilica Vision P6• Level 1 Cache, 2x 256kB• Level 2 Cache, 2x 1MB

Algorithm: Ego Motion Estimation

Tensilica VP6 Arm Cortex A53

Fast feature detector

Image nBRIEF 64B Descriptors

MatchingFAST

feature extration

Image n+1BRIEF 64B Descriptors

create tranlation

and rotation matrix

3D ego-motion vector

• XI Library:• FAST, BRIEF

• Vector types 512b width• 64x int 8b, 32x int 16b, 16x int 32b

• Instruction set for the Vision P6 to compute vector types• One descriptor can be stored in one vector register

(64B descritpor)• Only save matches to shared memory (DDR)

Store matches n-1

Load tilen+1

Compute tile n

FlexNoc

Tenilica Vision P6

L2 CacheBank 0: 1MBBank 1: 1MB

L1 CacheBank 0: 256kBBank 1: 256kB

Implementation on Tensilica Vision P6

Store matches n-1

Load tilen+1

Compute tile n

FlexNoc

Tenilica Vision P6

L2 CacheBank 0: 1MBBank 1: 1MB

L1 CacheBank 0: 256kBBank 1: 256kB

Implementation on Tensilica Vision P6

• Use the DMA • Use different banks to load/store and

process in parallel

Reduce executen time

• Load the tiles in the L1 Cache• Compute FAST and BRIEF• Store all descriptors of the image in the L2

Cache• Match the descriptors from the L2 Cache of

two following images • Store the matches in the DDR

Reduce load and store operations

Store matches n-1

Load tilen+1

Compute tile n

FlexNoc

Tenilica Vision P6

L2 CacheBank 0: 1MBBank 1: 1MB

L1 CacheBank 0: 256kBBank 1: 256kB

Implementation on Tensilica Vision P6

• Split the image in three regions• One region is computed on one core• One Core is for the synchronisation

• No data dependences between the cores• Every image only loaded once

Ego Motion Estimation

• Interfaces

• Input: Mono Grayscale Image, 30fps

• Output: Ego motion vectors

• C model using OpenCV

• Target platform

• SDIP SoC (Things2Do Project)

• ADAS - High performance platform

• 4x Tensilica VP6, 4x Arm Cortex A53