DE102011075335A1

DE102011075335A1 - Verfahren zur rechnergestützten Lageschätzung eines Objekts

Info

Publication number: DE102011075335A1
Application number: DE102011075335A
Authority: DE
Inventors: Wendelin Feiten; Thilo Grundmann
Original assignee: Siemens AG
Current assignee: Siemens AG
Priority date: 2011-05-05
Filing date: 2011-05-05
Publication date: 2012-11-08
Also published as: WO2012150150A1

Abstract

Die Erfindung betrifft ein Verfahren zur rechnergestützten Lageschätzung eines Objekts. In dem Verfahren wird eine Modell-Datenbank verwendet, welche eine Vielzahl von Merkmalen (m) des Objekts (O) enthält, wobei jedem Merkmal (m) eine lokale Position (x) auf dem Objekt zugeordnet ist. Im erfindungsgemäßen Verfahren wird zunächst basierend auf mit einem Kamerasystem (C) aufgenommenen Kamerabildern (B) des Objekts (O) eine Hypothese für eine Objektpose des in den Kamerabildern enthaltenen Objekts (O) ermittelt. Anschließend wird basierend auf einem Sensormodell des Kamerasystems und der Hypothese für die Objektpose über die Minimierung einer Fehlerfunktion (ferr) eine verbesserte Schätzung der Objektpose erhalten. Schließlich wird mittels einer Funktion (f), welche über die Minimierung der Fehlerfunktion (ferr) implizit definiert ist, eine Wahrscheinlichkeitsverteilung (p) der geschätzten Objektpose ermittelt. Das erfindungsgemäße Verfahren zeichnet sich dadurch aus, dass eine genaue Schätzung einer Objektpose zusammen mit einem Maß für die Unsicherheit dieser Schätzung in der Form einer Wahrscheinlichkeitsverteilung erhalten wird. Diese Wahrscheinlichkeitsverteilung kann beispielsweise dazu benutzt werden, um geschätzte Objektpositionen zu verwerfen, wenn die Streuung der Wahrscheinlichkeitsverteilung zu groß ist. Ein Anwendungsbereich des erfindungsgemäßen Verfahrens ist dessen Verwendung in einem Roboter, der seine Bewegungen basierend auf der geschätzten Objektpose durchführt. Beispielsweise kann der Roboter das entsprechende Objekt greifen und an eine andere Position bewegen.

Description

Die Erfindung betrifft ein Verfahren zur rechnergestützten Lageschätzung eines Objekts sowie eine entsprechende Vorrichtung und ein Computerprogrammprodukt.
In einer Vielzahl von Anwendungsgebieten ist eine maschinell durchgeführte genaue Lokalisation eines Objekts erforderlich. Beispielsweise muss in robotischen Anwendungen sichergestellt werden, dass ein entsprechender Roboter seine Umgebung genau manipuliert, um z.B. bestimmte Objekte zu bearbeiten bzw. zu bewegen.
Aus dem Stand der Technik ist eine Vielzahl von Verfahren bekannt, mit denen die Lage bzw. Pose eines Objekts rechnergestützt geschätzt wird. Unter Pose ist dabei die Kombination von Position und Orientierung des Objekts zu verstehen. Im Rahmen einer dreidimensionalen Lokalisierung umfasst eine Pose eine 3D-Position und 3D-Orientierung und somit insgesamt 6 Dimensionen. Bekannte Verfahren zur Lageschätzung werten mit einem entsprechenden Kamerasystem aufgenommene Bilder aus, um hierdurch die darin enthaltenen Objekte zu identifizieren und deren Lage zu bestimmen. Solche Verfahren liefern zwar genaue Ergebnisse, jedoch wird durch die Verfahren nicht angegeben, mit welcher Unsicherheit die entsprechend ermittelte Objektpose behaftet ist. Diese Information kann jedoch in bestimmten Anwendungen hilfreich sein. Beispielsweise kann bei einer zu großen Unsicherheit der Schätzung die geschätzte Pose verworfen werden bzw. weitere Daten zur genauen Positionsschätzung akquiriert werden.
Aufgabe der Erfindung ist es deshalb, ein Verfahren zur rechnergestützten Lageschätzung eines Objekts zu schaffen, welches neben einer genauen Objektpose ferner auch ein Maß für die Unsicherheit der geschätzten Objektpose angibt.
Diese Aufgabe wird durch die unabhängigen Patentansprüche gelöst. Weiterbildungen der Erfindung sind in den abhängigen Ansprüchen definiert.
Das erfindungsgemäße Verfahren zur rechnergestützten Lageschätzung eines Objekts verwendet eine Modell-Datenbank, welche eine Vielzahl von Merkmalen des Objekts enthält, wobei jedem Merkmal eine lokale Position auf dem Objekt und eine erste Kovarianzmatrix für die lokale Position zugeordnet ist. Die Modell-Datenbank wird als vorgegeben angesehen und deren Generierung ist nicht Bestandteil des erfindungsgemäßen Verfahrens. Es ist dabei aus dem Stand der Technik bekannt, entsprechende Modell-Datenbanken zur Lageschätzung eines Objekts zu erzeugen.
Im Rahmen des erfindungsgemäßen Verfahrens werden in einem Schritt a) Merkmale des Objekts, welche aus einem oder mehreren, mit einem Kamerasystem aufgenommenen Kamerabildern des Objekts extrahiert werden, mit Merkmalen des Objekts aus der Modell-Datenbank verglichen, wodurch eine Hypothese für eine Objektpose in einem Referenzkoordinatensystem erhalten wird. Entsprechende Verfahren zur Durchführung des Schritts a) sind im Stand der Technik beschrieben. Insbesondere findet sich in der Druckschrift [1] ein solches Verfahren. Der gesamte Offenbarungsgehalt dieser Druckschrift wird durch Verweis zum Inhalt der vorliegenden Anmeldung gemacht.
In einem Schritt b) des erfindungsgemäßen Verfahrens wird basierend auf einem Sensormodell des Kamerasystems und der Hypothese für die Objektpose eine Fehlerfunktion in Abhängigkeit von der Objektpose als Variablen minimiert, wodurch eine geschätzte Objektpose erhalten wird. Das Sensormodell beschreibt dabei die Zuordnung zwischen einer lokalen Position auf dem Objekt (d.h. einer Position in einem festen Kommunikationssystem des Objekts) und einer Bildposition im jeweiligen Kamerabild in Abhängigkeit von der Objektpose. Ferner wird durch das Sensormodell eine zweite Kovarianzmatrix für jede Bildposition im jeweiligen Kamerabild definiert. Die Fehlerfunktion beschreibt ein Fehlermaß zwischen über das Sensormodell ermittelten Bildpositionen von im jeweiligen Kamerabild enthaltenen Merkmalen des Objekts und über das Kamerasystem gemessenen Bildpositionen der Merkmale des Objekts. Mit Hilfe von Schritt b) wird eine verbesserte Lageschätzung des Objekts unter Einbeziehung des Sensormodells des Kamerasystems und einer damit verbundener Unsicherheiten erreicht. Die entsprechende zweite Kovarianzmatrix ist dabei vorgegeben und kann für das entsprechende Kamerasystem beispielsweise empirisch bestimmt werden.
Um nunmehr ein Maß für die Unsicherheit der in Schritt b) geschätzten Position zu ermitteln, wird in einem Schritt c) eine Wahrscheinlichkeitsverteilung der geschätzten Objektpose bestimmt. Dies geschieht dadurch, dass mittels einer Funktion, welche über die Minimierung der Fehlerfunktion impliziert definiert ist und die Abhängigkeit der geschätzten Objektpose von den gemessenen Bildpositionen der Merkmale des Objekts beschreibt, eine Jacobi-Matrix für die implizite Funktion mit Hilfe impliziter Differentiation bestimmt wird und hieraus unter Verwendung der ersten und zweiten Kovarianzmatrizen die Wahrscheinlichkeitsverteilung der geschätzten Objektpose ermittelt wird.
Man macht sich erfindungsgemäß die Erkenntnis zunutze, dass mit der an sich bekannten Methode der impliziten Differentiation aus der Minimierung der Fehlerfunktion eine Wahrscheinlichkeitsverteilung für die geschätzte Pose abgeleitet werden kann. Das erfindungsgemäße Verfahren liefert somit neben einer hochgenauen Schätzung der Objektpose als weitere Information ein Maß für die Unsicherheit der geschätzten Objektpose in der Form einer Wahrscheinlichkeitsverteilung.
In einer besonders bevorzugten Ausführungsform des erfindungsgemäßen Verfahrens ist die zu minimierende Fehlerfunktion eine Summe aus Distanzmaßen, wobei ein jeweiliges Distanzmaß den Unterschied zwischen einer über das Sensormodell ermittelten Bildposition von einem jeweiligen, im entsprechenden Kamerabild enthaltenen Merkmal des Objekts und der über das Kamerasystem gemessenen Bildposition des Merkmals beschreibt.
In einer weiteren Ausführungsform der Erfindung wird für jede gemessene Bildposition eines im entsprechenden Kamerabild enthaltenen Merkmals eine dritte Kovarianzmatrix aus der ersten und zweiten Kovarianzmatrix ermittelt.
In einer besonders bevorzugten Variante wird die soeben beschriebene dritte Kovarianzmatrix in der Fehlerfunktion berücksichtigt. Dabei ist die Fehlerfunktion eine gewichtete Summe der Distanzmaße, wobei ein Distanzmaß derart von der dritten Kovarianzmatrix der zum jeweiligen Distanzmaß gehörigen gemessenen Bildposition abhängt, dass das Distanzmaß um so weniger gewichtet wird, je größer die durch die dritte Kovarianzmatrix beschriebene Streuung ist.
In einer besonders bevorzugten Ausführungsform des erfindungsgemäßen Verfahrens wird aus den dritten Kovarianzmatrizen und der Jacobi-Matrix mittels einer Matrixmultiplikation eine Kovarianzmatrix für die Wahrscheinlichkeitsverteilung der geschätzten Objektpose ermittelt. Insbesondere wird dabei eine Block-Diagonalmatrix aus allen dritten Kovarianzmatrizen gebildet und diese Matrix mit der Jacobi-Matrix sowie der Transponierten der Jacobi-Matrix multipliziert.
In einer weiteren Ausgestaltung der Erfindung wird die Wahrscheinlichkeitsverteilung der geschätzten Objektpose durch eine Gauß-Verteilung mit der geschätzten Objektpose als Mittelwert beschrieben. Insbesondere wird dabei als Kovarianzmatrix die oben beschriebene Matrix verwendet, welche aus den dritten Kovarianzmatrizen und der Jacobi-Matrix mittels Matrixmultiplikation bestimmt wurde.
Zur Beschreibung des Kamerasystems wird in einer besonders bevorzugten Ausführungsform ein Sensormodell verwendet, welches auf einem Lochkameramodell basiert. Das Lochkameramodell zur Beschreibung von optischen Abbildungen ist dabei hinlänglich aus dem Stand der Technik bekannt und wird nochmals in der detaillierten Beschreibung beschrieben. Dieses Modell ist nicht-linear. In einer bevorzugten Ausführungsform wird deshalb zur Bestimmung der dritten Kovarianzmatrix eine Linearisierung des Lochkameramodells verwendet. In der detaillierten Beschreibung wird dargelegt, wie eine solche Linearisierung zur Bestimmung der dritten Kovarianzmatrix durchgeführt werden kann.
Die Merkmale des Objekts, welche im Rahmen der Lageschätzung betrachtet werden, sind in einer besonders bevorzugten Ausführungsform die hinlänglich aus dem Stand der Technik bekannten SIFT-Merkmale (SIFT = Scale-Invariant Feature Transform).
In einer weiteren bevorzugten Ausführungsform des erfindungsgemäßen Verfahrens werden aus der in Schritt c) ermittelten Wahrscheinlichkeitsverteilung der geschätzten Objektpose mittels Stichprobenentnahme Objektposen entnommen und ein Histogramm aus entnommenen Objektposen ermittelt, welche ein vorgegebenes Schwellwertkriterium erfüllen, wodurch eine neue Wahrscheinlichkeitsverteilung erhalten wird. Auf diese Weise kann eine nochmals genauere Schätzung der Unsicherheit für die Objektpose ermittelt werden. In einer bevorzugten Variante ist das Schwellwertkriterium dabei von dem Wert der Fehlerfunktion für die entnommene Objektpose abhängig. Insbesondere ist das Schwellwertkriterium derart definiert, dass die entnommene Objektpose das Schwellwertkriterium dann erfüllt, wenn die Exponentialfunktion mit dem mit –1 multiplizierten Wert der Fehlerfunktion für die entnommene Objektpose als Exponenten größer als ein vorgegebener Schwellwert ist. Die geeignete Wahl des vorgegebenen Schwellwerts liegt dabei im Rahmen von fachmännischem Handeln.
In einer weiteren bevorzugten Ausführungsform des erfindungsgemäßen Verfahrens wird eine geschätzte Objektpose verworfen, wenn die in Schritt c) ermittelte Wahrscheinlichkeitsverteilung und/oder die durch Stichprobenentnahme ermittelte neue Wahrscheinlichkeitsverteilung eine Streuung aufweisen, welche größer als eine vorbestimmte Schwelle ist. Die geeignete Wahl der vorbestimmten Schwelle liegt dabei im Rahmen von fachmännischem Handeln.
Neben dem oben beschriebenen Verfahren betrifft die Erfindung ferner eine Vorrichtung zur rechnergestützten Lageschätzung eines Objekts, wobei in der Vorrichtung eine Modell-Datenbank gespeichert ist, welche eine Vielzahl von Merkmalen des Objekts enthält, wobei jedem Merkmal eine lokale Position auf dem Objekt und eine erste Kovarianzmatrix für die lokale Position zugeordnet ist, wobei die Vorrichtung ein Kamerasystem zur Aufnahme von Kamerabildern des Objekts sowie eine Rechnereinheit umfasst, wobei die Rechnereinheit derart ausgestaltet ist, dass mit dieser Rechnereinheit das erfindungsgemäße Verfahren bzw. eine oder mehrere Varianten des erfindungsgemäßen Verfahrens durchführbar sind.
Die Erfindung betrifft darüber hinaus einen Roboter, der die erfindungsgemäße Vorrichtung umfasst, wobei der Roboter im Betrieb seine Bewegungen unter Verwendung der mit der Vorrichtung geschätzten Objektposen durchführt.
Die Erfindung betrifft darüber hinaus ein Computerprogrammprodukt mit einem auf einem maschinenlesbaren Träger gespeicherten Programmcode zur Durchführung des erfindungsgemäßen Verfahrens bzw. einer oder mehrerer Varianten des erfindungsgemäßen Verfahrens, wenn das Programm auf einem Rechner abläuft.
Ausführungsbeispiele der Erfindung werden nachfolgend anhand der beigefügten Figuren detailliert beschrieben.
Es zeigen:
1 eine beispielhafte Darstellung eines Objekts, anhand der die Ermittlung von SIFT-Merkmalen verdeutlicht wird;
2 ein Ablaufdiagramm, welches die Bestimmung einer Hypothese für eine Objektposition basierend auf einer Ausführungsform der Erfindung wiedergibt;
3 eine schematische Darstellung, welche die Schätzung einer Objektposition basierend auf der Minimierung einer Fehlerfunktion anhand einer Ausführungsform des erfindungsgemäßen Verfahrens verdeutlicht; und
4 eine schematische Darstellung, welche die mit einer Ausführungsform der Erfindung ermittelte Wahrscheinlichkeitsverteilung sowie eine weitere Wahrscheinlichkeitsverteilung wiedergibt, welche durch Stichprobenentnahme aus der ursprünglichen Wahrscheinlichkeitsverteilung bestimmt wird.
Nachfolgend wird eine Ausführungsform des erfindungsgemäßen Verfahrens basierend auf der Lageschätzung eines Objekts mit Hilfe eines 3D-Kamerasystems beschrieben. Die Objektschätzung kann insbesondere in einer entsprechenden Recheneinheit eines Roboters dazu verwendet werden, um eine möglichst genaue Pose für die Objekte anzugeben, welche dann im Rahmen der Durchführung einer Aufgabe durch den Roboter zu bearbeiten bzw. zu ergreifen sind. In der hier beschriebenen Ausführungsform wird neben der geschätzten Objektpose auch eine Wahrscheinlichkeitsverteilung bestimmt, welche die Unsicherheit der Schätzung der Objektpose berücksichtigt. Bei der Implementierung des Verfahrens in einem Roboter kann dann beispielsweise bei einer zu großen Streuung der Wahrscheinlichkeitsverteilung auf eine Verwendung der Objektpose im Rahmen der durch den Roboter durchgeführten Aktion verzichtet werden bzw. die Aufnahme von weiteren 3D-Kamerabildern zur Ermittlung einer genaueren Schätzung der Objektpose initiiert werden.
Das erfindungsgemäße Verfahrens beruht auf einer sog. modellbasierten Objekterkennungsmethode, bei der in einer Datenbank entsprechende Modelle von einem oder mehreren zu erkennenden Objekten enthalten ist, deren Objektpose, d.h. deren Position und Orientierung im Raum, geschätzt werden kann. Das Modell eines Objekts wird dabei durch eine Vielzahl von sog. SIFT-Merkmalen auf der Oberfläche des Objekts beschrieben (SIFT = Scale-Invariant Feature Transform). SIFT-Merkmale sind hinlänglich aus dem Stand der Technik bekannt und werden deshalb nicht im Detail erläutert. Diese Merkmale wurden für aussagekräftige lokale Positionen auf dem Objekt ermittelt und sind in der Datenbank abgelegt. Verfahren zur Ermittlung von SIFT-Merkmalen von Objekten sind bekannt und nicht Gegenstand der Erfindung. Die Erfindung geht vielmehr davon aus, dass bereits eine entsprechende Modell-Datenbank vorliegt, welche eine Vielzahl von Merkmalen zumindest eines Objekts enthält, dessen Position geschätzt werden soll.
Eine Möglichkeit zur Ermittlung von SIFT-Merkmalen eines Objekts besteht darin, dass das Objekt auf einer drehenden Scheibe angeordnet wird und über ein Stereokamerasystem aus einer Vielzahl von unterschiedlichen Winkeln aufgenommen wird. Über bekannte Software können dann entsprechende SIFT-Merkmale in Kombination mit zugeordneten lokalen Positionen auf dem Objekt (d.h. Positionen, welche in einem lokalen Koordinatensystem des Objekts definiert sind) ermittelt werden.
1 verdeutlicht nochmals anhand eines Objekts in der Form einer Safttüte die Bestimmung der SIFT-Merkmale. Das Objekt O ist im linken Teil der 1 als eine Punktwolke wiedergegeben. Aus diesem Objekt werden für eine Vielzahl von lokalen Positionen x SIFT-Merkmale des Objekts bestimmt, was durch den Pfeil P angedeutet ist. Das im rechten Teil der 1 wiedergegebene Objekt zeigt dabei die Positionen an, an denen auf der Objektoberfläche SIFT-Merkmale ermittelt werden. Man erkennt, dass in bestimmten Bereichen mehr SIFT-Merkmale vorliegen als in anderen.
In 1 ist beispielhaft ein SIFT-Merkmal m angegeben, wobei im Folgenden die SIFT-Merkmale mit dem Index k mit k ∊ [0, K] versehen werden, wobei K die Gesamtanzahl der lokalen Positionen bezeichnen, für welche SIFT-Merkmale ermittelt wurden. Die lokale Position x = {x, y, z} eines SIFT-Merkmals m_k = {x, Σ_x, x ~^ν, s, d} ist dabei eine dreidimensionale Position im Raum, für welche auch eine (erste) Kovarianzmatrix Σ_x existiert. Ferner enthält das SIFT-Merkmal eine Liste von v Sichtlinien x ~^ν, einen an sich bekannten Scale s und den klassischen, ebenfalls bekannten SIFT-Deskriptor d.
In der hier beschriebenen Ausführungsform wird die 3D-Position x über die an sich bekannte Bundler-Software (siehe Dokument [2]) ermittelt, und die entsprechende Kovarianzmatrix wird unter der Verwendung der Kovarianzmatrix der minimalen Distanz zwischen der berechneten 3D-Position des SIFT-Merkmals und den entsprechenden Sichtlinien bestimmt. Der Deskriptor d ist der Mittelwert von allen v Deskriptoren, welche zu dem entsprechenden 3D-Punkt des Merkmals beitragen. Die Liste der Sichtlinien x ~^ν, besteht aus normalisierten Vektoren ausgehend von den 3D-Positionen der entsprechenden Merkmale zu den v Kamera-Posen, die bei der Bestimmung des jeweiligen Merkmals berücksichtigt wurden, und repräsentiert, zusammen mit dem gemittelten Scale, den Bereich der Sichtrichtungen, aus denen ein entsprechendes SIFT-Merkmal detektiert werden kann. In dem Modell werden dabei nur 3D-Positionen mit v > 5 Sichtlinien verwendet.
Basierend auf den in einer Datenbank abgelegten SIFT-Merkmalen des zu lokalisierenden Objekts wird anschließend eine Schätzung der Pose des Objekts über ein Paar von Aufnahmen eines 3D-Stereo-Kamerasystems vorgenommen, wobei das Kamerasystem das entsprechende Objekt erfasst. Es werden dabei die Bilder der beiden Kameras j ∊ [L, R] des Stereo-Kamerasystems analysiert (L = links, R = rechts).
Die Objektlokalisierung läuft in mehreren Schritten ab, die in 2 verdeutlicht sind. In einem ersten Schritt S1 werden zunächst die SIFT-Merkmale und deren Bildpositionen ẑ_i,j in den Bildern der beiden Kameras j ∊ [L, R] berechnet bzw. extrahiert, wobei die Bildpositionen 2D-Positionen in der Form von Pixelpositionen in den jeweiligen Kamerabildern sind. In einem Schritt S2 werden dann aus den Bildpositionen ẑ_i,j die korrespondierenden lokalen Positionen x ^_i auf dem Objekt über Triangulation bestimmt und zusammen mit den entsprechenden Positionen ẑ_i,j aus den beiden Bildern abgespeichert. Dann wird im Schritt S3 ermittelt, welche lokalen Deskriptoren d der extrahierten Merkmale gut mit Deskriptoren aus der Datenbank übereinstimmen. Die sich hieraus ergebenden SIFT-Merkmale m_k aus der Datenbank werden dann geclustert, um Sätze zu bilden, welche räumlich und in Bezug auf die entsprechenden Objekttypen konzentriert sind. Die Clusterung erfolgt im Schritt S4. Hieraus wird schließlich im Schritt S5 eine Hypothese für die Objektpose des durch das Kamerasystem aufgenommenen Objekts bestimmt. Das soeben beschriebene Verfahren zur Objektlokalisation ist dabei an sich aus dem Stand der Technik bekannt und wird im Detail in der Druckschrift [1] beschrieben.
Um erfindungsgemäß eine gegenüber der Hypothese verbesserte Schätzung der Objektpose und gleichzeitig eine entsprechende Wahrscheinlichkeitsverteilung für diese geschätzte Objektpose zu erhalten, wird das nachfolgend erläuterte probabilistische Sensormodell eingesetzt.
Für eine beliebige Hypothese einer Objektpose ω in den 6D-Kamerakoordinaten C_j können die projizierten Bildkoordinaten z_i,j in dem entsprechenden Kamerabild j ∊ [L, R] mit einem an sich bekannten Standard-Lochkameramodell h_j(ω, x_i) wie folgt ermittelt werden
Dabei bezeichnet f die für das entsprechende Kamerasystem bekannte Brennweite der Kamera und
ist die an sich bekannte homogene Transformation von der lokalen 3D-Position auf dem Objekt in die Kamerakoordinaten des linken und rechten Bildes des 3D-Kamerasystems. Diese homogene Transformation hängt dabei von der Objektpose ω in einem ortsfesten Weltkoordinatensystem ab. Im Folgenden bezeichnet der Satz z =(z₁, ..., z_N) die Bildpositionen (d.h. Pixelpositionen) von N SIFT-Merkmale eines Objekts, für welche eine Korrespondenz der jeweiligen Merkmale mit Merkmalen aus der Datenbank gefunden wurde. Jeder Eintrag z_n = z_i,j dieses Satzes gibt dabei eine Bildposition im linken und rechten Bild des Kamerasystems an. Wie bereits oben erwähnt, enthalten die SIFT-Merkmale m_i aus der Datenbank eine Kovarianzmatrix Σ_i. Ebenso ist für das Sensormodell in geeigneter Weise eine Kovarianzmatrix Σ_j festgelegt, welche empirisch bestimmt wurde. Die Bestimmung einer solchen Kovarianzmatrix liegt dabei im Rahmen von fachmännischem Handeln.
In der hier beschriebenen Ausführungsform des erfindungsgemäßen Verfahrens wird nunmehr aus den beiden Kovarianzmatrizen Σ_i und Σ_j über die Linearisierung des oben beschriebenen Lochkameramodells eine neue (dritte) Kovarianzmatrix bestimmt. Zur Linearisierung wird der über die Kovarianzmatrix Σ_i modellierte Gaußsche Modellfehler an der entsprechenden lokalen Position x_i in die Bildebenen der beiden Kameras des Kamerasystems projiziert. Unter diesen Annahmen ist z_n näherungsweise wie folgt normalverteilt:

p(z_n|ω) = Ν(h_j(ω, x_i), Σ_i,j) (5)
Dabei bezeichnet
die Jacobi-Matrix (auch Funktionalmatrix oder Ableitungsmatrix genannt) der perspektivischen Projektion gemäß Gleichung (2) in Bezug auf die lokalen 3D-Positionen x_i.
Mit Hilfe der über die Linearisierung des Sensormodells ermittelten Kovarianzmatrix Σ_i,j wird dann eine verbesserte Schätzung der Objektpose ω bestimmt. Hierzu wird der Vektor der tatsächlich im Bild ermittelten (d.h. gemessenen) Bildpositionen entsprechender Merkmale verwendet, der wie folgt lautet ẑ = {ẑ₁, ..., ẑ_N}. Unter Verwendung der Verteilung der Fehler im Bild gemäß Gleichung (5) sowie der Bezeichnungen z_i,j(ω) ≕ z_n(ω) und Σ_i,j ≕ Σ_n werden folgende Größen berechnet: d_n(ẑ_n, ω) ≔ z_n(ω) – ẑ_n (6)
Dabei bezeichnet d_n(ẑ_n, ω) die Distanz zwischen einer über das Lochkameramodell gemäß Gleichung (2) ermittelten Bildposition und der entsprechenden, mit dem Kamerasystem gemessenen Bildposition. Basierend auf diesem Unterschied wird die obige Fehlerfunktion f_errdefiniert, welche eine Summe der Quadrate der Distanzen unter Einbeziehung der Werte der (dritten) Kovarianzmatrix ist. Auf diese Weise wird erreicht, dass eine entsprechende Distanz umso weniger gewichtet wird, je größer die Unsicherheit in der Messung ist, d.h. je größer die Streuung gemäß der dritten Kovarianzmatrix ist. Schließlich wird eine verbesserte geschätzte Objektposition basierend auf der Minimierung der obigen Fehlerfunktion f_err unter Variation der ursprünglichen Hypothese der Objektpose bestimmt. Diese Minimierung kann durch folgende Gleichung beschrieben werden:
Die Minimierung wird dabei numerisch mit an sich bekannten Verfahren, wie z.B. dem Gradientenabstiegsverfahren, gelöst. Durch die Minimierung gemäß Gleichung (8) wird implizit eine Funktion f(ẑ) = ω ^ definiert, welche die optimale (geschätzte) Objektpose ω ^ für eine gegebene Messung ẑ darstellt.
3 verdeutlicht nochmals in schematischer Darstellung die soeben beschriebene Schätzung einer Objektpose ω ^ . Es ist dabei schematisch ein Objekt O wiedergegeben, welches eine Objektpose ω aufweist, die in Bezug auf ein ortsfestes Welt-Koordinatensystem angegeben ist, das in 3 mit WK bezeichnet ist. Ferner ist in 3 schematisch das Kamerasystem C angedeutet, mit dem Bilder des Objekts O zur Schätzung seiner Pose aufgenommen werden. Für ein entsprechendes SIFT-Merkmal m an einer lokalen Position x auf dem Objekt erhält man in dem Kamerabild B, das nur ausschnittsweise wiedergegeben ist, eine entsprechende gemessene 2D-Position ẑ. Ferner kann mit dem oben beschriebenen Sensormodell basierend auf einer Lochkamera eine 2D-Bildposition über die lokale Position x berechnet werden. Diese berechnete Position ist in 3 mit z bezeichnet. Erfindungsgemäß werden nunmehr die Distanzen d zwischen diesen Positionen für alle SIFT-Merkmale des Objekts in Bild B berechnet (siehe Gleichung (6)) und daraus mittels der oben definierten Fehlerfunktion aus Gleichung (7) das in 3 angegebene Minimierungsproblem gelöst. Wie bereits oben erwähnt, wird durch das Minimierungsproblem die implizite Funktion f(ẑ) definiert.
Basierend auf dieser impliziten Funktion wird nunmehr in einem erfindungswesentlichen Schritt die Ableitung dieser impliziten Funktion in der Form der Jacobi-Matrix bestimmt, welche wie folgt lautet:
Diese Jacobi-Matrix wird durch die Anwendung der an sich bekannten Methode der impliziten Differentiation auf die obige Funktion g gemäß Gleichung (8) ermittelt. Dabei werden die partiellen Ableitungen einer Messung ẑ_n
explizit von der geschlossenen Form für h_j ermittelt. In der hier beschriebene Ausführungsform wird hierzu ein Computer-Algebra-System eingesetzt. Man erhält schließlich die Jacobi-Matrizen für den gesamten Messvektor ẑ, die wie folgt lauten:
Unter Verwendung dieser partiellen Ableitungen erhält man dann die Jacobi-Matrix für die implizite Funktion wie folgt:
Unter Verwendung der 2N × 2N blockdiagonalen Kovarianzmatrix Σ der tatsächlichen Messung ẑ, welche eine Zusammensetzung aus allen dritten Kovarianzmatrizen für die gemessenen Bildpunkte in den beiden Bildern des Kamerasystems ist, erhält man schließlich ein Gaußsches Modell für die Pose ω, welches durch folgende Wahrscheinlichkeitsverteilung beschrieben ist:
Diese Wahrscheinlichkeitsverteilung ist ein erfindungswesentliches Ergebnis, denn hierdurch wird für eine geschätzte Objektposition auch noch angegeben, wie groß die Streuung dieser Objektposition ist. Ist die Kovarianzmatrix der obigen Wahrscheinlichkeitsverteilung, die durch J_fΣ_ẑJ T / f gegeben ist, sehr groß, kann beispielsweise die geschätzte Position verworfen werden bzw. weitere Aufnahmen durch das Kamerasystem initiiert werden, um hierdurch eine bessere Schätzung der Objektpose zu erhalten.
4 zeigt beispielhaft ein Diagramm DI, welches in der linken Spalte C1 für die sechs Koordinaten einer Objektpose die im Rahmen einer Ausführungsform der Erfindung ermittelte Wahrscheinlichkeitsverteilung p gemäß Gleichung (13) wiedergibt. In einer besonders bevorzugten Ausführungsform der Erfindung wird ferner mit dieser Wahrscheinlichkeitsverteilung p ein Sampling durchgeführt, bei dem durch Stichprobenentnahme basierend auf der Funktion p eine neue verbesserte Wahrscheinlichkeitsverteilung für die Objektpose ermittelt wird. Eine über das Sampling entnommene Objektpose wird dabei gemäß einem Schwellwertkriterium nur dann in ein Histogramm aufgenommen, wenn der Wert
größer als ein geeignet festgelegter Schellwert ist. Die basierend auf diesem Histogramm erhaltene Wahrscheinlichkeitsverteilung stellt die neue Wahrscheinlichkeitsverteilung dar. Diese ist in 4 in der Spalte C2 für die einzelnen Dimensionen der Pose wiedergegeben und mit p’ bezeichnet. Man erkennt aus 4 insbesondere, dass die neue Wahrscheinlichkeitsverteilung p’ die tatsächliche, nicht Gaußsche Unsicherheit besser widerspiegelt als die vorgeschlagene Gaußsche Wahrscheinlichkeitsverteilung p.
Die im Vorangegangenen beschriebenen Ausführungsformen des erfindungsgemäßen Verfahrens weisen eine Reihe von Vorteilen auf. Insbesondere wird unter Verwendung eines geeigneten Sensormodells eine gute Schätzung einer Objektpose über die Minimierung einer Fehlerfunktion erreicht. Darüber hinaus kann basierend auf der Minimierung der Fehlerfunktion unter Verwendung der Methode der impliziten Differentiation auch eine Wahrscheinlichkeitsverteilung für die geschätzte Objektpose und damit eine Unsicherheit dieser Objektpose ermittelt wer- den. Diese Wahrscheinlichkeitsverteilung kann ggf. durch Sampling nochmals genauer bestimmt werden. Die Wahrscheinlichkeitsverteilung der geschätzten Objektpose kann dann beispielsweise dazu verwendet werden, um die geschätzte Objektpose zu verwerfen, wenn die Streuung gemäß der Wahrscheinlichkeitsverteilung sehr groß ist. Wird das Verfahren im Rahmen der Bewegung eines Roboters eingesetzt, der die geschätzte Objektpose verarbeitet, kann z.B. bei einer hohen Streuung der Wahrscheinlichkeitsverteilung eine nochmals genauere Positionsschätzung über weitere Kamerabilder durch den Roboter initiiert werden. Der Roboter führt dann eine an die geschätzte Objektpose gekoppelte Aktion, beispielsweise das Greifen des Objekts, erst dann aus, wenn die Schätzung der Objektpose eine Streuung aufweist, die unter einem vorgegebenen Schwellwert liegt.
Literaturverzeichnis:

[1] T. Grundmann, R. Eidenberger, M. Schneider, M. Fiegert, and G. Wichert v., "Robust high precision 6d pose determination in complex environments for robotic manipulation," in ICRA 2010 Workshop: Best Practice in 3D Perception and Modelling for Mobile Manipulation, 2010.
[2] N. Snavely, S.M. Seitz and R. Szeliski, "Photo tourism: exploring photo collections in 3d," ACM Trans. Graph., vol. 25, no.3, pp. 835–846, 2006.

Claims

Verfahren zur rechnergestützten Lageschätzung eines Objekts (O), wobei eine Modell-Datenbank verwendet wird, welche eine Vielzahl von Merkmalen (m) des Objekts (O) enthält, wobei jedem Merkmal (m) eine lokale Position (x) auf dem Objekt und eine erste Kovarianzmatrix (Σ_x) für die lokale Position (x) zugeordnet ist, bei dem: a) Merkmale (m) des Objekts (O), welche aus einem oder mehreren, mit einem Kamerasystem (C) aufgenommenen Kamerabildern (B) des Objekts (O) extrahiert werden, mit Merkmalen (m) des Objekts (O) aus der Modell-Datenbank verglichen werden, wodurch eine Hypothese für eine Objektpose (ω) erhalten wird; b) basierend auf einem Sensormodell des Kamerasystems (C) und der Hypothese für die Objektpose (ω) eine Fehlerfunktion (f_err) in Abhängigkeit von der Objektpose (ω) als Variablen minimiert wird, wodurch eine geschätzte Objektpose (ω ^) erhalten wird, wobei das Sensormodell die Zuordnung zwischen einer lokalen Position (x) auf dem Objekt (O) und einer Bildposition (z) im jeweiligen Kamerabild in Abhängigkeit von der Objektpose (ω) beschreibt und im Sensormodell eine zweite Kovarianzmatrix (Σ_j) für jede Bildposition (z) im jeweiligen Kamerabild (B) definiert ist, wobei die Fehlerfunktion (f_err) ein Fehlermaß zwischen über das Sensormodell ermittelten Bildpositionen (z) von im jeweiligen Kamerabild (B) enthaltenen Merkmalen (m) des Objekts (O) und über das Kamerasystem (C) gemessenen Bildpositionen (ẑ) der Merkmale (m) des Objekts (O) beschreibt; c) mittels einer Funktion (f), welche über die Minimierung der Fehlerfunktion (f_err) implizit definiert ist und die Abhängigkeit der geschätzten Objektpose (ω ^) von den gemessenen Bildpositionen (ẑ) der Merkmale (m) des Objekts (O) beschreibt, eine Jacobi-Matrix für die implizite Funktion (f) mittels impliziter Differentiation bestimmt wird und hieraus unter Verwendung der ersten und zweiten Kovarianzmatrizen (Σ_x, Σ_j) eine Wahrscheinlichkeitsverteilung (p) der geschätzten Objektpose (ω ^) ermittelt wird.
Verfahren nach Anspruch 1, bei dem die Fehlerfunktion (f_err) eine Summe aus Distanzmaßen (d) ist, wobei ein jeweiliges Distanzmaß (d) den Unterschied zwischen einer über das Sensormodell ermittelten Bildposition (z) von einem jeweiligen, im entsprechenden Kamerabild (B) enthaltenen Merkmal (m) des Objekts (O) und der über das Kamerasystem (C) gemessenen Bildposition (ẑ) des Merkmals (m) beschreibt.
Verfahren nach Anspruch 1 oder 2, bei dem für jede gemessene Bildposition (ẑ) eines im entsprechenden Kamerabild (C) enthaltenen Merkmals (m) eine dritte Kovarianzmatrix aus der der ersten und zweiten Kovarianzmatrix (Σ_x, Σ_j) ermittelt wird.
Verfahren nach Anspruch 2 und 3, bei dem die Fehlerfunktion (f_err) eine gewichtete Summe der Distanzmaße (d) ist, wobei ein Distanzmaß (d) derart von der dritten Kovarianzmatrix der zum jeweiligen Distanzmaß d gehörigen gemessenen Bildposition (ẑ) abhängt, dass das Distanzmaß (d) umso weniger gewichtet wird, je größer die durch die dritte Kovarianzmatrix beschriebene Streuung ist.
Verfahren nach Anspruch 3 oder 4, bei dem aus den dritten Kovarianzmatrizen und der Jacobi-Matrix mittels einer Matrixmultiplikation eine Kovarianzmatrix für die Wahrscheinlichkeitsverteilung (p) der geschätzten Objektpose (ω ^) ermittelt wird.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem die Wahrscheinlichkeitsverteilung (p) der geschätzten Objektpose (ω ^) durch eine Gaußverteilung mit der geschätzten Objektpose (ω ^) als Mittelwert beschrieben wird.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem das Sensormodell auf einem Lochkameramodell basiert.
Verfahren nach Anspruch 7, wenn abhängig von Anspruch 3, bei dem über eine Linearisierung des Lochkameramodells die dritte Kovarianzmatrix bestimmt wird
Verfahren nach einem der vorhergehenden Ansprüche, bei dem die Merkmale (m) des Objekts (O) SIFT-Merkmale sind.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem aus der Wahrscheinlichkeitsverteilung (p) der geschätzten Objektpose (ω ^) mittels Stichprobenentnahme Objektposen (ω) entnommen werden und ein Histogramm aus entnommenen Objektposen (ω) ermittelt wird, welche ein vorgegebenes Schwellwertkriterium erfüllen, wodurch eine neue Wahrscheinlichkeitsverteilung (p’) erhalten wird.
Verfahren nach Anspruch 10, bei dem das vorgegebene Schwellwertkriterium von dem Wert der Fehlerfunktion für die entnommene Objektpose (ω) abhängt und insbesondere derart definiert ist, dass die entnommene Objektpose (ω) das Schwellwertkriterium erfüllt, wenn die Exponentialfunktion mit dem mit –1 multiplizierten Wert der Fehlerfunktion (f_err) für die entnommene Objektpose (ω) als Exponenten größer als ein vorgegebener Schwellwert ist.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem eine geschätzte Objektpose (ω ^) verworfen wird, wenn die in Schritt c) ermittelte Wahrscheinlichkeitsverteilung (p) und/oder die neue Wahrscheinlichkeitsverteilung (p’) eine Streuung aufweisen, welche größer als eine vorbestimmte Schwelle ist.
Vorrichtung zur rechnergestützten Lageschätzung eines Objekts (O), wobei in der Vorrichtung eine Modell-Datenbank gespeichert ist, welche eine Vielzahl von Merkmalen (m) des Objekts (O) enthält, wobei jedem Merkmal (m) eine lokale Position (x) auf dem Objekt und eine erste Kovarianzmatrix (Σ_x) für die lokale Position (x) zugeordnet ist, wobei die Vorrichtung ein Kamerasystem (C) zur Aufnahme von Kamerabildern (B) des Objekts (O) sowie eine Rechnereinheit umfasst, wobei die Rechnereinheit derart ausgestaltet ist, dass sie ein Verfahren durchführt, bei dem: a) Merkmale (m) des Objekts (O), welche aus einem oder mehreren, mit dem Kamerasystem (C) aufgenommenen Kamerabildern (B) des Objekts (O) extrahiert werden, mit Merkmalen (m) des Objekts (O) aus der Modell-Datenbank verglichen werden, wodurch eine Hypothese für eine Objektpose (ω) erhalten wird; b) basierend auf einem Sensormodell des Kamerasystems (C) und der Hypothese für die Objektpose (ω) eine Fehlerfunktion (f_err) in Abhängigkeit von der Objektpose (ω) als Variablen minimiert wird, wodurch eine geschätzte Objektpose (ω ^) erhalten wird, wobei das Sensormodell die Zuordnung zwischen einer lokalen Position (x) auf dem Objekt (O) und einer Bildposition (z) im jeweiligen Kamerabild in Abhängigkeit von der Objektpose (ω) beschreibt und im Sensormodell eine zweite Kovarianzmatrix (Σ_j) für jede Bildposition (z) im jeweiligen Kamerabild (B) definiert ist, wobei die Fehlerfunktion (f_err) ein Fehlermaß zwischen über das Sensormodell ermittelten Bildpositionen (z) von im jeweiligen Kamerabild (B) enthaltenen Merkmalen (m) des Objekts (O) und über das Kamerasystem (C) gemessenen Bildpositionen (ẑ) der Merkmale (m) des Objekts (O) beschreibt; c) mittels einer Funktion (f), welche über die Minimierung der Fehlerfunktion (f_err) implizit definiert ist und die Abhängigkeit der geschätzten Objektpose (ω ^) von den gemessenen Bildpositionen (ẑ) der Merkmale (m) des Objekts (O) beschreibt, eine Jacobi-Matrix für die implizite Funktion (f) mittels impliziter Differentiation bestimmt wird und hieraus unter Verwendung der ersten und zweiten Kovarianzmatrizen (Σ_x, Σ_j) eine Wahrscheinlichkeitsverteilung (p) der geschätzten Objektpose (ω ^) ermittelt wird.
Vorrichtung nach Anspruch 13, welche derart ausgestaltet ist, dass mit der Vorrichtung ein Verfahren nach einem der Ansprüche 2 bis 12 durchführbar ist.
Roboter, umfassend eine Vorrichtung nach Anspruch 13 oder 14, wobei der Roboter im Betrieb seine Bewegungen unter Verwendung der mit der Vorrichtung geschätzten Objektposen (ω ^) durchführt.
Computerprogrammprodukt mit einem auf einem maschinenlesbaren Träger gespeicherten Programmcode zur Durchführung eines Verfahrens nach einem der Ansprüche 1 bis 12, wenn das Programm auf einem Rechner abläuft.