DE102022111926A1 - Verfahren und Vorrichtung für eine tiefengestützte visuelle Trägheitsodometrie - Google Patents

Abstract

Ein Verfahren und eine Vorrichtung sind zum Durchführen einer visuellen Trägheitsodometrie (VIO) vorgesehen. Es werden Messwerte von einer inertialen Messeinheit (IMU), einer Kamera und einem Tiefensensor verarbeitet. Der Keyframe-Restwert, der zumindest einen Tiefenrestwert beinhaltet, wird basierend auf den verarbeiteten Messwerten bestimmt. Ein Gleitfenstergraph wird basierend auf Faktoren erzeugt und optimiert, die von dem Keyframe-Restwert abgeleitet werden. Eine Objektpose wird basierend auf dem optimierten Gleitfenstergraphen geschätzt.

Description

• Priorität
• Die vorliegende Anmeldung basiert auf und beansprucht die Priorität der am 24. Mai 2021 beim US-Patent- und Markenamt eingereichten vorläufigen US-Patentanmeldung mit der zugewiesenen Nummer 63/192,488 , deren Inhalt hierin durch Verweis aufgenommen ist.
• Technisches Gebiet
• Die vorliegende Offenbarung betrifft im Allgemeinen ein visuelles Trägheitsodometriesystem und insbesondere ein visuelles Trägheitsodometrie- (VIO-) System, welches Tiefenmesswerte in einer visuellen und trägheitsbezogenen Messung beinhaltet.
• Hintergrund
• In modernen Smartphones hat sich die Anzahl von Sensoren vergrößert, welche für eine Positionsbestimmung in einer Augmented-Reality- (AR-) oder einer dreidimensionalen Rekonstruktions- (3DR-) Anwendung verwendet werden können. Verschiedene Arten von simultanen Positionsbestimmungs- und Kartierungs- (SLAM-) Systemen beinhalten ausschließlich visuelle SLAM-Systeme und durch eine inertiale Messeinheit (IMU) gestützte visuelle SLAM-Systeme, die als VIO-Systeme bezeichnet werden. Ausschließlich visuelle SLAM-Systeme, die sowohl Kamera- als auch Tiefensensoren zum Schätzen einer Kameratrajektorie verwenden, beinhalten einen Oriented-Fast-and-Rotated-Brief- (ORB-) SLAM einer Version 2 (SLAM2) und einen Red-Green-Blue-Depth- (RGBD-) SLAM. Diese beiden Systeme verwenden Tiefenmesswerte zur Verbesserung einer Posenschätzung, die durch das zweidimensionale (2D-) merkmalbasierte ausschließlich visuelle SLAM-System durchgeführt wird.
• VIO-Systeme werden beim Nachverfolgen von sechs Freiheitsgraden (6DOF) für eine Objektpose unter Verwendung von Daten von einem Kamerabild (z. B. visuellen Daten) und einer IMU verwendet. Die VIO-Systeme verwenden eindeutige Merkmale, welche in Kamerabildern, Beschleunigungsmesserdaten und Gyroskopdaten nachverfolgt werden, als Messwerte zur Schätzung der Objektpose durch ein eng gekoppeltes System, das mathematische Optimierungstechniken verwendet.
• Kurzfassung
• Gemäß einer Ausführungsform ist ein Verfahren zur Durchführung einer VIO an einer Benutzervorrichtung (UE) bereitgestellt. Messwerte werden von einer IMU, einer Kamera und einem Tiefensensor der UE verarbeitet. Ein Keyframe-Restwert, der zumindest einen Tiefenrestwert beinhaltet, wird basierend auf den verarbeiteten Messwerten bestimmt. Ein Gleitfenstergraph wird basierend auf Faktoren erzeugt und optimiert, welche von dem Keyframe-Restwert abgeleitet werden. Eine Objektpose der UE wird basierend auf dem optimierten Gleitfenstergraphen geschätzt.
• Gemäß einer Ausführungsform ist eine UE bereitgestellt, welche eine IMU, eine Kamera, einen Tiefensensor, einen Prozessor und ein nichttransitorisches computerlesbares Speichermedium beinhaltet, das Anweisungen speichert. Wenn die Anweisungen ausgeführt werden, bewirken sie, dass der Prozessor Messwerte von der IMU, der Kamera und dem Tiefensensor verarbeitet. Die Anweisungen bewirken zudem, dass der Prozessor den Keyframe-Restwert, der zumindest einen Tiefenrestwert beinhaltet, basierend auf den verarbeiteten Messwerten bestimmt, und einen Gleitfenstergraphen basierend auf Faktoren erzeugt und optimiert, welche von dem Keyframe-Restwert abgeleitet werden. Die Anweisungen bewirken ferner, dass der Prozessor eine Objektpose der UE basierend auf dem optimierten Gleitfenstergraphen schätzt.
• Figurenliste
• Die vorstehenden und weiteren Aspekte, Merkmale und Vorteile von bestimmten Ausführungsformen der vorliegenden Offenbarung werden anhand der nachstehenden ausführlichen Beschreibung im Zusammenhang mit der beigefügten Zeichnung näher erläutert. Es zeigen:
• 1 ein Diagramm, das einen Faktorgraphen von einem monokularen visuellen Trägheitsnavigationssystem (VINS-Mono) darstellt;
• 2 ein Diagramm, das einen Faktorgraphen für eine MSL-tiefengestützte VIO (DVIO) gemäß einer Ausführungsform darstellt;
• 3 ein Diagramm, das eine Laufzeit- (TOF-) Kamera gemäß einer Ausführungsform darstellt;
• 4 ein Diagramm, das strukturierte Lichtsensoren gemäß einer Ausführungsformen darstellt,
• 5 ein Diagramm, das einen Systemfluss der MSL-DVIO gemäß einer Ausführungsformen darstellt;
• 6 ein Diagramm, das einen ausführlichen Systemfluss der MSL-DVIO gemäß einer Ausführungsform der Offenbarung darstellt;
• 7 ein Diagramm, das eine MSL-DVIO-1D-Merkmalsimplementieurng gemäß einer Ausführungsform darstellt;
• 8 ein Diagramm, das Einzelheiten auf der Implementierung der MSL-DVIO für eine 3D-Merkmalsparameterisierung gemäß einer Ausführungsform darstellt;
• 9 ein Flussdiagramm, das ein Verfahren zum Durchführen einer VIO an einer UE gemäß einer Ausführungsform darstellt; und
• 10 ein Blockdiagramm von einer elektronischen Vorrichtung in einer Netzwerkumgebung gemäß einer Ausführungsform.
• Ausführliche Beschreibung
• Nachstehend werden Ausführungsformen der vorliegenden Offenbarung unter Bezugnahme auf die beigefügte Beschreibung ausführlich beschrieben. Es wird darauf hingewiesen, dass identische Elemente mit identischen Bezugszeichen gekennzeichnet sind, auch wenn sie in unterschiedliche Figuren gezeigt sind. In der nachstehenden Beschreibung sind bestimmte Details, wie z. B. ausführliche Konfigurationen und Komponenten, lediglich dazu vorgesehen, für ein umfassendes Verständnis der Ausführungsformen der vorliegenden Offenbarung zu sorgen. Daher sollte es für Fachleute offenkundig sein, dass verschiedene Veränderungen und Modifikationen der hierin beschriebenen Ausführungsformen vorgenommen werden können, ohne vom Schutzbereich der vorliegenden Erfindung abzuweichen. Darüber hinaus wird aus Gründen der Übersichtlichkeit auf die Beschreibung von hinreichend bekannten Funktionen und Konstruktion verzichtet. Bei den nachstehend beschriebenen Begriffen handelt es sich um Begriffe, die unter Berücksichtigung der Funktionen in der vorliegenden Offenbarung definiert sind, und können je nach Benutzer, Intention des Benutzers oder Gewohnheit variieren. Die Definitionen der Begriffe sollten basierend auf dem Inhalt der gesamten Beschreibung bestimmt sein.
• Die vorliegende Offenbarung kann verschiedene Modifikationen und Ausführungsformen aufweisen, von denen einige Ausführungsformen nachstehend unter Bezugnahme auf die beigefügte Zeichnung näher erläutert sein. Es wird darauf hingewiesen, dass die vorliegende Offenbarung nicht auf die Ausführungsformen beschränkt ist, sondern alle Modifikationen, Entsprechungen und Alternativen innerhalb des Schutzbereichs der vorliegenden Offenbarung beinhaltet.
• Obgleich die Begriffe, die eine Ordinalzahl beinhalten, wie z. B. erste/r/s, zweite/r/s etc., zur Beschreibung unterschiedlicher Elemente verwendet werden können, sind die strukturellen Elemente nicht durch die Begriffe eingeschränkt. Die Begriffe werden lediglich zur Unterscheidung des einen Elements von dem anderen verwendet. Ohne vom Schutzbereich der vorliegenden Offenbarung abzuweichen, kann z. B. ein erstes strukturelles Element als ein zweites strukturelles Element bezeichnet werden. Das zweite strukturelle Element kann auch als das erste strukturelle Element bezeichnet werden. Der hierin verwendete Begriff „und/oder“ beinhaltet alle möglichen Kombinationen von einem oder mehreren zugeordneten Elementen.
• Die hierin verwendeten Begriffe werden lediglich zur Beschreibung verschiedener Ausführungsformen der vorliegenden Offenbarung verwendet, sind aber nicht als Einschränkung der vorliegenden Offenbarung gedacht. Soweit der Kontext nicht deutlich etwas anderes besagt, sollen in Singularformen auch Pluralformen enthalten sein. In der vorliegenden Offenbarung wird darauf hingewiesen, dass die Begriffe „beinhalten“ oder „aufweisen“ das Vorhandensein von einem Merkmal, einer Zahl, eines Schritts, einer Operation, eines strukturellen Elements, von Teilen oder von einer Kombination aus denselben anzeigt, und das Vorhandensein oder die Wahrscheinlichkeit der Hinzufügung von einer/m oder mehreren anderen Merkmalen, Zahlen, Schritten, Operationen, strukturellen Elementen, Teilen oder Kombinationen aus denselben nicht ausschließt.
• Soweit sie in ihrer Definition nicht abweichen, haben alle hierin verwendeten Begriffe dieselbe Bedeutung wie jene, wie sie von einem Fachmann auf dem Gebiet der Technik verstanden werden, auf welches sich die vorliegende Offenbarung bezieht. Begriffe, wie z. B. jene, wie sie in einem allgemein verwendeten Wörterbuch definiert sind, sind so zu interpretieren, als hätten sie dieselbe Bedeutung wie im Kontext des einschlägigen Gebiets der Technik, und nicht so, als wäre deren Bedeutung eine ideale oder überaus formale, sofern die vorliegende Offenbarung keine eindeutig Definition dazu enthält.
• Die elektronische Vorrichtung kann eine von verschiedenen Typen von elektronischen Vorrichtungen sein. Die elektronischen Vorrichtungen können z. B. eine tragbare Kommunikationsvorrichtung (z. B. ein Smartphone), einen Computer, eine tragbare Multimediavorrichtung, eine tragbare medizinische Vorrichtung, eine Kamera, eine tragbare Vorrichtung oder ein Haushaltsgerät beinhalten. Gemäß einer Ausführungsform der Offenbarung ist eine elektronische Vorrichtung nicht auf die vorstehend beschriebenen beschränkt.
• Die in der vorliegenden Offenbarung verwendeten Begriffe sollen die vorliegende Offenbarung nicht einschränken, sondern verschiedene Veränderungen, Entsprechungen oder Ersetzungen für eine entsprechende Ausführungsform beinhalten. In Bezug auf die Beschreibungen der beigefügten Zeichnung können identische Bezugszeichen zur Benennung identischer oder verwandter Elemente verwendet werden. Eine Singularform eines Nomens, das einem Element entspricht, kann ein oder mehrere Dinge beinhalten, es sei denn, aus dem entsprechenden Kontext geht eindeutig anderes hervor. Die hierin verwendeten Ausdrücke „A oder B“, „zumindest eine/r/s von A und B“, „zumindest eine/r/s von A oder B“, „A, B oder C“, „zumindest eine/r/s von A, B, und C“ und „zumindest eine/r/s von A, B oder C“ können jeweils alle möglichen Kombinationen der Elemente beinhalten, die in einem entsprechenden der Ausdrücke gemeinsam aufgezählt sind. Die hierin verwendeten Ausdrücke „1.“, „2.“, erste/r/s und „zweite/r/s“ können verwendet werden, um eine entsprechende Komponente von einer anderen Komponente zu unterscheiden, sollen die Komponenten aber nicht in anderen Aspekten (z. B. Bedeutung oder Reihenfolge) beeinträchtigen. Wenn auf ein Element (z. B. ein erstes Element), mit oder ohne den Begriff „operativ“ oder „kommunikativ“, Bezug genommen werden soll als „gekoppelt mit“, „gekoppelt an“, „verbunden mit“ oder „verbunden an“ ein anderes / einem anderen Element (z. B. ein zweites / einem zweiten Element), zeigt dies an, dass das Element mit dem anderen Element direkt (z. B. verdrahtet), drahtlos oder über ein drittes Element gekoppelt sein kann.
• Der hierin verwendete Begriff „Modul“ kann eine Einheit beinhalten, die in einer Hardware, einer Software oder einer Firmware implementiert ist, und kann austauschbar mit anderen Begriffen verwendet werden, wie z. B. „Logik“, „Logikblock“, „Teil“ und „Schaltungsaufbau“. Ein Modul kann eine einzelne integrale Komponente oder eine minimale Einheit oder ein Teil derselben sein, welche/s so angepasst ist, dass sie/es eine oder mehrere Funktionen durchführt. Zum Beispiel kann gemäß einer Ausführungsform ein Modul in Form von einer anwendungsspezifischen integrierten Schaltung (ASIC) implementiert sein.
• 1 ist ein Diagramm, das einen Faktorgraphen von einem VINS-Mono darstellt. x_k bezeichnet einen vollständigen Zustand einer Roboterbewegung. Zustandsvariablen können wie durch Tabelle 1 definiert als nachstehende Gleichung (1) geschrieben werden. $$x_k^{nav}=\left[\begin{array}{c}{x}_k^R\\ x_k^{SB}\end{array}\right]=\left[\begin{array}{c}{p}_k^w\\ q_k^w\\ w_k^w\\ b_{a_k}\\ b_{{\omega }_k}\end{array}\right]$$

  
• Der Roboterposenzustand ist definiert als $x_k^R:=\left[\begin{array}{c}{p}_k^w\\ q_k^w\end{array}\right],$

  

  wobei der Robotergeschwindigkeits- und Bias-Zustand definiert ist als $x_k^{SB}:=\left[\begin{array}{c}{v}_k^w\\ b_{a_k}\\ b_{w_k}\end{array}\right].$

  

  Tabelle 1

  Zustandsvariable	Größe	Beschreibungen
  $x_k^{nav}$	16×1	Vollständiger Roboterzustand bei Frame k
  $x_k^R$	7×1	Roboterposenzustand
  $x_k^{SB}$	9×1	Robotergeschwindigkeits- und Bias-Zustand
  $p_k^w$	3×1	Position (im World-Frame bzw. festen Referenzrahmen)
  $q_k^w$	4×1	Ausrichtungsquaternion (im World-Frame)
  $v_k^w$	3×1	Geschwindigkeit (im World-Frame)
  bak	3×1	Bias-Schätzung von einem Beschleunigungsmesser
  bωk	3×1	Bias-Schätzung von einem Gyroskop

• Für das gesamte VINS-Mono von 1 beinhaltet eine gleitfensterbasierte, enggekoppelte VIO für eine Zustandsschätzung n + 1 Roboterzustände x^nav und m + 1 inverse 3D-Orientierungspunkt-Tiefen ${\lambda }_i=\frac{1}{d_i},i\in \left\{\mathrm{0,}\dots ,m\right\}\mathrm{..}$

  

  Der Zustandsvektor innerhalb des Gleitfensters ist definiert wie in Gleichung (2) unten aufgeführt. $$X=\left[x^{nav};x^{feat}\right]$$

  

  wenn $x^{nav}=\left[x_0^{nav};x_1^{nav};\dots ;x_n^{nav}\right]$

  

  mit $x_k^{nav}=\left[\begin{array}{c}{x}_k^R\\ x_k^{SB}\end{array}\right],$

  

  Pose, Geschwindigkeit und Bias für k. Keyframes, und x^feat = [λ₀...λ_m], inverse Orientierungspunkttiefe wie im ersten Kamera-Keyframe beobachtet. Dieser erste Keyframe, mit dem der Orientierungspunktmesswert verankert ist, wird als Anker-Frame bezeichnet.
• Im VINS-Mono wird nach einer Lösung für Gleichung (3) unten gesucht. $$\underset{x}{\text{min}}\left\{{\Vert r_m\left(X_r\right)\Vert }^2+{\displaystyle \sum _{k\in B_w}{\Vert r_{IMU}\left(x_k,x_{k+1}\right)\Vert }_{P_{b_{k+1}}^{b_k}}^2+}{\displaystyle \sum _{\left(i,j,l\right)\in F_w}{\Vert r_{VIS}\left(x_i^R,x_j^R,{\lambda }_l\right)\Vert }_Q^2}\right\}$$

  
• Hier ist B_w ein Index des Gleitfensters, ist F_w ein Satz von Feststellungen innerhalb des Gleitfensters, ist X =U_k∈Bw {x_k} ein Satz von Zustandsvariablen zur Überoptimierung, ist r_IMU(x_k,x_k+1) ein IMU-Faktor, ist $r_{VIS}\left(x_i^R,x_j^R,{\lambda }_l\right)$

  

  ein visueller Faktor, ist r_M (X_r) ein Marginalisierungsfaktor, ist X_r ein voller Zustand nach einer Marginalisierung, ist $P_{b_{k+1}}^{b_k}$

  

  eine Kovarianzmatrix für das IMU-Teil zum Erzeugen des Mahalanobis-Abstands für den Restwert und ist Q eine Kovarianzmatrix für den visuellen Teil, der zum Erzeugen des Mahalanobis-Abstands für den Restwert notwendig ist.
• Somit stellt 1 eine Beziehung zwischen den Faktoren $x_1^{SB}$

  

  102, $x_2^{SB}$

  

  104, $x_3^{SB}$

  

  106, r_IMU(x₁,x₂) 108, r_IMU(x₂,x₃) 110, $x_1^R$

  

  112, $x_2^R$

  

  114, $x_3^R$

  

  116, $r_{VIS}\left(x_1^R,x_2^R,{\lambda }_j\right)$

  

  1118, $r_{VIS}\left(x_1^R,x_2^R,{\lambda }_i\right)$

  

  and $r_{VIS}\left(x_1^R,x_3^R,{\lambda }_i\right)$

  

  120, λ_j 122, und λ_i 124 dar.
• Da Ausführungsformen in der MSL-DVIO Einfluss ausüben auf den visuellen Faktor in dem vorstehend beschriebenen Faktorgraphenansatz, wird der visuelle Faktor nachstehend ausführlich beschrieben. Der IMU-Faktor ist weiter unten ausführlich beschrieben.
• In der MSL-DVIO, bei der es sich um ein RGBD+IMU-System handelt, wird der visuelle Faktor von dem Merkmalsmesswert des Kamerabildes und der Tiefenkarte erzeugt. Der IMU-Faktor für die MSL-DVIO ist mit der für das VINS-Mono identisch. Im Gegensatz zum VINS-Mono ist die Tiefenkarte zusammen mit der Kameraaufnahme für die MSL-DVIO verfügbar.
• 2 ist ein Diagramm, das einen Faktorgraphen für die MSL-DVIO gemäß einer Ausführungsform darstellt. Insbesondere 2 stellt eine Beziehung dar zwischen den Faktoren $x_1^{SB}$

  

  202, $x_2^{SB}$

  

  204, $x_3^{SB}$

  

  206, $x_4^{SB}$

  

  208, r_IMU(x₁,x₂) 210, r_IMU(x₂,x₃) 212, r_IMI(x₃,x₄) 214, $x_1^R$

  

  216, $x_2^R$

  

  218, $x_3^R$

  

  220, $x_4^R$

  

  222, $r_{VIS}\left(x_1^R,x_2^R,{\lambda }_j\right)$

  

  224, $r_{VIS}\left(x_1^R,x_2^R,{\lambda }_i\right)$

  

  and $r_{VIS}\left(x_1^R,x_3^R,{\lambda }_i\right)$

  

  226, $r_{VIS}\left(x_3^R,x_4^R,{\lambda }_m\right)$

  

  228, $r_{DP}\left(x_1^R,{\lambda }_i\right)$

  

  230, $r_{DP}\left(x_1^R,x_3^R,{\lambda }_j\right)$

  

  232, $r_{DP}\left(x_4^R,{\lambda }_m\right)$

  

  234, λ_j 236, λ_i 238, und λ_m 240. r_DP ist ein Tiefenrestwert.
• Die Begriffe „Restwert“ und „Rest-“ sind austauschbar und definieren im Allgemeinen einen Rechenverlust zwischen einem geschätzten Wert von Zustandsparametern und einem Messwert von einem Sensor.
• Die Bildmerkmalserfassung und -nachverfolgung von VINS-Mono werden in der MSL-DVIO übernommen. Für jedes erfasste Merkmal wird eine entsprechende Tiefe von der Tiefenkarte extrahiert und ein Tiefenrestfaktor gebildet. Aufgrund der Beschaffenheit der Tiefenkarte können Verdeckungen und ein Out-of-Range auftreten, wobei die entsprechende Tiefe für den Merkmalspunkt nicht immer verfügbar sein kann. Daher ist die Anzahl von Tiefenfaktoren kleiner oder gleich der Anzahl von Bildmerkmalsfaktoren, wie in 2 gezeigt ist.
• Für einen RGBD-Sensor werden Real-World-3D-Orientierungspunkte auf eine Bildebene projiziert, um 2D-Positionen von diesen Orientierungspunkten auf dem RGBD-Sensor und die Tiefe dieser Orientierungspunkte auf dem Tiefensensor zu erzeugen. Das Kameramessungsmodell ist in Gleichung (4) unten aufgeführt. $$z_l^{IMG}=K\left[R|t\right]\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right]+n_{feat}$$

  
• Das Tiefenmessungsmodell ist in Gleichung (5) unten aufgeführt. $$z_l^{DP}=\left({\left|\left[R|t\right]\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right]\right|}_z\right)+n_{depth}$$

  
• Hier ist $z_l^{IMG}$

  

  eine 2D-Position eines Orientierungspunkts in einem Image-Frame, ist $z_l^{DP}$

  

  eine Tiefe des Orientierungspunkts in einem Referenz-Kamera-Frame, ist K eine intrinsische Kameramatrix. [R|t] ist eine Posenmatrix der Kamera in einer WorldFrame-Referenz, $\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right]$

  

  ist eine 3D-Position des Orientierungspunkts / in einem ReferenzWorld-Frame und n_feat und n_depth sind ein Messungsrauschen, von dem man annimmt, dass es sich um ein Gaußsches Rauschen handelt.
• Für einen gegebenen Orientierungspunkt l wird der Frame i auf den ersten Frame gesetzt, der den Orientierungspunkt l feststellt. Der Frame i wird als der Anker-Frame des Orientierungspunkts l bezeichnet. $Z_{li}^{IMG}={\left[\overline{u_{il}},\overline{v_{il}}\right]}^T$

  

  wird als der Merkmalsmesswert des Orientierungspunkts l im Frame i gesetzt, und $Z_{Ij}^{IMG}={\left[\overline{u_{jl}},\overline{v_{jl}}\right]}^T$

  

  wird als der Merkmalsmesswert des Orientierungspunkts l in einem zukünftigen Frame j gesetzt.
• Da die MSL-DVI kalibrierte Kameras verwendet, wird der Merkmalsmesswert in der normalisierten Ebene und nicht in der Bildebene gespeichert. Zur Berechnung dieses Messwerts wird der Merkmalspunkt von der Bildebene auf eine normalisierte Ebene unter Verwendung einer inversen Kameraprojektionsmatrix angehoben, wie in Gleichung (6) unten aufgeführt ist. $$\left[\begin{array}{c}\overline{x_l}\\ \overline{y_l}\\ \overline{z_l}\end{array}\right]={\pi }^{-1}\left(z_l^{IMG}\right)$$

  
• Eine Linsenverzeichnung wird dann durch rekursives Anwenden des Verzerrungsmodells auf den normalisierten Merkmalsmesswert gehandhabt. Der endgültige Merkmalsmesswert für einen Orientierungspunkt in einem Kamera-Frame wird wie unten in Gleichung (7) aufgeführt gespeichert. $$Z_l^{IMG}=\left[\begin{array}{c}\overline{u_l}\\ \overline{v_l}\end{array}\right]=\left[\begin{array}{c}\frac{\overline{x_l}}{\overline{z_l}}\\ \frac{\overline{y_l}}{\overline{z_l}}\end{array}\right]$$

  
• Da der Orientierungspunkt unter Verwendung der inversen Tiefe des Merkmals im Anker-Frame geschätzt wird, wird diese Implementierung der MSL-DVIO bezeichnet als MSL-DVIO 1D.
• Unter Verwendung des Merkmalsmesswerts in den Frames i und j wird der BildRestwert für das Merkmal in dem Nicht-Anker-Frame j berechnet wie in Gleichung (8) unten aufgeführt. $$r_{IMG}\left(x_i^R,x_j^R,{\lambda }_l\right)=g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{il}}}{{\lambda }_l},\frac{\overline{v_{il}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{lj}^{IMG}$$

  
• Hier ist $g\left({\left[X,Y,Z\mathrm{,1}\right]}^T\right)={\left[\frac{X}{Z},\frac{Y}{Z}\right]}^T,T_{bc}=\left[\begin{array}{cc}{R}_{bc}& p_c^b\\ 0& 1\end{array}\right]$

  

  die Transformation von der Kamerakoordinate zur IMU-Koordinate, ist $T_{cb}=T_{bc}^T,T_{b_{j}w}=\left[\begin{array}{cc}{R}_{b_{j}w}& p_w^{b_j}\\ 0& 1\end{array}\right]$

  

  die Roboterpose für den Frame j und ist $T_{b_{i}w}=\left[\begin{array}{cc}{R}_{b_{i}w}& p_w^{b_i}\\ 0& 1\end{array}\right]$

  

  die Roboterpose für den Frame i, $T_{w{b}_i}=T_{b_{i}w}^T.$

  
• Unter der Annahme, dass die Kamera und die Tiefenkarte in Zeit und Raum ausgerichtet sind, wird in der Tiefenkarte ebenso das Tiefenfeld von Orientierungspunkt / gemessen. Die geschätzte Tiefe des Orientierungspunkts im Anker-Frame kann zum Schätzen der Tiefe des Orientierungspunkts im Frame j verwendet werden. Dementsprechend ist der Tiefenrestwert in Gleichung (9) unten aufgeführt. $$r_{DP}\left(x_i^R,x_j^R,{\lambda }_l\right)={\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{il}}}{{\lambda }_l},\frac{\overline{v_{il}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-Z_{lj}^{DP}$$

  
• Hier ist $Z_{lj}^{DP}$

  

  der inverse Tiefenmesswert des Orientierungspunkts in dem Kamera-Frame einer Referenz gegeben durch $Z_{lj}^{DP}=\frac{1}{z_l^{DP}}.$

  

  Der Einfachheit halber kann angenommen werden, dass das Messungsrauschen für einen inversen Tiefenmesswert ebenfalls ein Gaußsches Rauschen ist.
• Unter Verwendung des Tiefenrestwerts und des Bildrestwerts wird der visuelle Faktorrestwert für einen Nicht-Anker-Frame durch Stapeln des Bild- und Tiefenrestwerts miteinander erzeugt, wie in Gleichung (10) unten aufgeführt ist. $$r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)=\left[\begin{array}{c}{r}_{IMG}\left(x_i^R,x_j^R,{\lambda }_l\right)\\ r_{DP}\left(x_i^R,x_j^R,{\lambda }_l\right)\end{array}\right]$$

  
• Dieser Restwert wird dann in einer Graphenoptimierung verwendet, um den Roboterzustand unter Verwendung einer nichtlinearen Optimierung zu lösen. Beim Durchführen der Optimierung wird die Jacobische des Restwerts in Bezug auf die Zustandsvariable erzeugt. Zur Berechnung der Jacobischen des visuellen Restwerts des Nicht-Anker-Frames wird eine Orientierungspunktfeststellung im Kamerabereich wie in Gleichung (11) unten aufgeführt definiert. $$p_l^{c_j}={\left[x_l^{c_j},y_l^{c_j},z_l^{c_j}\mathrm{,1}\right]}^T=T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{il}}}{{\lambda }_l},\frac{\overline{v_{il}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T$$

  
• Der RGBD-Messwert des Merkmals ist gegeben durch $ƒ\left(p_l^{c_j}\right)=\left[\begin{array}{c}\frac{x_l^{c_j}}{z_l^{c_j}}\\ \frac{y_l^{c_j}}{z_l^{c_j}}\\ \frac{1}{z_l^{c_l}}\end{array}\right],$

  

  was in der Gleichung (12) unten resultiert. $$\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}=\left[\begin{array}{ccc}\frac{1}{z_l^{c_j}}& 0& -\frac{x_l^{c_j}}{z_l^{c_j{}^2}}\\ 0& \frac{1}{z_l^{c_j}}& -\frac{y_l^{c_j}}{z_l^{c_j{}^2}}\\ 0& 0& \frac{-1}{z_l^{c_j{}^2}}\end{array}\right]$$

  
• Die Jacobi-Matrizen des visuellen Restwerts können wie unten aufgeführt berechnet werden.
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Anker-Frame i, (3x3-Matrix), ist unten in Gleichung (13) aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die Rotation des Anker-Frame j, (3x3-Matrix), ist unten in Gleichung (14) aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\theta }_i}=-\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{\left[p_l^{b_i}\right]}_x$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Frame j, (3x3-Matrix), ist unten in Gleichung (15) aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_j}^w}=\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die Rotation des Frame j, (3x3-Matrix), ist unten in Gleichung (16) aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\theta }_i}=\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{\left[p_l^{b_j}\right]}_x$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die inverse Tiefe des Merkmals l, (3x1-Matrix), ist unten in Gleichung (17) aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\lambda }_l}=\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[-\frac{\overline{u_{il}}}{{\lambda }_l^2},-\frac{\overline{v_{il}}}{{\lambda }_l^2},-\frac{1}{{\lambda }_l^2}\right]}^T$$

  
• In dem vorstehend beschriebenen Szenario wird der Tiefenmesswert in dem Graphen unter Verwendung des ursprünglichen Messwerts im Anker-Keyframe i verwendet und der Messwert an den Keyframe j übertragen, um den Restwert für die Merkmals- und Tiefenmessung zu berechnen. Darüber hinaus kann ein Block für die Tiefenmessung im Anker-Keyframe hinzugefügt werden, um die Tiefenaktualisierung stärker einzuschränken.
• In diesem Fall ist das verwendete Messwertmodell nur der Tiefenmesswert, wie in Gleichung (18) unten aufgeführt ist. $$Z_l^{DP}={\left({\left|\left[R|p\right]\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right]\right|}_z\right)}^{-1}+n_{depth}$$

  
• Da an dem 2D-Merkmalsmesswert keine Aktualisierung vorgenommen wird, ist der Restwert für den Merkmalsmesswert null, wobei der Restwert zum Hinzufügen des Parameters auf nur den Restwert von dem Tiefenmesswert reduziert wird, wie in Gleichung (19) unten aufgeführt ist. $$r_{DP}\left({\lambda }_l\right)={\left({\left|{\left[\frac{\overline{u_{il}}}{{\lambda }_l},\frac{\overline{v_{il}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-Z_{\text{li}}^{DP}$$

  
• Dadurch wird die Jacobische in die inverse Tiefe von Merkmal 1, (3x1-Matrix), verwandelt, wie in Gleichung (20) unten aufgeführt ist. $$\frac{\partial r_{DP}\left({\lambda }_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,0,1}\right]}^T$$

  
• Viele Sensoren, die RGBD- und IMU-Sensoren beinhalten, sind unsynchronisiert. In diesem Fall liegt ein kleiner Zeitversatz zwischen Kamera- und IMU-Zeitstempeln vor. Dieser Zeitversatz gilt typischerweise als konstant, aber unbekannt. Für eine Online-Schätzung des Zeitversatzes wird dieser Zeitversatz modelliert, wie in Gleichung (21) unten aufgeführt ist. $$t_{IMU}=t_{CAM}+\mathrm{\Delta }t$$

  
• Hier ist t_IMU die IMU-Taktzeit, ist t_CAM die Kamerataktzeit, und ist Δt der Zeitversatz zwischen den beiden Takten. Δt wird als Random Walk abgebildet und unter Verwendung des Merkmals- und Tiefenmesswerts optimiert.
• Gemäß Ausführungsformen unter Verwendung der MSL-DVIO wird mit dem Zeitversatz zu einem beliebigen Zeitpunkt der Kamerazeitstempel kompensiert, um diesen auf den IMU-Zeitstempel zu bringen. Zum Berechnen eines Merkmalsmesswerts zu einem beliebigen Zeitpunkt in der Zukunft kann der Merkmalsmesswert unter Verwendung eines konstanten Geschwindigkeitsbewegungsmodells verbreitet werden, wie in Gleichung (22) unten aufgeführt ist. $$Z_{\text{liTD}}^{IMG}=Z_{\text{li}}^{IMG}-\left(\mathrm{\Delta }t-\mathrm{\Delta }{t}_i+T{R}_i\right)V_{li}^{IMG}$$

  
• Wenn ${\text{Z}}_{\text{liTD}}^{\text{IMG}}$

  

  der zeitversatzkompensierte 2D-Messwert des Orientierungspunkts l1 im Frame i ist, ist $Z_{\text{li}}^{IMG}$

  

  der ursprüngliche 2D-Messwert des Orientierungspunkts l im Frame i, ist Δt der aktuelle Schätzwert des Zeitversatzes, ist Δt_i der Schätzwert des Zeitversatzes, als der Kamera-Frame i verarbeitet wurde, und ist $V_{li}^{IMG}$

  

  die 2D-Geschwindigkeit des Merkmals, die während der Merkmalsnachverfolgung berechnet wird.
• TR_i ist der Zeitversatz, der aus der Rolling-Shutter-Kamera entsteht, der in der Gleichung (23) unten aufgeführt ist. $$T{R}_i=\frac{TR}{ROW}RO{W}_i$$

  
• Wenn TR die Rolling-Shutter-Auslesezeit ist, ist ROW die Zeilengesamtanzahl in dem Frame und ist OW_i die Zeile, in der das Merkmal festgestellt wird.
• Desgleichen ist ein Merkmalsmesswert für die Tiefe in Gleichung (24) unten aufgeführt. $$z_{\text{liTD}}^{DP}=z_{\text{li}}^{DP}-\left(\mathrm{\Delta }t-\mathrm{\Delta }{t}_i+T{R}_i\right)V_{li}^{DP}$$

  
• $V_{li}^{DP}$

  

  ist die Veränderungsrate der Merkmalstiefe in der Tiefenkarte, wie in Gleichung (25) unten aufgeführt ist. $$V_{li}^{DP}=\left(z_{\text{li}}^{DP}-z_{\text{l}\left(i-1\right)}^{DP}\right)\text{/}dt$$

  
• Unter Verwendung dieses neuen Messwerts wird der Restwert berechnet wie in den Gleichungen (26), (27) und (28) unten aufgeführt ist. $$r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)=\left[\begin{array}{c}{r}_{IMG}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)\\ r_{DP}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)\end{array}\right]$$

  

  $$r_{IMG}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)=g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{ilTD}}}{{\lambda }_l},\frac{\overline{v_{ilTD}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{ljTD}^{IMG}$$

  

  $$r_{DP}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)={\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{ilTD}}}{{\lambda }_l},\frac{\overline{v_{ilTD}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-\frac{1}{z_{lj\text{TD}}^{DP}}$$

  
• Ähnlich wie oben beschrieben ist eine Orientierungspunktfeststellung im Kamerabereich definiert wie in Gleichung (29) unten aufgeführt. $$p_{lTD}^{c_j}={\left[x_{lTD}^{c_j},y_{lTD}^{c_j},z_{lTD}^{c_j}\mathrm{,1}\right]}^T=T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{ilTD}}}{{\lambda }_l},\frac{\overline{v_{ilTD}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T$$

  
• Der RGBD-Messwert des Merkmals ist gegeben durch $ƒ\left(p_{lTD}^{c_j}\right)=\left[\begin{array}{c}\frac{x_{lTD}^{c_j}}{z_{lTD}^{c_j}}\\ \frac{y_l^{c_j}}{z_{lTD}^{c_j}}\\ \frac{1}{z_{lTD}^{c_j}}\end{array}\right]$

  
• Mit dem neuen Restwert können die Jacobi-Matrizen des visuellen Restwerts berechnet werden wie nachstehend aufgeführt ist.
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Anker-Frame i, (3x3-Matrix), ist in Gleichung (30) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_{lTD}^{c_j}\right)}{\partial p_{lTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die Rotation des Anker-Frame i, (3x3-Matrix), ist in Gleichung (31) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\theta }_i}=-\frac{\partial ƒ\left(p_{lTD}^{c_j}\right)}{\partial p_{lTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{\left[p_{lTD}^{b_i}\right]}_x$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Frame j, (3x3-Matrix), ist in Gleichung (32) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_j}^w}=-\frac{\partial ƒ\left(p_{lTD}^{c_j}\right)}{\partial p_{lTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die Rotation des Frame jj, (3x3-Matrix), ist in Gleichung (33) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\theta }_j}=\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{\left[p_{lTD}^{b_j}\right]}_x$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die inverse Tiefe des Merkmals l, (3x1-Matrix), ist in Gleichung (34) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\lambda }_l}=\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[-\frac{\overline{u_{ilTD}}}{{\lambda }_l^2},-\frac{\overline{v_{ilTD}}}{{\lambda }_l^2},-\frac{1}{{\lambda }_l^2}\right]}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf den Zeitversatz, (3x1-Matrix), ist in Gleichung (35) unten aufgeführt. $$\frac{\alpha r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_i\right)}{\partial \mathrm{\Delta }t}=\frac{\partial ƒ\left(p_{lTD}^{c_j}\right)}{\partial p_{lTD}^{c_j}}\frac{-1}{{\lambda }_l}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}\left[\begin{array}{c}{V}_{li}^{IMG}\\ 0\end{array}\right]+\left[\begin{array}{c}{V}_{lj}^{IMG}\\ 0\end{array}\right]$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die inverse Tiefe im Anker-Frame, (3x1-Matrix), ist in Gleichung (36) unten aufgeführt. $$\frac{\partial r_{DP}\left({\lambda }_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,0,1}\right]}^T$$

  
• Im VINS-Mono wird das Merkmal unter Verwendung der inversen Tiefe in dem Anker-Keyframe parametrisiert. Somit kann der Merkmalsmesswert im Anker-Frame nicht als ein Restwert zur Durchführung der Graphenoptimierung verwendet werden. Damit der Merkmalsmesswert in dem Anker-Frame als ein Restwert in dem Graphenoptimierungsproblem verwendet werden kann, wird die 2D-Position des Merkmals in dem Anker als Teil des Merkmalszustands hinzugefügt, wodurch die Merkmalszustandsparametrisierung auf 3D umgewandelt wird. Dieser neue Zustand des Gleitfensters ist in Gleichung (37) unten aufgeführt. $$X=\left[x^{nav};x^{feat}\right]$$

  
• Wenn $x^{nav}=\left[x_0^{nav};x_1^{nav};\dots ;x_n^{nav}\right]\text{ mit }{x}_k^{nav}=\left[\begin{array}{c}{x}_k^R\\ x_k^{SB}\end{array}\right],$

  

  Pose, Geschwindigkeit und Bias für k. Keyframes, ist x^feat = [f₀ ... f_m], f₁ = [u₁, v₁, λ₁], (u₁,v₁) der Schätzwert für die 2D-Position des Merkmals im Anker-Frame und ist λ₁ der Schätzwert der inversen tiefe des Merkmals im Anker-Frame.
• Da der Orientierungspunkt unter Verwendung der vollen 3D-Position des Merkmals in dem Anker-Frame geschätzt wird, wird diese Implementierung der MSL-DVIO als MSL-DVIO 3D bezeichnet.
• Mit der vorstehend beschriebenen 3D-Merkmalsparametrisierung kann der visuelle Restwert für sowohl Anker- als auch Nicht-Anker-Frames berechnet werden.
• Für den Nicht-Anker-Frame wird der Schätzwert des 3D-Merkmals in dem Anker-Frame zur Projektion im Nicht-Anker-Frame verwendet, um Restwerte zu berechnen, wie in den Gleichungen (38), (39) und (40) unten aufgeführt ist. $$r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)=\left[\begin{array}{c}{r}_{IMG}\left(x_i^R,x_j^R,{ƒ}_l^c\right)\\ r_{DP}\left(x_i^R,x_j^R,{ƒ}_l^c\right)\end{array}\right]$$

  

  $$r_{IMG}\left(x_i^R,x_j^R,{ƒ}_l\right)=g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{il}}{{\lambda }_l},\frac{v_{il}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{lj}^{IMG}$$

  

  $$r_{DP}\left(x_i^R,x_j^R,{ƒ}_l\right)={\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{il}}{{\lambda }_l},\frac{v_{il}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-Z_{lj}^{DP}$$

  
• Ebenso wird der visuelle Restwert für den Anker-Frame unter Verwendung des 3D-Schätzwerts in dem Anker-Frame berechnet, wie in den Gleichungen (41), (42) und (43) unten aufgeführt ist. $$r_{VIS}^{anchor}\left(u_{il},v_{il},{\lambda }_l\right)=\left[\begin{array}{c}{r}_{anchor}\left(u_{il},v_{il}\right)\\ r_{anchor}\left({\lambda }_l\right)\end{array}\right]$$

  

  $$r_{anchor}\left(u_{il},v_{il}\right)={\left[u_{il},v_{il}\right]}^T-Z_{li}^{IMG}$$

  

  $$r_{anchor}\left({\lambda }_i\right)={\lambda }_i-Z_{li}^{DP}$$

  
• Ähnlich wie oben beschrieben, wird die Jacobische für die 3D-Parametrisierung durch Verwendung des geschätzten 2D-Werts des Merkmals von einem Zustand berechnet, wie in Gleichung (44) unten aufgeführt ist. $$p_{I3D}^{c_j}={\left[x_{I3D}^{c_j},y_{I3D}^{c_j},z_{I3D}^{c_j}\mathrm{,1}\right]}^T=T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{il}}{{\lambda }_l},\frac{v_{il}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T$$

  
• Der RGBD-Messwert des Merkmals ist gegeben durch $\left(p_{I3D}^{c_j}\right)=\left[\begin{array}{c}\frac{x_{I3D}^{c_j}}{z_{I3D}^{c_j}}\\ \frac{y_{I3D}^{c_j}}{z_{I3D}^{c_j}}\\ \frac{1}{z_{I3D}^{c_j}}\end{array}\right],$

  

  was in der Gleichung (45) unten resultiert. $$\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}=\left[\begin{array}{ccc}\frac{1}{z_{I3D}^{c_j}}& 0& -\frac{x_{I3D}^{c_j}}{z_{I3D}^{c_j}{}^{{}^2}}\\ 0& \frac{1}{z_{I3D}^{c_j}}& -\frac{y_{I3D}^{c_j}}{z_{I3D}^{c_j}{}^{{}^2}}\\ 0& 0& \frac{-1}{z_{I3D}^{c_j}{}^{{}^2}}\end{array}\right]$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Anker-Frame i, (3x3-Matrix), ist in Gleichung (46) unten aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwertes in Bezug auf die Rotation des Anker-Frame i, (3x3-Matrix), ist in Gleichung (47) unten aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_i}=-\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{\left[p_{I3D}^{b_i}\right]}_x$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Frame j, (3x3-Matrix), ist in Gleichung (48) unten aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_j}^w}=-\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die Rotation des Frame j, (3x3-Matrix), ist in Gleichung (49) unten aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_j}=\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{\left[p_{I3D}^{b_j}\right]}_x$$

  
• Indem die 3D-Parametrisierung des Merkmals im Zustand vorgenommen wird, verändert sich die Jacobische des Restwerts in Bezug auf das Merkmal in eine 3x3-Matrix wie unten beschrieben ist.
• Die Jacobische für den Nicht-Anker-Frame ist in Gleichungen (50), (51) (52) unten aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial u_{il}}=\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\frac{1}{{\lambda }_l}\mathrm{,0,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial v_{il}}=\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\mathrm{0,}\frac{1}{{\lambda }_l}\mathrm{,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\lambda }_l}=\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[-\frac{u_{il}}{{\lambda }_l{}^2},-\frac{v_{il}}{{\lambda }_l{}^2},-\frac{1}{{\lambda }_l{}^2}\right]}^T$$

  
• Die Jacobische für den Anker-Frame ist in Gleichungen (53), (54) und (55) unten aufgeführt. $$\frac{\partial r_{VIS}^{anchor}\left(u_{il},v_{il},{\lambda }_l\right)}{\partial u_{il}}={\left[\mathrm{1,0,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{anchor}\left(u_{il},v_{il},{\lambda }_l\right)}{\partial v_{il}}={\left[\mathrm{0,1,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{anchor}\left(u_{il},v_{il},{\lambda }_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,0,1}\right]}^T$$

  
• Wie oben beschrieben werden durch die 3D-Parametrisierung volle 3D-Merkmalsmesswerte in dem Anker-Frame als Teil des Anker-Frame-Restwerts für die Graphenoptimierung hinzugefügt.
• Ähnlich wie der Zeitversatzschätzvorgang für die MSL-DVIO 1D kann der zeitversatzkorrigierte Messwert der 3D-Position des Merkmals berechnet werden. Die 2D-Position des Merkmals kann unter Verwendung der Zeitversatzschätzung ähnlich wie die MSL-DVIO 1D geschätzt werden, außer dass der geschätzte Wert der Gleichung (56) verwendet wird. $$Z_{\text{I}3\text{DiTD}}^{IMG}=Z_{\text{Ii}}^{IMG}=\left(\Delta t-\Delta t_i+T{R}_i\right)V_{li}^{IMG}$$

  
• Ebenso kann für der Tiefenmesswert der zeitversatzkorrigierte Messwert, wie in der MSL-DVIO 1D beschrieben, berechnet werden, wie in Gleichung (57) unten aufgeführt ist. $$z_{\text{I}3\text{DiTD}}^{DP}=z_{\text{li}}^{DP}-\left(\Delta t-\Delta t_i+T{R}_i\right)V_{li}^{DP}$$

  
• Unter Verwendung des neuen Messwerts kann der Restwert für den Nicht-Anker-Frame berechnet werden, wie in Gleichungen (58), (59) und (60) unten aufgeführt ist. $$r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)=\left[\begin{array}{c}{r}_{IMG}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)\\ r_{DP}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)\end{array}\right]$$

  

  $$r_{IMG}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)=g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{ilTD}}{{\lambda }_l},\frac{v_{ilTD}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{\text{I}3\text{DjTD}}^{IMG}$$

  

  $$r_{DP}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)={\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{ilTD}}{{\lambda }_l},\frac{v_{ilTD}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-\frac{1}{z_{\text{I}3\text{DjTD}}^{DP}}$$

  
• Für den Anker-Frame ist der Restwert ebenso in Gleichungen (61), (62) und (63) unten aufgeführt. $$r_{VIS}^{TDanchor}\left(u_{il},v_{il},{\lambda }_l\right)=\left[\begin{array}{c}{r}_{anchor}^{TD}\left(u_{il},v_{il}\right)\\ r_{anchor}^{TD}\left({\lambda }_l\right)\end{array}\right]$$

  

  $$r_{anchor}^{TD}\left(u_{il},v_{il}\right)={\left[u_{il},v_{il}\right]}^T-Z_{I3DiTD}^{IMG}$$

  

  $$r_{anchor}^{TD}\left({\lambda }_l\right)={\lambda }_l-Z_{I3DiTD}^{DP}$$

  
• Die Jacobische für dieses System wird berechnet wie nachstehend beschrieben ist.
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Anker-Frame i, (3x3-Matrix), ist in Gleichung (64) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die Rotation des Anker-Frame i, (3x3-Matrix), ist in Gleichung (65) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_i}=-\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{\left[p_{I3D}^{b_j}\right]}_x$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die World-Position des Frame j, (3x3-Matrix), ist in Gleichung (66) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_i}^w}=-\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$$

  
• Die Jacobische des visuellen Restwerts in Bezug auf die Rotation des Frame j, (3x3-Matrix), ist in Gleichung (67) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_j}=\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}{R}_{cb}{\left[p_{I3D}^{b_j}\right]}_x$$

  
• Indem die 3D-Parametrisierung des Merkmals im Zustand vorgenommen wird, verändert sich die Jacobische des Restwerts in Bezug auf das Merkmal in eine 3x3-Matrix, wie unten beschrieben ist.
• Die Jacobische für den Nicht-Anker-Frame ist in Gleichungen (68), (69) und (70) unten aufgeführt. $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial u_{il}}=\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\frac{1}{{\lambda }_l}\mathrm{,0,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial v_{il}}=\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\mathrm{0,}\frac{1}{{\lambda }_l}\mathrm{,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\lambda }_l}=\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[-\frac{u_{ilTD}}{{\lambda }_l{}^2},-\frac{v_{ilTD}}{{\lambda }_l{}^2},-\frac{1}{{\lambda }_l{}^2}\right]}^T$$

  
• Die Jacobische für den Anker-Frame ist in Gleichungen (71), (72) und (73) unten aufgeführt. $$\frac{\partial r_{VIS}^{TDanchor}\left(u_{il},v_{il},{ƒ}_l\right)}{\partial u_{il}}={\left[\mathrm{1,0,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{TDanchor}\left(u_{il},v_{il},{ƒ}_l\right)}{\partial v_{il}}={\left[\mathrm{0,1,0}\right]}^T$$

  

  $$\frac{\partial r_{VIS}^{TDanchor}\left(u_{il},v_{il},{ƒ}_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,0,1}\right]}^T$$

  
• Die Jacobische für den visuellen Restwert in Bezug auf den Zeitversatz, (3x1-Matrix), ist in Gleichung (74) unten aufgeführt. $$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial \Delta t}=\frac{\partial ƒ\left(p_{I3DTD}^{c_j}\right)}{\partial p_{I3DTD}^{c_j}}\frac{-1}{{\lambda }_l}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}\left[\begin{array}{c}{V}_{li}^{IMG}\\ 0\end{array}\right]+\left[\begin{array}{c}{V}_{lj}^{IMG}\\ 0\end{array}\right]$$

  
• In Tabelle 2 und 3 sind ein Systemrestwert und ein Jacobischer Vergleich der MSL-DVIO für alle vorstehend beschriebenen Kombinationen aufgeführt. Tabelle 2 - MSL-DVIO-Restwert und Jacobische ohne Zeitversatz

  	1D-Merkmalsparameter	3D-Merkmalsparameter
  Bildrestwert (Nicht-Anker)	$g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{il}}}{{\lambda }_l},\frac{\overline{v_{il}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{\text{lj}}^{IMG}$	$g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{il}}{{\lambda }_l},\frac{v_{il}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{lj}^{IMG}$
  Tiefenrestwert (Nicht-Anker)	${\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{il}}}{{\lambda }_l},\frac{\overline{v_{il}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^1-Z_{lj}^{DP}$	${\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{il}}{{\lambda }_l},\frac{v_{il}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-Z_{lj}^{DP}$
  Bildrestwert (Anker)	unzutreffend	${\left[u_{il},v_{il}\right]}^T-Z_{li}^{IMG}$
  Tiefenrestwert (Anker)	${\left({\left|{\left[\frac{\overline{u_{il}}}{{\lambda }_l},\frac{\overline{v_{il}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-Z_{\text{li}}^{DP}$	${\lambda }_l-Z_{li}^{DP}$
  Jacobische (Anker-Pose)	$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_I^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$	$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{l3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$
  Jacobische (Anker-Rotation)	$\begin{array}{l}\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\theta }_i}\hfill \\ =-\frac{\partial ƒ\left(p_I^{c_j}\right)}{\partial p_I^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_I}{\left[p_I^{b_I}\right]}_x\hfill \end{array}$	$\begin{array}{l}\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_i}\hfill \\ =-\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_I}{\left[p_{I3D}^{b_I}\right]}_x\hfill \end{array}$
  Jacobische (Nicht-Anker-Pose)	$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_j}^w}=-\frac{\partial ƒ\left(p_I^{c_j}\right)}{\partial p_I^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$	$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_j}^w}=-\frac{\partial ƒ\left(p_{I3D}^{c_j}\right)}{\partial p_{I3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$
  Jacobische (Nicht-Anker-Rotation)	$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\theta }_j}=-\frac{\partial ƒ\left(p_I^{c_j}\right)}{\partial p_I^{c_j}}{R}_{cb}{\left[p_I^{b_j}\right]}_x$	$\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_j}=\frac{\partial ƒ\left(p_{l3D}^{cj}\right)}{{\partial }_{l3D}^{cj}}{R}_{cb}{\left[p_{l3D}^{bj}\right]}_{\text{x}}$
  Jacobische (Nicht-Anker-Rotation)	$\begin{array}{l}\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\lambda }_l}\\ =\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\frac{\overline{u_{\mathrm{\iota }\text{l}}}}{{\lambda }_l{}^2},-\frac{\overline{v_{\mathrm{\iota }\text{l}}}}{{\lambda }_l{}^2},-\frac{1}{{\lambda }_l{}^2}\right]}^T\end{array}$	$\begin{array}{l}\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial u_{il}}\\ =\frac{\partial ƒ\left(p_{l3D}^{c_j}\right)}{\partial p_{l3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\frac{1}{{\lambda }_l\mathrm{,0,0}}\right]}^T\\ \frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial v_{il}}\\ =\frac{\partial ƒ\left(p_{l3D}^{c_j}\right)}{\partial p_{l3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\mathrm{0,}\frac{1}{{\lambda }_l}\mathrm{,0}\right]}^T\\ \frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\lambda }_l}\\ =\frac{\partial ƒ\left(p_{l3D}^{c_j}\right)}{\partial p_{l3D}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[-\frac{u_{il}}{{\lambda }_l{}^2},-\frac{v_{il}}{{\lambda }_l{}^2},-\frac{1}{{\lambda }_l{}^2}\right]}^T\end{array}$
  Jacobische (Anker-Merkmal)	$\frac{\partial r_{DP}\left({\lambda }_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,01}\right]}^T$	$\frac{\partial r_{VIS}^{anchor}\left(u_{il},v_{il},{\lambda }_l\right)}{\partial u_{il}}={\left[\mathrm{1,0,0}\right]}^T$ $\frac{\partial r_{VIS}^{anchor}\left(u_{il},v_{il},{\lambda }_l\right)}{\partial u_{il}}={\left[\mathrm{0,1,0}\right]}^T$ $\frac{\partial r_{VIS}^{anchor}\left(u_{il},v_{il},{\lambda }_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,0,1}\right]}^T$

  Tabelle 3 - MSL-DVIO-Restwert und Jacobische mit Zeitversatz

  	1 D-Merkmalsparameter	3D-Merkmalsptinmeter
  Bildrestwert (Nicht-Anker)	$g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{\mathrm{\iota }lTD}}}{{\lambda }_l},-\frac{\overline{v_{\mathrm{\iota }lTD}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{\text{ljTD}}^{IMG}$	$g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{\mathrm{\iota }lTD}}{{\lambda }_l},-\frac{v_{\mathrm{\iota }lTD}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{\text{l3DjTD}}^{IMG}$
  Tiefenrestwert (Nicht-Anker)	$\begin{array}{l}{\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{{\text{u}}_{\mathrm{\iota }lTD}}}{{\lambda }_l},-\frac{\overline{v_{\mathrm{\iota }lTD}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}\\ -\frac{1}{z_{\text{ljTD}}^{DP}}\end{array}$	$\begin{array}{l}{\left({\left|T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{u_{ilTD}}{{\lambda }_l},-\frac{v_{ilTD}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)}^{-1}-\frac{1}{z_{\text{l3Dj}TD}^{DP}}\\ \end{array}$
  Bildrestwert (Anker)	unzutreffend	${\left[u_{il},v_{il}\right]}^T-Z_{l3DiTD}^{IMG}$
  Tiefenrestwert (Anker)	$\begin{array}{l}\left({\left|{\left[\frac{\overline{{\text{u}}_{\mathrm{\iota }lTD}}}{{\lambda }_l},-\frac{\overline{v_{\mathrm{\iota }lTD}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right|}_z\right)-\frac{1}{z_{\text{ljTD}}^{DP}}\\ \end{array}$	${\lambda }_l-Z_{l3DiTD}^{DP}$
  Jacobische (Anker-Pose)	$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_{lTD}^{cj}\right)}{\partial p_{lTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$	$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_i}^w}=\frac{\partial ƒ\left(p_{l3DTD}^{cj}\right)}{\partial p_{l3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$
  Jacobische (Anker-Rotation)	$\begin{array}{l}\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_j}\\ =-\frac{\partial ƒ\left(p_{lTD}^{cj}\right)}{{\partial }_{p_{lTD}}^{cj}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{\left[p_{lTD}^{b_i}\right]}_{\text{x}}\end{array}$	$\begin{array}{l}\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_i}==-\frac{\partial ƒ\left(p_{l3DTD}^{c_j}\right)}{\partial p_{l3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_l}{\left[p_{l3D}^{b_i}\right]}_{\text{x}}\\ \end{array}$
  Jacobische (Nicht-Anker Pose)	$\begin{array}{l}\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial p_{b_j}^w}=-\frac{\partial ƒ\left(p_{lTD}^{c_j}\right)}{\partial p_{lTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T\\ \end{array}$	$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial p_{b_j}^w}=-\frac{\partial ƒ\left(p_{l3DTD}^{c_j}\right)}{\partial p_{l3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T$
  Jacobische (Nicht-Anker-Rotation)	$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\theta }_j}=-\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{\left[p_{lTD}^{b_j}\right]}_{\text{x}}$	$\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\theta }_j}=-\frac{\partial ƒ\left(p_{l3DTD}^{c_j}\right)}{\partial p_{l3DTD}^{c_j}}{R}_{cb}{\left[p_{l3D}^{b_j}\right]}_{\text{x}}$
  Jacobische (Nicht-Anker-Merkmal)	$\begin{array}{l}\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\lambda }_l}\\ =\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\frac{\overline{{\text{u}}_{\mathrm{\iota }lTD}}}{{\lambda }_l{}^2},-\frac{\overline{{\text{u}}_{\mathrm{\iota }lTD}}}{{\lambda }_l{}^2},-\frac{1}{{\lambda }_l{}^2}\right]}^T\end{array}$	$\begin{array}{l}\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial u_{il}}=\frac{\partial ƒ\left(p_{l3DTD}^{c_j}\right)}{\partial p_{l3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\frac{1}{{\lambda }_l}\mathrm{,0,0}\right]}^T\\ \frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial v_{il}}=\frac{\partial ƒ\left(p_{l3DTD}^{c_j}\right)}{\partial p_{l3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\mathrm{0,}\frac{1}{{\lambda }_l}\mathrm{,0}\right]}^T\\ \frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial {\lambda }_l}\\ =\frac{\partial ƒ\left(p_{l3DTD}^{c_j}\right)}{\partial p_{l3DTD}^{c_j}}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{\left[\frac{u_{ilTD}}{{\lambda }_l{}^2},-\frac{v_{ilTD}}{{\lambda }_l{}^2},-\frac{1}{{\lambda }_l{}^2}\right]}^T\end{array}$
  Jacobische (Anker-Merkmal)	$\frac{\partial r_{DP}\left({\lambda }_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,0,1}\right]}^T$	$\frac{\partial r_{VIS}^{TDanchor}\left(u_{il},v_{il},{ƒ}_l\right)}{\partial u_{il}}={\left[\mathrm{1,0,0}\right]}^T$ $\frac{\partial r_{VIS}^{TDanchor}\left(u_{il},v_{il},{ƒ}_l\right)}{\partial v_{il}}={\left[\mathrm{0,1,0}\right]}^T$ $\frac{\partial r_{VIS}^{TDanchor}\left(u_{il},v_{il},{ƒ}_l\right)}{\partial {\lambda }_l}={\left[\mathrm{0,0,1}\right]}^T$
  Jacobische (Zeitversatz)	$\begin{array}{l}\frac{\partial r_{VIS}^{TD}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial \Delta t}\\ =-\frac{\partial ƒ\left(p_{lTD}^{c_j}\right)}{{\partial }_{lTD}^{c_j}}\frac{-1}{{\lambda }_l}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}\left[\begin{array}{c}{V}_{li}^{IMG}\\ 0\end{array}\right]\\ +\left[\begin{array}{c}{V}_{lj}^{IMG}\\ 0\end{array}\right]\end{array}$	$\begin{array}{l}\frac{\partial r_{VIS}^{non}\left(x_i^R,x_j^R,{ƒ}_l\right)}{\partial \Delta t}\\ =-\frac{\partial ƒ\left(p_{l3DTD}^{c_j}\right)}{\partial p_{l3DTD}^{c_j}}\frac{-1}{{\lambda }_l}{R}_{cb}{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}\left[\begin{array}{c}{V}_{li}^{IMG}\\ 0\end{array}\right]+\left[\begin{array}{c}{V}_{lj}^{IMG}\\ 0\end{array}\right]\\ \end{array}$

• Die Veränderungen an der Graphenoptimierung in der MSL-DVIO sind oben beschrieben. Ausführungsformen sind bezogen auf die 1D- und 3D-Merkmalsparametrisierung bereitgestellt. Zur Schätzung des initialen Werts für den Merkmalsparameter ist der Schätzvorgang in zwei separate Blöcke unterteilt. Ein erster Block schätzt die inverse Tiefenschätzung für das Merkmal, und ein zweiter Block schätzt den initialen Wert des 2D-Merkmalsmesswerts im Anker-Frame.
• Eine Merkmalstiefe wird in Bezug auf den Anker-Frame berechnet. In der MSL-DVIO wird der initiale Schätzwert der Merkmalstiefe unter Verwendung des Gleitfenstermesswerts und Verwendung des Merkmalstiefenmesswerts von der Tiefenkarte berechnet.
• In dem ersten Schritt zur Schätzung des initialen Tiefenschätzwerts wird der Tiefenmesswert für den Merkmalspunkt von der Tiefenkarte zur Berechnung der Merkmalstiefe verwendet. K_l = {k₁, k₂, ..., k_n} ist der 3D-Merkmalsmesswert für einen einzigen Orientierungspunkt l in dem vollen Gleitfenster. Hier entspricht $k_1=\left(\overline{u_{1l}},\overline{v_{1l}},z_{1l}^{DP}\right)$

  

  den Merkmalsmesswert des Orientierungspunkts l in dem Anker-Frame, und k₂, ..., k_n entspricht dem Merkmalsmesswert des Orientierungspunkts l in dem Nicht-Anker-Frame. $X^R=\left\{x_1^R,\dots ,x_n^R\right\}$

  

  ist die Roboterpose für das Gleitfenster, für welches die Graphenoptimierung durchgeführt wird. Unter Verwendung der Gleitfensterpose und des Merkmalsmesswerts für den Orientierungspunkt l, wird ein 3D-Punkt auf den Anker-Frame projiziert, wie in Gleichung (75) unten aufgeführt ist. $$\begin{array}{c}=\left|\begin{array}{c}\widehat{u_{1l}}{z}_{1l}^{\widehat{DP}}\\ \widehat{v_{1l}}{z}_{1l}^{\widehat{DP}}\\ z_{1l}^{\widehat{DP}}\end{array}\right|=T_{cb}{T}_{b_{1}w}{T}_{w{b}_j}{T}_{bc}\left|\begin{array}{c}\overline{u_{jl}}{z}_{jl}^{DP}\\ \overline{v_{jl}}{z}_{jl}^{DP}\\ z_{jl}^{DP}\end{array}\right|\\ fürallejFrameimGleitfenster.\end{array}$$

  
• Ein Anker-Frame-3D-Merkmals-Schätzwert wird als $\widehat{k_1}=\left(\widehat{u_{1l}},\widehat{v_{1l}},z_{1l}^{\widehat{DP}}\right)=\left(\frac{A_1}{A_3},\frac{A_2}{A_3},A_3\right)$

  

  $$

  

  erzeugt. Unter Verwendung des 2D-Schätzwerts $\left(\widehat{u_{1l}},\widehat{v_{1l}}\right)$

  

  wird ein Reprojektionsfehler des Merkmals auf dem Anker-Frame erzeugt. Auf diesen Reprojektionsfehler wird ein Schwellwert angewendet, so dass eine Einordnung in „Inliers“ und „Outliers“ bzw. Ausreißer vorgenommen werden kann. Die Feature-Tiefe wird unter Verwendung des Mittelwerts aller durch Inlier geschätzten Anker-Frame-Tiefen initialisiert.
• Zur Schätzung des zweidimensionalen Positionsschätzwerts der Tiefe und des Merkmals im Anker-Frame unter Verwendung von ausschließlich der Gleitfensterdaten und der zweidimensionalen Merkmalsposition wird ein direkter linearer Transformations- (DLT-) Algorithmus verwendet. Das Problem wird in der Form AX = 0 formuliert und unter Verwendung einer Einzelwertzerlegung gelöst. Zum Beispiel kann der Orientierungspunkt $l=\left(x_l^{anch},y_l^{anch},z_l^{anch}\right)$

  

  ein 3D-Punkt sein, der in dem Kamera-Anker-Frame für das Gleitfenster $X^R=\left\{x_1^R,\dots ,x_n^R\right\}$

  

  festgestellt wird, und kann einen Merkmalsmesswert als K_l = {k₁, k₂, ..., k_n) aufweisen. $x_1^R$

  

  ist der Anker-Frame und k₁ ist der Merkmalsmesswert im Anker-Frame. Unter Verwendung der Kameraprojektionsmatrix wird die Beziehung der Gleichung (76) bereitgestellt. $$\left[\begin{array}{c}\overline{u_j{}_l}\\ \overline{v_j{}_l}\\ 1\end{array}\right]=\alpha T\left(x_j^R\right)\left[\begin{array}{c}{x}_l^{anch}\\ y_l^{anch}\\ z_l^{anch}\\ 1\end{array}\right]$$

  
• Wenn j jeder Keyframe in dem Gleitfenster ist, ist $T\left(x_j^R\right)$

  

  eine relative Transformationsmatrix zwischen dem Keyframe und dem Anker-Frame unter Verwendung des Rotationsvektors und des Translationsvektors in $x_j^R$

  

  und $x_1^R$

  

  , wobei α der Skalenfaktor ist.
• Das Kreuzprodukt von zwei Vektoren in der gleichen Richtung ist null, wobei anhand der Beziehung Gleichung (77) bereitgestellt wird. $$\left[\begin{array}{c}\overline{u_j{}_l}\\ \overline{v_j{}_l}\\ 1\end{array}\right]\times T\left(x_j^R\right)\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$

  
• In der Gleichung (77) wird $T\left(x_j^R\right)$

  

  auf seine erweiterte Form der 3×4-Matrix abgeleitet, wobei die Notation minimiert wird, wie in Gleichung (78) unten aufgeführt ist. $$T\left(x_j^R\right)\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right]=\left[\begin{array}{llll}{t}_1\hfill & t_2\hfill & t_3\hfill & t_4\hfill \\ t_5\hfill & t_6\hfill & t_7\hfill & t_8\hfill \\ t_9\hfill & t_{10}\hfill & t_{11}\hfill & t_{12}\hfill \end{array}\right]\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right]=\left[\begin{array}{c}{T}_1^{T}L\\ T_2^{T}L\\ T_3^{T}L\end{array}\right]$$

  
• Wobei: $$T_1^T=\left[t_1{t}_2{t}_3{t}_4\right],T_1^T=\left[t_5{t}_6{t}_7{t}_8\right]$$

  

  und $$T_1^T=\left[t_9{t}_{10}{t}_{11}{t}_{12}\right],\text{und }L=\left[\begin{array}{c}{x}_l^w\\ y_l^w\\ z_l^w\\ 1\end{array}\right].$$

  
• Der Kreuzproduktterm ist in Gleichung (79) unten aufgeführt. $$\left[\begin{array}{c}\overline{u_{jl}}\\ \overline{v_{jl}}\\ 1\end{array}\right]\times \left[\begin{array}{c}{T}_1^{T}L\\ T_2^{T}L\\ T_3^{T}L\end{array}\right]=\left[\begin{array}{c}{v}_{jl}{T}_3^{T}L-T_2^{T}L\\ T_1^{T}L-u_{jl}{T}_3^{T}L\\ u_{jl}{T}_2^{T}L-v_{jl}{T}_1^{T}L\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$

  
• Die Gleichung (79) kann weiter reduziert werden, da die dritte Zeile die lineare Kombination aus der ersten und der zweiten Zeile ist, was in der Gleichung (80) für eine einzelne Feststellung resultiert, wie unten aufgeführt ist. $$\left[\begin{array}{c}\overline{v_{jl}}{T}_3^{T}L-T_2^{T}L\\ T_1^{T}L-\overline{u_{jl}}{T}_3^{T}L\end{array}\right]=\left[\begin{array}{c}\overline{v_{jl}}{T}_3^T-T_2^T\\ T_1^T-\overline{u_{jl}}{T}_3^T\end{array}\right]L=\left[\begin{array}{c}0\\ 0\end{array}\right]$$

  
• Die Gleichung (80) kann als Gleichung (81) unten geschrieben werden. $$A_{j}L=0$$

  
• Die Feststellungen für das jeweilige Merkmal sind gestapelt, wobei ein System aus linearen Gleichungen erzeugt wird, das die Form AL = 0 aufweisen wird. Zur Auflösung dieses homogenen linearen Systems wird ein Minimieren des algebraischen Fehlers der kleinsten Quadrate unter Verwendung einer Einzelwertzerlegung gelöst, wenn die Lösung der Eigenvektor ist, der dem kleinsten Eigenwert von A^TA entspricht, wie unten in den Gleichungen (82) und (83) aufgeführt ist. $$U\Sigma V=SVD\left(A\right)$$

  

  $$\widehat{L}=\left[\begin{array}{c}\widehat{I_1}\\ \widehat{I_2}\\ \widehat{I_3}\\ \widehat{I_4}\end{array}\right]=\left[\begin{array}{c}{V}_{\mathrm{1,4}}\\ V_{\mathrm{2,4}}\\ V_{\mathrm{3,4}}\\ V_{\mathrm{4,4}}\end{array}\right]$$

  
• Der Schätzwert des 2D-Messwerts und die Tiefe des Orientierungspunkts in dem Anker-Frame werden erzeugt, wie unten in Gleichung (84) aufgeführt ist. $$\widehat{u_l}=\frac{\widehat{I_1}/\widehat{I_4}}{\widehat{z_I}},\widehat{v_I}=\frac{\widehat{I_2}/\widehat{I_4}}{\widehat{z_I}},\widehat{z_I}=\frac{\widehat{I_3}}{\widehat{I_4}}$$

  
• Diese Schätzwerte werden dann zur Initialisierung der Zustandsvariablen im Graphenoptimierungsproblem verwendet. In den vorstehend beschriebenen Verfahren hängt der Initialwert der Merkmalsparameter vom Merkmalsmesswert und den Gleitfensterposen ab. Im Fall der Tiefenschätzung wird die gesamte Tiefe mit dem Mittelungswert des Einlieger-Tiefenmesswerts initialisiert, wobei nur die Merkmale, die unter Verwendung des ersten Schritts nicht geschätzt werden, unter Verwendung des zweiten Verfahrens initialisiert werden. Im Fall einer zweidimensionalen Merkmalsparameter-Initialisierung kommt nur das zweite Verfahren zur Initialisierung in Frage.
• Wenn in den Gleitfensterposen große Fehler vorliegen, ist das vorstehend beschriebene Verfahren für eine fehlerhafte Initialisierung anfällig. Da die Schätzung von der Qualität der Gleitfensterposen abhängt, ist dieses Problem in dem ersten Optimierungsdurchlauf als Teil des Initialisierungsschritts ausgeprägter. Um ein fehlerhaftes Verhalten des MSL-DVIO-Systems zu vermeiden, werden die Optimierungsschritte durch Fixe ergänzt, die nur für den erstmaligen Optimierungsdurchlauf als Teil der Initialisierungsschritte vorgesehen sind.
• Zur Durchführung einer Tiefenstützung bzw. des Depth-Aiding wird der Tiefenkarten-Frame verwendet. Diese Tiefenkarten-Frames werden unter Verwendung eines Tiefenerfassungsmoduls erzeugt. Es gibt zwei Arten von Tiefenerfassungsmodulen. Eine erste Art ist ein passiver Tiefensensor, und eine zweite Art betrifft einen aktiven Tiefensensor. Ein Beispiel für einen passiven Tiefensensor beinhaltet eine Tiefe von einem Stereokamerapaar, wobei ein Beispiel eines aktiven Tiefensensors eine TOF-Kamera oder eine Strukturlichtkamera beinhaltet.
• Alle Sensoren schätzen die Szenentiefe basierend auf Messmodellen. Da sich die Ausführungsformen schwerpunktmäßig auf RGBD-Sensoren beziehen, sind aktive Sensoren hierin ebenfalls darauf fokussiert.
• 3 ist ein Diagramm, dass eine TOF-Kamera gemäß einer Ausführungsform darstellt. Die TOF-Kamera beinhaltet einen Controller 302, einen Sensor 304 und einen Infrarot- (IR-) Emitter 306. Eine Phasendifferenz 308 zwischen einem übertragenen Signal 310 und einem empfangenen Signal 312 von einem Objekt 314 wird zur Schätzung einer Tiefe verwendet.
• 4 ist ein Diagramm, das Strukturlichtsensoren gemäß einer Ausführungsform darstellt. Ein Punktmuster unter Verwendung einer Grundlinie 402 und einer Brennweite 404 wird zum Berechnen eines Disparitätsmesswerts zwischen einem Projektor 406 und einer Kamera 408 verwendet, welcher dann zur Schätzung einer Tiefe 410 von einem Punkt 412 verwendet wird.
• Wie vorstehend in zwei stark vereinfachten Beispielen der Messmodelle beschrieben, ist der Tiefenmesswert von einem Sensor in Bezug auf die Art des verwendeten Sensors sehr spezifisch. Um diesen Tiefenmesswert zusammen mit dem zweidimensionalen Tiefenmesswert zur Durchführung einer Graphenoptimierung zu verwenden, wird ein Modell auf einer Pro-Sensor-Basis für das Messrauschen benötigt.
• Während in der MSL-DVIO eine Tiefenstützung bzw. Depth-Aiding durchgeführt wird, wird davon ausgegangen, dass ein additives Gaußsches Modell für das Messrauschen für den inverse Tiefenmesswert verwendet werden soll. Diese Art von Modell kann sich für unterschiedliche Arten von Sensoren möglicherweise als nicht akkurat erweisen. Zum Beispiel kann ein Sensor mit einem aktiven IR-Stereosensor verwendet werden. Dieser Sensor verwendet sowohl ein IR- als auch ein sichtbares Licht, um eine dichtes Stereosehen basierend auf Merkmalen durchzuführen, die für ihn sichtbar sind. Da der Sensor eine Disparität als einen Messwert zur Schätzung der Tiefe von einem Merkmalspunkt verwendet, ist die Verwendung eines additiven Gaußschen Rauschen für die inverse Tiefe ein zulässiges Modell für Tiefenunsicherheit.
• In dem Graphenoptimierungsproblem wird zur Beibehaltung einer feststehenden Größe für das Optimierungsproblem ein Gleitfenster zum Durchführen der Graphenoptimierung verwendet. Bei diesem Lösungsansatz werden eine feststehende Größe und eine kürzere Verarbeitungszeit in dem nichtlinearen Optimierungsschritt beibehalten. Sobald ein Gleitfenster mit einer maximalen Anzahl von Knoten befüllt ist, die die feststehende Größe des Problems aufrechterhalten können, wird eine Marginalisierung durchgeführt, in der ein bestimmter Teilsatz von Knoten X_m fallengelassen werden und ein Graph mit den verbliebenen Knoten X_r (X_m ∪ X_r = X, X_m ∩ X_r = ∅) gebildet wird.
• Die Kostenfunktion, die in der Batch-Optimierung minimiert wird, ist von der in Gleichung (85) unten gezeigten Form. $$C\left(X\right)={\stackrel{\rightharpoonup }{\text{e}}}^{\text{T}}\left(X\right)\Omega \stackrel{\rightharpoonup }{e}\left(X\right)$$

  
• Hier ist $\stackrel{\rightharpoonup }{\text{e}}\left(X\right)$

  

  ein Säulenvektor einer Länge L, welche alle Einschränkungen unter X enthält, wobei Ω eine L × L -Diagonalmatrix als das kombinierende Gewicht für jede Einschränkung ist.
• Nach der Marginalisierung findet die Batch-Optimierung nur über den verbliebenen Variablen X_r statt. Die revidierte Kostenfunktion wird bezeichnet als F(X_r). Der Zweck der Marginalisierung ist die Identifikation einer solchen revidierten Kostenfunktion F(X_r), die dieselbe optimale Auswahl von X_r wie C(X) ermöglicht.
• Dadurch wird F(X_r) das nachstehende Erfordernis auferlegt: Finde F(X_r), so dass Gleichung (86) ist wie unten. $$\text{F}\left(X_r\right)=\underset{X_m|X_r}{\text{min}}\text{C}\left(\left[\begin{array}{c}{X}_m\\ X_r\end{array}\right]\right)$$

  
• Der Vorgang der Marginalisierung kann dann in den nachfolgenden Schritten zusammengefasst werden.
• Gegeben seien eine Informationsmatrix H und ein Fehlervektor $\stackrel{\rightharpoonup }{b},$

  

  wie in Gleichung (87) unten gezeigt: $$H=\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right],\stackrel{\rightharpoonup }{b}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_m\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]$$

  
• Der Marginalisierungsvorgang berechnet die neue Informationsmatrix und den Fehlervektor wie in Gleichung (88) unten gezeigt. $$H_{new}=H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}$$

  

  $${\stackrel{\rightharpoonup }{b}}_{new}={\stackrel{\rightharpoonup }{b}}_r-H_{rm}{H}_{mm}^{-1}{\stackrel{\rightharpoonup }{b}}_m$$

  
• Zur Durchführung des Marginalisierungsvorgangs wird die Inverse der Informationsmatrix bezogen auf den marginalisierten Zustand Hmm benötigt. Abhängig davon, wie groß der marginalisierte Zustand ist, kann dies sehr zeitaufwändig sein. In Systemen, wie z. B. VINS-Mono, wird eine Eigenzerlegung der kompletten Matrix durchgeführt, um die Inverse der Matrix zu berechnen, was ein sehr zeitaufwendiger Prozess sein kann und die Echtzeitleistung des endgültigen Algorithmus beeinträchtigen kann.
• Dementsprechend werden zwei Änderungen vorgenommen, um den Marginalisierungsprozess für MSL-DVIO 1D zu optimieren. Zunächst wird das Problem in Blöcke unterteilt: 1) Marginalisierung aller Orientierungspunkte; und 2) Marginalisierung der Keyframe-Pose, der Geschwindigkeit und der Bias-Terme. Die Marginalisierung aller Orientierungspunkte ist einfach, da eine Diagonalmatrix von der eindimensionalen Merkmalsparametrisierung bereitgestellt wird. Das Marginalisierungsproblem kann als Gleichung (89) unten umgeschrieben werden, es gegeben seien die Matrix H und der Fehlervektor $\stackrel{\rightharpoonup }{b}.$

  

  $$H=\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]=\left[\begin{array}{ccc}{H}_{mm11}& H_{mm12}& H_{mr1}\\ H_{mm21}& H_{mm22}& H_{mr2}\\ H_{rm1}& H_{rm2}& H_{rr}\end{array}\right],\stackrel{\rightharpoonup }{b}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_m\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_{m1}\\ {\stackrel{\rightharpoonup }{b}}_{m2}\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]$$

  
• H_mm11 ist die Informationsmatrix für Orientierungspunkte, die eine einfache Diagonalmatrix $H_{mm11}=diag\left({\stackrel{\rightharpoonup }{h}}_{mm11}\right)$

  

  ist.
• Im zweiten Schritt werden die Keyframe-Pose, die Geschwindigkeit und die Bias-Terme marginalisiert, wie in Tabelle 4 unten gezeigt ist. Tabelle 4 Blockmarginalisierung für die MSL-DVIO 1D

  Schritt 1): Marginalisiere Orientierungspunkte

  $H=\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]=\left[\begin{array}{ccc}{H}_{mm11}& H_{mm12}& H_{mr1}\\ H_{mm21}& H_{mm22}& H_{mr2}\\ H_{rm1}& H_{rm2}& H_{rr}\end{array}\right],\stackrel{\rightharpoonup }{b}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_m\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_{m1}\\ {\stackrel{\rightharpoonup }{b}}_{m2}\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]$

  $H_{mm11}^{-1}=diag\left(\frac{1}{{\stackrel{\rightharpoonup }{h}}_{mm11}}\right)$

  $\begin{array}{c}{H}_{new-temp}=\left[\begin{array}{cc}{H}_{mm22}& H_{mr2}\\ H_{rm2}& H_{rr}\end{array}\right]-\left[\begin{array}{c}{H}_{mm21}\\ H_{rm1}\end{array}\right]H_{mm11}^{-1}\left[\begin{array}{cc}{H}_{mm12}& H_{mr1}\end{array}\right]=\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\\ \end{array}$

  ${\stackrel{\rightharpoonup }{b}}_{new-temp}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_{m2}\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]-\left[\begin{array}{c}{H}_{mm21}\\ H_{rm1}\end{array}\right]H_{mm11}^{-1}{\stackrel{\rightharpoonup }{b}}_{m1}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_1\\ {\stackrel{\rightharpoonup }{b}}_2\end{array}\right]$

  Schritt 2): Marginalisiere andere Terme

  $H_{new}=H_{22}-H_{21}{H}_{11}^{-1}{H}_{12}$

  ${\stackrel{\rightharpoonup }{b}}_{new}={\stackrel{\rightharpoonup }{b}}_2-H_{21}{H}_{11}^{-1}{\stackrel{\rightharpoonup }{b}}_1$

• Zur Durchführung der Marginalisierung der Keyframe-Pose, der Geschwindigkeit und der Bias-Terme wird der Sicherheits-Cholesky-Algorithmus verwendet. Dieser ermöglicht eine robustere und schnellere Verarbeitung für den zweiten Schritt der Blockmarginalisierung in der MSL-DVIO 1D. Die Zusammenfassung dieses Algorithmus ist unten in Tabelle 5 aufgeführt. Tabelle 5 - Sicherheits-Cholesky-Zerlegung

  H ist eine N × N symmetrische reelle positive endgültige Matrix. Initialisiere L , so dass sie eine N × N -Nullmatrix ist. Denn j = 1:N

  $d_j=H_{j,j}-{\displaystyle \sum _{k=1}^{j-1}{L}_{j,k}^2}$

  Wenn d j > ∈2

  $L_{j,j}=\sqrt{d_j}$

  Denn i = j + 1:N

  $L_{i,j}=\frac{1}{L_{j,j}}\left(H_{i,j}-{\displaystyle \sum _{k=1}^{j-1}{L}_{i,k}{L}_{j,k}}\right)$

  Sonst falls d j > -∈2

  $L_{j,j}=\in ,L_{i,j}=0$

  Sonst Rückgabe von FALSE; //Choleskv-Zerlegung scheitert

• Im Fall von MSL-DVIO 3D kann ein einziger einfacher Schritt nicht zur Berechnung der Inversen des Blocks für den Orientierungspunkt durchgeführt werden, da jeder Orientierungspunkt mit 3 Zuständen in dem Graphen korreliert. Somit wird die Inverse der 3x3-Matrix für jeden Orientierungspunkt der Reihe nach gehandhabt. Somit verändert sich der Block-um-Block-Marginalisierungsvorgang für die MSL-DVIO 3D-Veränderungen in den in Tabelle 6 unten gezeigten Vorgang. Tabelle 6 - Blockmarginalisierung für MSL DVIO 3D

  n Orientierungspunkt, der zu marginalisieren ist. H 3×3 und ${\stackrel{\rightharpoonup }{b}}_{3\times 3}$ bezeichnet die Informationsmatrix und den Restwert für den ersten Orientierungspunkt, wobei folgende Matrizen bereitgestellt werden:

  $H=\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]=\left[\begin{array}{ccc}{H}_{3\times 3}& H_{mm12}& H_{mr1}\\ H_{mm21}& H_{mm22}& H_{mr2}\\ H_{rm1}& H_{rm2}& H_{rr}\end{array}\right],\stackrel{\rightharpoonup }{b}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_m\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_{3\times 3}\\ {\stackrel{\rightharpoonup }{b}}_{m2}\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]$

  $H_{new-temp}=\left[\begin{array}{cc}{H}_{mm22}& H_{mr2}\\ H_{rm2}& H_{rr}\end{array}\right]-\left[\begin{array}{c}{H}_{mm21}\\ H_{rm1}\end{array}\right]H_{3\times 3}^{-1}\left[\begin{array}{cc}{H}_{mm12}& H_{mr1}\end{array}\right]=\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]$

  ${\stackrel{\rightharpoonup }{b}}_{new-temp}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_{m2}\\ {\stackrel{\rightharpoonup }{b}}_r\end{array}\right]-\left[\begin{array}{c}{H}_{mm21}\\ H_{rm1}\end{array}\right]H_{3\times 3}^{-1}{\stackrel{\rightharpoonup }{b}}_{3\times 3}=\left[\begin{array}{c}{\stackrel{\rightharpoonup }{b}}_1\\ {\stackrel{\rightharpoonup }{b}}_2\end{array}\right]$

  Schritt 2): Marginalisiere andere Terme

  $H_{new}=H_{22}-H_{21}{H}_{11}^{-1}{H}_{12}$

  ${\stackrel{\rightharpoonup }{b}}_{new}={\stackrel{\rightharpoonup }{b}}_2-H_{21}{H}_{11}^{-1}{\stackrel{\rightharpoonup }{b}}_1$

• Wie vorstehend beschrieben wird jeder Orientierungspunkt für die MSL-DVIO 3D separat marginalisiert. Bei diesem Lösungsansatz wird in Bezug auf die Verarbeitungszeit eine große Verbesserung erreicht, da der Umfang der Matrixinversion gering ist, was im Vergleich zu dem Versuch, eine große Matrix in einem einzigen Schritt zu invertieren, viel schneller gelingt.
• Der Plattformentwurf des VINS-Mono-Systems kann für den vorstehend beschriebenen Systementwurf verwendet werden. VINS-Mono weist eine dem aktuellen Stand der Technik entsprechende VIO-Implementierung unter Verwendung eines Graphenoptimierungslösungsansatzes auf. Das aktuelle System kann obendrein noch um eine Tiefenstützung bzw. Depth-Aiding ergänzt werden, um die beim Implementieren eines kompletten VIO-Systems notwendige Nachbearbeitung zu vermeiden.
• Indem die nachstehenden Änderungen aufgenommen werden, wird der visuelle Teil des VINS-Mono um eine Tiefenstützung bzw. Depth-Aiding ergänzt.
• Eine Tiefendaten-Eingabe/Ausgabe (I/O) erfordert das Lesen des Tiefenmesswerts und eine Verarbeitung dieses Messwerts synchronisiert mit dem 2D-Merkmalsmesswert.
• Der Tiefenmesswert erfordert das Berechnen des Tiefenmesswerts, der einem jeweiligen 2D-Merkmalsmesswert zugeordnet ist, und das Hinzufügen dieses Messwerts zu der aktuellen Verarbeitungs-Pipeline.
• Die Optimierung berechnet den Tiefenrestwert basierend auf dem Tiefenmesswert und den Zustandsparametern. Dieser Restwert kommt als ein Parameterblock im Graphenoptimierungsproblem hinzu.
• Die Tiefendaten-I/O, der Tiefenmesswert und der Tiefenrestwert sowie -Optimierung werden separat für eine Tiefenstützung bzw. ein Depth-Aiding für den Anker-Keyframe, eine Tiefenstützung bzw. Depth-Aiding für den Nicht-Anker-Keyframe, eine Tiefenstützung bzw. ein Depth-Aiding für unsynchronisierte Sensoren und eine Tiefenstützung bzw. ein Depth-Aiding für eine 3D-Merkmalsparametrisierung durchgeführt.
• 5 ist ein Diagramm, das einen Systemfluss der MSL-DVIO gemäß einer Ausführungsform darstellt.
• In einem Messwertverarbeitungsblock 502 werden die eingegebenen Daten von einer IMU 504, einer RGB-Kamera 506 und einem Tiefensensor 508 genommen und dann separat verarbeitet. Die RGB-Kamera 506 und der Tiefensensor 508 können als ein RGB-Sensor kombiniert werden. Für den IMU-Datenstrom von der IMU 504 wird die IMU-Vorintegration an dem Messwertverarbeitungsblock 502 durchgeführt. Für den RGB-Kamera-Frame von der RGB-Kamera 506 werden eine Merkmalserfassung und -nachverfolgung an dem Messwertverarbeitungsblock 502 durchgeführt. Für die Tiefenkarte von dem Tiefensensor 508 wird die Tiefe für das Merkmal erzeugt, das in dem RGB-Kamera-Frame am Messwertverarbeitungsblock 502 gemessen wird.
• In einem Initialisierungsblock 510 werden ein IMU-Vorintegrationsterm und eine 2D-Merkmalsspur von dem Messwertverarbeitungsblock 502 empfangen, wobei eine ausschließlich visuelle Struktur von einer Bewegung durchgeführt wird. Eine visuelle Trägheitsausrichtung wird zudem durchgeführt, um einen initialen Keyframe für eine Gleitfensterkarte zu erzeugen.
• In einem Bewegungs-BA-Block 512 werden eine 2D-Merkmalsspur und der IMU-Vorintegrationsterm von dem Messwertverarbeitungsblock 502 empfangen, um eine Frame-to-Frame-Posen-Nachverfolgung durchzuführen.
• In einem Zustandsschätzeinrichtungsblock 514 wird der initiale Keyframe von dem Initialisierungsblock 510 empfangen, wobei der IMU-Vorintegrationsterm, die 2D-Merkmalsspur und der Tiefenmesswert von dem Messwertverarbeitungsblock 502 empfangen werden. Die Optimierung des Faktorgraphen wird durchgeführt, wobei ein Gleitgraph basierend auf 2, am Zustandsschätzeinrichtungsblock 514, erzeugt wird. Sobald der Graph erzeugt worden ist, wird das System unter Verwendung eines nichtlinearen Solver optimiert.
• In einem Schleifenschlussblock 516 wird der Posengraph von der Zustandsschätzeinrichtung 514 in einem Versuch verwendet, Langzeitfehler vom Zustandsschätzeinrichtungsblock 514 zu minimieren.
• 6 ist ein Diagramm, das einen detaillierten Systemfluss der MSL-DVIO gemäß einer Ausführungsform der Offenbarung darstellt.
• Der Messwertverarbeitungsblock 502 beinhaltet einen IMU-Vorintegrationsblock 602, einen Merkmalserfassungs- und -nachverfolgungsblock 604 und einen Tiefenmesswertblock 606.
• Der Zustandsschätzeinrichtungsblock 514 beinhaltet einen Keyframe-Restwertblock 608, einen Gleitfenstergraphenblock 610 und einen Graphenoptimierungsblock 612. Der Keyframe-Restwertblock 608 beinhaltet einen IMU-Restwertblock 614, einen 2D-Merkmalsrestwertblock 616 und einen Tiefenrestwertblock 618. Der Gleitfenstergraphenblock 610 beinhaltet einen IMU-Parameterblock 620, einen Anker-Frame-Parameterblock 622, einen Nicht-Anker-Frame-Parameterblock 624 und einen Zeitversatz-Parameterblock 626.
• Der IMU-Restwert ist der Verlust zwischen dem geschätzten Wert der 6DOF-Pose für die Keyframes, die Geschwindigkeit und die Biases für die IMU, und dem IMU-Messwert (z. B. Beschleunigungssensor- und Gyroskopmesswert). Der Bildrestwert ist der Verlust zwischen dem geschätzten Wert der 6DOF-Pose für die Keyframes und die Position von einem Orientierungspunkt (1D oder 3D), und dem 2D-Messwert des Orientierungspunkts auf dem Keyframe. Der Tiefenrestwert ist der Verlust zwischen dem geschätzten Wert der 6DOF-Pose für die Keyframes und die Position von einem Orientierungspunkt (1D oder 3D) und dem Tiefenmesswert des Orientierungspunkts auf der Tiefenkarte.
• 7 ist ein Diagramm, das eine MSL-DVIO 1D-Merkmalsimplementierung gemäß einer Ausführungsform darstellt. In der MSL-DVIO ist die Implementierung der tiefenstützenden Veränderungen auf zwei Threads verteilt. Ein erster Thread ist ein Merkmalsnachverfolgungs-Thread 702, und ein zweiter Thread befindet sich in einem Zustandsschätzeinrichtungs-Thread 704.
• Der Merkmalsnachverfolgungs-Thread 702 beinhaltet eine Tiefen- und Bildmesswertverarbeitung. Die RGBD-Daten von der RGB-Kamera 506 und dem Tiefensensor 508 werden an einem Roboterbetriebssystem (ROS-) Synchronisatorblock 706 synchronisiert. Der ROS-Synchronisatorblock 706 liefert eine RGB-Bildnachricht- und Tiefenbildnachricht an den Merkmalserfassungs- und -nachverfolgungsblock 604. Der Merkmalserfassungs- und -nachverfolgungsblock 604 liefert einen Merkmalsmesswert und eine Tiefenbildnachricht an den Tiefenmesswertblock 606 bereit. Der Tiefenmesswertblock liefert den Merkmalsmesswert und den Tiefenmesswert an einen Geschwindigkeitsberechnungsblock 708 bereit, der eine Merkmalsgeschwindigkeit in den Bild- und Tiefenbereichen berechnet. Die Geschwindigkeits-, Merkmals- und Tiefenmesswerte werden dann auf der ROS-Plattform bei 710 veröffentlicht.
• Der Zustandsschätzeinrichtungs-Thread 704 handhabt das Gros der Arbeit für die Graphenoptimierung. Die IMU- und Merkmalsdaten werden bei 712 empfangen. Die IMU-Daten werden einem IMU-Vorintegrationsblock 602 bereitgestellt, um den IMU-Vorintegrationsterm für die Optimierung zu erzeugen. Die Merkmalsdaten werden einem Keyframe-Auswahlblock 714 zum Erzeugen von Keyframes bereitgestellt, die dem Gleitfenster hinzugefügt werden können. Ein Gleitfenster-Einrichtungsblock 716 empfängt den IMU-Vorintegrationsterm und die Keyframes. Bei Block 718 wird basierend auf den Gleitfensterdaten von dem Gleitfenster-Einrichtungsblock 716 bestimmt, ob die Initialisierung vollständig. Die Gleitfensterdaten werden einem Merkmal-Triangulieren-Block 720 bereitgestellt, wenn eine Initialisierung abgeschlossen ist. Wenn eine Initialisierung nicht abgeschlossen ist, werden die Gleitfensterdaten zunächst einem Initialstrukturblock 722 bereitgestellt. Die Gleitfensterdaten werden einem Posengraphen-Einrichtungsblock 724 bereitgestellt, der einen Posengraphen durch Initialisieren der Tiefe des Merkmals in dem Gleitfenster einrichtet.
• Unter Verwendung des Posengraphen wird der IMU-Restwert bei Block 726 bestimmt, wobei ein IMU-Faktor für eine Posengraphenoptimierung bei Block 728 bestimmt wird. Unter Verwendung des Posengraphen wird bestimmt, ob eine Zeitdeltaschätzung bei Block 730 bekannt ist. Wenn eine Zeitdeltaschätzung nicht bekannt ist, wird bestimmt, ob ein Frame der Anker-Frame bei Block 732 ist. Wenn es sich um den Anker-Frame handelt, wird die Anker-Frame-Tiefe bei Block 734 bestimmt, wobei ein visueller Anker-Frame-Faktor für eine Posengraphenoptimierung bei Block 736 bestimmt wird. Wenn es sich nicht um den Anker-Frame handelt, wird der Nicht-Anker-Frame-Bild- und Tiefenrestwert bei Block 738 bestimmt, wobei ein visueller Nicht-Anker-Frame-Faktor für eine Posengraphenoptimierung bei Block 740 bestimmt wird.
• Im Allgemeinen wird ein Visionsfaktor unter Verwendung von visuellen Sensoren erzeugt, die die 6DOF-Pose der Keyframes, die Position des Orientierungspunkts, den Tiefenrestwert und den Bildrestwert beinhalten. Ein IMU-Faktor wird unter Verwendung der IMU-Sensoren, die die 6DOF-Pose der Keyframes, die Geschwindigkeit und die Biases der IMU beinhalten, und des IMU-Restwerts erzeugt.
• Wenn die Zeitdeltaschätzung bekannt ist, wird bestimmt, ob ein Frame der Anker-Frame bei Block 742 ist. Wenn es sich um den Anker-Frame handelt, wird der Anker-Frame-Zeitdelta-Tiefenrestwert bei Block 744 bestimmt, wobei ein Anker-Frame-Zeitdelta-Visionsfaktor für eine Posengraphenoptimierung bei Block 746 bestimmt wird. Wenn es sich nicht um den Anker-Frame handelt, wird der Nicht-Anker-Frame-Bild- und Tiefenzeitdelta-Restwert bei Block 748 bestimmt, wobei ein Nicht-Anker-Frame-Zeitdelta-Visions faktor für eine Posengraphenoptimierung bei Block 750 bestimmt.
• Ein Posengraphenoptimierungsblock 752 bestimmt einen optimierten Graphen unter Verwendung des Posengraphen, des IMU-Faktors, des Anker-Frame-Visionsfaktors und des Nicht-Anker-Frame-Visionsfaktors. Eine Statusaktualisierung wird auf dem optimierten Posengraphen bei Block 754 durchgeführt, wobei eine Marginalisierung bei Block 756 durchgeführt wird, was in einem endgültigen Gleitfenster bei 758 resultiert.
• 8 ist ein Diagramm, das Einzelheiten über die Implementierung der MSL-DVIO für die 3D-Merkmalsparameterisierung gemäß einer Ausführungsform darstellt. Der Hauptunterschied zwischen dem MSL-DVIO-3D-Merkmal und dem MSL-DVIO 1D-Merkmal ist, dass das MS-DVIO-3D-Merkmal eine 2D-Merkmalsposition als Teil des Zustands aufweist, so dass die 2D-Position des Merkmals, bei Block 802, geschätzt wird, bevor der Posengraph erzeugt wird. Ein zweiter Unterschied ist, dass der Anker-Frame-Visionsfaktor sowohl einen Bild- als auch einen Tiefenrestwert aufweist. Insbesondere werden der Anker-Frame-Bild- und Tiefenrestwert bei Block 834 bestimmt, wobei der Anker-Frame-Bild- und Tiefenzeitdelta-Restwert bei Block 844 bestimmt wird.
• In dieser Implementierung des MSL-DVIO ist die Optimierung für den Visionsfaktor modifiziert, doch bleibt der IMU-Faktor für das Gesamtoptimierungsproblem unverändert. Wenn das VIO-System keiner aggressiven Bewegung ausgesetzt wird, hat dies keine Auswirkungen. In einigen Fällen, in denen die Bewegung relativ aggressiv ist, insbesondere während der Initialisierungsphase, kann jedoch eine Abweichung in der Trajektorie festgestellt werden.
• Die Zustände können beginnen, abzuweichen nachdem die erste Graphenoptimierung als Teil des Initialisierungsschritts durchgeführt worden ist. Somit kann eine Systementwurfsentscheidung vorgenommen werden, damit zwischen dem Initialisierungsschritt für die MSL-DVIO und das VINS-Mono Gleichwertigkeit herrscht. Zur Erlangung dieser Gleichwertigkeit kann das System modifiziert werden, wie nachstehend aufgeführt ist.
• Im MSL-DVIO-1D-Merkmal wird eine Berechnung des Tiefenrestwerts für den Anker-Frame unterbrochen, wobei der Tiefenrestwert in dem Nicht-Anker-Frame für den Initialisierungsschritt auf null gesetzt wird. Auch die Jacobische in dem Visionsfaktor, der für den Tiefenrestwert spezifisch ist, wird ebenfalls auf null gesetzt.
• Der Visionsrestwert ist nachstehend in den Gleichungen (90), (91) und (92) aufgeführt. $$r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)=\left[\begin{array}{c}{r}_{IMG}\left(x_i^R,x_j^R,{\lambda }_l\right)\\ r_{DP}\left(x_i^R,x_j^R,{\lambda }_l\right)\end{array}\right]$$

  

  $$r_{IMG}\left(x_i^R,x_j^R,{\lambda }_l\right)=g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{jl}}}{{\lambda }_l},\frac{\overline{v_{jl}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{\text{li}}^{IMG}$$

  

  $$r_{DP}\left(x_i^R,x_j^R,{\lambda }_l\right)=0$$

  
• Der veränderte Jacobische Teil ist nachstehend in Gleichung (93) aufgeführt. $$\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}=\left[\begin{array}{ccc}\frac{1}{z_l^{c_j}}& 0& \frac{x_l^{c_j}}{z_l^{c_j}{}^{{}^2}}\\ 0& \frac{1}{z_l^{c_j}}& \frac{y_l^{clj}}{z_l^{c_j}{}^{{}^2}}\\ 0& 0& 0\end{array}\right]$$

  
• Im MSL-DVIO-3D-Merkmal kann die Initialisierung des 2D-Merkmals im Zustand unter Verwendung des vorstehend beschriebenen Schätzalgorithmus unterbrochen werden. Der 2D-Merkmalszustand wird mit dem 2D-Merkmalsmesswert in dem Anker-Frame initialisiert. Die Berechnung des Tiefen- und -Bildrestwerts wird für den Anker-Frame unterbrochen, wobei der Tiefenrestwert für den Nicht-Anker-Frame auf null gesetzt wird. Die Jacobische in dem Visionsfaktor, der in Bezug auf ein 2D-Merkmal spezifisch ist, sowie der Tiefenrestwert werden auf null gesetzt.
• Der Visionsrestwert ist nachstehend in Gleichungen (94), (95) und (96) aufgeführt. $$r_{VIS}^{non}\left(x_i^R,x_j^R,{\lambda }_l\right)=\left[\begin{array}{c}{r}_{IMG}\left(x_i^R,x_j^R,{\lambda }_l\right)\\ r_{DP}\left(x_i^R,x_j^R,{\lambda }_l\right)\end{array}\right]$$

  

  $$r_{IMG}\left(x_i^R,x_j^R,{\lambda }_l\right)=g\left(T_{cb}{T}_{b_{j}w}{T}_{w{b}_i}{T}_{bc}{\left[\frac{\overline{u_{\mathrm{\iota }l}}}{{\lambda }_l},\frac{\overline{v_{\mathrm{\iota }l}}}{{\lambda }_l},\frac{1}{{\lambda }_l}\mathrm{,1}\right]}^T\right)-Z_{\text{lj}}^{IMG}$$

  

  $$r_{DP}\left(x_i^R,x_j^R,{\lambda }_l\right)=0$$

  
• Der veränderte Jacobische Teil ist nachstehend in Gleichung (97) aufgeführt. $$\frac{\partial ƒ\left(p_l^{c_j}\right)}{\partial p_l^{c_j}}=\left[\begin{array}{ccc}\frac{1}{z_l^{c_j}}& 0& \frac{x_l^{c_j}}{z_l^{c_j}{}^{{}^2}}\\ 0& \frac{1}{z_l^{c_j}}& \frac{y_l^{clj}}{z_l^{c_j}{}^{{}^2}}\\ 0& 0& 0\end{array}\right]$$

  
• Die gestapelte Jacobische für das Merkmalparameter ist nachstehend in Gleichung (98) aufgeführt. $$\frac{\partial r_{VIS}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial x^{ƒeat}}=\left[\begin{array}{ccc}0& 0& A_1\\ 0& 0& A_2\\ 0& 0& 0\end{array}\right],wennA={\left[A_1,A_2,A_3\right]}^T=\frac{\partial r_{VIS}\left(x_i^R,x_j^R,{\lambda }_l\right)}{\partial {\lambda }_l}$$

  
• In der vorstehend beschriebenen Tiefeninitialisierung wird die Tiefeninitialisierung unter Verwendung des Tiefenmesswerts unterbrochen.
• 9 ist ein Flussdiagramm, das ein Verfahren zum Durchführen einer VIO an einer UE gemäß einer Ausführungsform darstellt. Bei 902 werden Messwerte von einer IMU, einer Kamera und einem Tiefensensor der UE verarbeitet. Ein Datenstrom wird von der IMU empfangen und eine Vorintegration durchgeführt, um einen IMU-Vorintegrationsterm zu erzeugen. Ein Frame wird durch die Kamera aufgenommen, wobei eine Merkmalserfassung- und -nachverfolgung durchgeführt wird, um eine 2D-Merkmalsspur zu erzeugen. Ein Tiefenmesswert wird für ein erfasstes Merkmal des aufgenommenen Frames durch den Tiefensensor erzeugt.
• Die Merkmalserfassung- und -nachverfolgung für den aufgenommenen Frame kann mit dem Tiefenmesswert synchronisiert werden. Die Merkmalsgeschwindigkeit in den Bild- und Tiefenbereichen kann basierend auf der Merkmalserfassung- und -nachverfolgung und der Tiefenmessung bzw. dem Tiefenmesswert berechnet werden.
• Bei 904 wird die Keyframe-Initialisierung unter Verwendung des IMU-Vorintegrationsterms und der 2D-Merkmalsspur durchgeführt, um einen initialen Keyframe zu erzeugen.
• Bei 906 wird der Keyframe-Restwert basierend auf den verarbeiteten Messwerten bestimmt. Der Keyframe-Restwert kann einen IMU-Restwert, einen 2D-Merkmalsrestwert und einen Tiefenrestwert beinhalten. Der IMU-Tiefenrestwert wird unter Verwendung des IMU-Vorintegrationsterms bestimmt, der 2D-Merkmalrestwert wird von der 2D-Merkmalsspur bestimmt, und der Tiefenrestwert wird von dem Tiefenmesswert bestimmt. Insbesondere der IMU-Restwert ist der Verlust zwischen dem geschätzten Wert der 6DOF-Pose für die Keyframes, die Geschwindigkeit und die Biases für die IMU, und dem IMU-Messwert (z. B. Beschleunigungssensor- und Gyroskopmesswert). Der Bildrestwert ist der Verlust zwischen dem geschätzten Wert der 6DOF-Pose für die Keyframes und die Position von einem Orientierungspunkt (1D oder 3D), und dem 2D-Messwert des Orientierungspunkts auf dem Keyframe. Der Tiefenrestwert ist der Verlust zwischen dem geschätzten Wert der 6DOF-Pose für die Keyframes und die Position von einem Orientierungspunkt (1D oder 3D) und dem Tiefenmesswert des Orientierungspunkts auf der Tiefenkarte.
• Bei 908 wird ein Gleitfenstergraph basierend auf dem initialen Keyframe und von dem Keyframe-Restwert hergeleiteten Faktoren erzeugt und optimiert. Ein IMU-Faktor wird basierend auf dem IMU-Restwert bestimmt. Ein Anker-Frame-Visionsfaktor wird basierend auf dem Tiefenrestwert oder basierend auf dem 2D-Merkmalsrestwert und dem Tiefenrestwert bestimmt, wenn der aufgenommene Frame ein Anker-Frame ist. Ein Nicht-Anker-Frame-Visionsfaktor wird basierend auf dem 2D-Merkmalsrestwert und dem Tiefenrestwert bestimmt, wenn der aufgenommene Frame ein Nicht-Anker-Frame ist. Der Anker-Frame-Visionsfaktor und der Nicht-Anker-Frame-Visionsfaktor können auf einem Zeitversatz basieren, der Zeitstempel von der Kamera, dem Tiefensensors und der IMU ausgleicht.
• Im Allgemeinen wird ein Visionsfaktor unter Verwendung von visuellen Sensoren erzeugt, die die 6DOF-Pose der Keyframes, der Position des Orientierungspunkts, des Tiefenrestwerts und des Bildrestwerts beinhalten. Ein IMU-Faktor wird unter Verwendung der IMU-Sensoren, die die 6DOF-Pose der Keyframes, die Geschwindigkeit und die Biases der IMU beinhalten, sowie des IMU-Restwerts erzeugt.
• Der Gleitfenstergraph wird basierend auf dem IMU-Faktor und einem von dem Anker-Frame-Visionsfaktor und dem Nicht-Anker-Frame-Visionsfaktor optimiert. Die Optimierung kann die vorstehend beschriebene Zwei-Schritte-Marginalisierung beinhalten.
• Während der Optimierung werden die Zustandsparameter derart geschätzt, dass ein sehr geringer Restwert für den Graphen zu einem gegebenen Zeitpunkt vorliegt.
• Bei 910 wird eine Objektpose der UE basierend auf dem optimierten Gleitfenstergraphen geschätzt.
• Für einen Vergleich der Leistung des MSL-DVIO-Systems können Testdatensätze herangezogen werden, die, neben einer Ground-Truth-Trajektorie, aus Sequenzen bestehen, die eine IMU-Kamera und Tiefendaten aufweisen. Es sind zwei Arten von Sequenzen vorgesehen.
• Simulationssequenzen erzeugen RGBD- und IMU-Daten für die Trajektorie.
• Durch das VINS-RGBD-System, das drei (3) handgehaltene Sequenzen des Abschreitens eines großen Raums beinhaltet, werden Real-World-Sequenzen erzeugt. Diese Sequenzen können unter Verwendung eines Realsense-D435-Sensors erzeugt werden, wobei die Ground-Truth unter Verwendung des MoCap-Systems erzeugt werden kann.
• Unter Verwendung eines Okzipitalstrukturkernsensors können Depth-uncertaintyincorporated- (DUI-) VIO-Sequenzen (bzw. VIO-Sequenzen mit Tiefenunschärfe) erzeugt werden, wobei die Ground-Truth unter Verwendung des MoCap-Systems erzeugt werden kann.
• Die Leistung des MSL-DVIO-Systems wird mit dem VINS-Mono und VINS-RGBD verglichen.
• Die für den Vergleich verwendete Testmetrik ist der Fehler des quadratischen Mittelwerts (RMSE) eines absoluten Übersetzungsfehlers, der nachstehend durch die Gleichung (99) gegeben ist. $$RMSE={\left(\frac{1}{n}{\displaystyle \sum _i^n{\Vert trans\left(Q_i^{-1}S{P}_i\right)\Vert }^2}\right)}^{1/2}$$

  

  wobei Q die Ground-Truth-Trajektorie ist, P die geschätzte Trajektorie ist und S die Festkörpertransformation zwischen der Ground-Truth und der geschätzten Trajektorie ist.
• Die Vergleichsleistung der drei Systeme für die Sequenzen ist in Tabelle 7 unten bereitgestellt. Tabelle 7 - ATE-Leistungsergebnis

  Sequenz-Name	MSL-DVIO 1D-Merkmal	MSL-DVIO 3D-Merkmal	VINS-Mono	VINS-RGBD
  u3d spiral	0,032	0,046	0,145	X
  u3d sine	0,036	0,037	0,353	0,084
  u3d simple	0,241	0,213	0,575	0,369
  u3d normal	0,122	0,189	0,795	0,453
  u3d rotation	0,836	0,496	0,924	0,682
  rgbd simple	0,184	0,205	0,228	0,183
  rgbd normal	0,181	0,204	0,229	0,059
  rgbd rotation	0,195	0,210	0,196	0,126
  corridor1	X	X	4,39	5,13
  corridor2	1,41	1,14	1,61	1,81
  corridor3	2,35	1,99	3,97	6,81
  corridor4	1,51	0,915	4,33	1,95
  halll	0,694	2,36	4,8	2,34
  hal12	2,176	1,019	12,71	7,98
  hall3	X	6,61	X	X
  easy1	0,075	0,079	0,142	0,137
  easy2	0,067	0,073	0,175	0,431
  easy3	0,079	0,084	0,236	0,116
  dynamic 1	0,157	0,159	0,287	0,216
  dynamic2	0,206	0,303	0,55	0,324
  dynamic3	0,294	0,293	0,653	0,52
  dynamic4	0,820	1,186	1,7	2,5
  dynamic5	0,060	0,118	0,469	0,177
  light1	0,871	0,361	X	X
  light2	0,37	0,3752	0,4	0,48
  light3	0,139	0,156	0,384	X
  light4	0,175	0,181	0,29	3,11
  light5	1,211	0,574	X	1,12
  light6	2,2	1,98	1,06	X
  motion1	0,292	0,371	0,526	0,581
  motion2	0,333	0,475	0,699	0,66
  motion3	0,158	0,161	0,452	0,361
  motion4	0,334	0,345	0,54	0,488
  motion5	0,250	0,226	0,373	0,378
  motion6	0,246	0,225	0,707	0,725

• Wie in Tabelle 7 gezeigt, kennzeichnen die fett gedruckten Zahlen die beste Leistung für die Sequenz, kennzeichnet ein „X“, dass die Initialisierung des Systems gescheitert ist oder dass das System in einer großen ATE (>50 Meter) resultierte.
• Die Timing-Leistung der unterschiedlichen Systeme kann auch zu dem Zweck verglichen werden, die Echtzeitleistung des Algorithmus sowie die Auswirkungen der Algorithmusveränderungen zu bestimmen. Für einen solchen Vergleich werden Timing-Zahlen der MSL-DVIO-1D- und der MSL-DVIO-3D-Systeme mit und ohne schnelle Marginalisierungsänderungen mit dem VINS-Mono- und dem VINS-RGBD-System geschätzt. Die Ergebnisse sind nachstehend in Tabelle 8 gezeigt. Tabelle 8 - Verarbeitungszeitschätzung von MSL-DVIO

  	Gesamtzeit (Millisek.)	Ceres Optimierungszeit (Millisek. )	Marginalisierungszeit (Millisek.)	Triangulationszeit (Millisek.)
  MSL-DVIO 1D (mit schneller Marginalisierung)	54,0985	0,0558	45,5234	4,9316
  MSL-DVIO 3D (mit schneller Marginalisierung)	83,527	1,0526	54,2526	25,4595
  MSL-DVIO 1D (ohne schnelle Marginalisierung)	69,6035	0,0462	37,3896	27,5371
  MSL-DVIO 1D (ohne schnelle Marginalisierung)	449,92	0,718	39,0122	407,3284
  VINS-Mono	61,4034	0,0505	30,7614	26,7074
  VINS-RGBD	61,4034	0,0505	30,7614	26,7074

• Wie in Tabelle 8 gezeigt ist, wird bei schnellen Marginalisierungsveränderungen die Marginalisierungsverarbeitungszeit für sowohl MSL-DVIO 1D (~ 80 % Verbesserung) als auch MSL-DVIO 3D (~ 94 % Verbesserung) verbessert. Dementsprechend wird eine Echtzeitleistung von ~ 18 Hz für MSL-DVIO 1D und ~ 11 Hz für MSL-DVIO 3D erreicht. Beide Zahlen übersteigen eine 10 Hz-Anforderung für den nichtlinearen Optimierungs-Thread, so dass eine Echtzeitleistung des vollständigen VIO-Systems ermöglicht wird.
• 10 ist ein Blockdiagramm einer elektronischen Vorrichtung in einer Netzwerkumgebung gemäß einer Ausführungsform. Bezugnehmend auf 10 kann eine elektronische Vorrichtung 1001 in einer Netzwerkumgebung 1000 mit einer elektronischen Vorrichtung 1002 über ein erstes Netzwerk 1098 (z. B. ein drahtloses Kommunikationsnetzwerk mit kurzer Reichweite) kommunizieren, oder eine elektronische Vorrichtung 1004 oder ein Server 1008 über ein zweites Netzwerk 1099 (z. B. ein drahtloses Langstrecken-Kommunikationsnetzwerk). Die elektronische Vorrichtung 1001 kann mit der elektronischen Vorrichtung 1004 über den Server 1008 kommunizieren. Die elektronische Vorrichtung 1001 kann einen Prozessor 1020, einen Speicher 1030, eine Eingabevorrichtung 1050, eine Tonausgabevorrichtung 1055, eine Anzeigevorrichtung 1060, ein Audiomodul 1070, ein Sensormodul 1076, eine Schnittstelle 1077, ein Haptikmodul 1079, ein Kameramodul 1080, ein Leistungsverwaltungsmodul 1088, eine Batterie 1089, ein Kommunikationsmodul 1090, ein Teilnehmeridentitätsmodul (SIM) 1096 oder ein Antennenmodul 1097 beinhalten. In einer Ausführungsform kann auf zumindest eine der Komponenten (z. B. die Anzeigevorrichtung 1060 oder das Kameramodul 1080) in der elektronischen Vorrichtung 1001 verzichtet werden, oder es können der elektronischen Vorrichtung 1001 eine oder mehrere Komponenten hinzugefügt werden. Einige der Komponenten können als eine einzelne integrierte Schaltung (IC) implementiert sein. Zum Beispiel kann das Sensormodul 1076 (z. B. ein Fingerabdrucksensor, ein Irissensor oder ein Helligkeitssensor) in die Anzeigevorrichtung 1060 (z. B. eine Anzeige) eingebettet sein.
• Der Prozessor 1020 kann z. B. eine Software (z. B. ein Programm 1040) ausführen, um zumindest eine andere Komponente (z. B. eine Hardware- oder Software-Komponente) der elektronischen Vorrichtung 1001, die mit dem Prozessor 1020 gekoppelt ist, zu steuern und kann verschiedene Datenverarbeitungen und -berechnungen durchführen. Als zumindest ein Teil der Datenverarbeitungen und -berechnungen kann der Prozessor 1020 einen Befehl oder Daten laden, die von einer anderen Komponente (z. B. dem Sensormodul 1076 oder dem Kommunikationsmodul 1090) in einem flüchtigen Speicher 1032 empfangen werden, den Befehl oder die Daten, die in dem flüchtigen Speicher 1032 gespeichert sind, verarbeiten und resultierende Daten in dem nicht-flüchtigen Speicher 1034 speichern. Der Prozessor 1020 kann einen Hauptprozessor 1021 (z. B. eine zentrale Verarbeitungseinheit (CPU) oder einen Anwendungsprozessor (AP)) und einen Hilfsprozessor 1023 (z. B. eine Graphikverarbeitungseinheit (GPU), einen Bildsignalprozessor (ISP), einen Sensorhubprozessor oder einen Kommunikationsprozessor (CP)) beinhalten, der unabhängig von oder zusammen mit dem Hauptprozessor 1021 betreibbar ist. Zusätzlich oder alternativ kann der Hilfsprozessor 1023 so angepasst sein, dass er weniger Leistung als der Hauptprozessor 1021 verbraucht oder eine spezielle Funktion ausführt. Der Hilfsprozessor 1023 kann separat oder als ein Teil von dem Hauptprozessor implementiert sein.
• Der Hilfsprozessor 1023 kann, im Gegensatz zum Hauptprozessor 1021, zumindest einige der Funktionen oder Zustände, welche auf zumindest eine Komponente (z. B. die Anzeigevorrichtung 1060, das Sensormodul 1076 oder das Kommunikationsmodul 1090) unter den Komponenten der elektronischen Vorrichtung 1001 bezogen sind, steuern, während sich der Hauptprozessor 1021 in einem inaktiven (z. B. Sleep-) Zustand befindet, oder er kann dies zusammen mit dem Hauptprozessor 1021 tun, während der Hauptprozessor 1021 sich in einem aktiven Zustand befindet (z. B. eine Anwendung ausführt). Der Hilfsprozessor 1023 (z. B. ein Bildsignalprozessor oder ein Kommunikationsprozessor) kann als ein Teil einer anderen Komponente (z. B. des Kameramoduls 1080 oder des Kommunikationsmoduls 1090) implementiert sein, welche funktionell auf den Hilfsprozessor 1023 bezogen ist.
• Der Speicher 1030 kann verschiedene Daten speichern, welche durch zumindest eine Komponente (z. B. den Prozessor 1020 oder das Sensormodul 1076) der elektronischen Vorrichtung 1001 verwendet werden. Die verschiedenen Daten können z. B. eine Software (z. B. das Programm 1040) und Eingabedaten oder Ausgabedaten für einen darauf bezogenen Befehl beinhalten. Der Speicher 1030 kann den flüchtigen Speicher 1032 oder den nichtflüchtigen Speicher 1034 beinhalten.
• Das Programm 1040 kann in dem Speicher 1030 als eine Software gespeichert sein und kann z. B. ein Betriebssystem (OS) 1042, eine Middleware 1044 oder eine Anwendung 1046 beinhalten.
• Die Eingabevorrichtung 1050 kann einen Befehl oder Daten, die durch eine andere Komponente (z. B. den Prozessor 1020) der elektronischen Vorrichtung 1001 verwendet werden sollen, von außerhalb (z. B. einem Benutzer) der elektronischen Vorrichtung 1001 empfangen. Die Eingabevorrichtung 1050 kann z. B. ein Mikrophon, eine Maus oder eine Tastatur beinhalten.
• Die Tonausgabevorrichtung 1055 kann Tonsignale nach außerhalb der elektronischen Vorrichtung 1001 ausgeben. Die Tonausgabevorrichtung 1055 kann z. B. einen Lautsprecher oder einen Empfänger beinhalten. Der Lautsprecher kann für allgemeine Zwecke verwendet werden, wie z. B. ein Wiedergeben von Multimedia oder einer Aufzeichnung, wobei der Empfänger zum Empfangen eines eingehenden Anrufs verwendet werden kann. Der Empfänger kann separat von oder als ein Teil von dem Lautsprecher implementiert sein.
• Die Anzeigevorrichtung 1060 kann Informationen nach außerhalb (z. B. einem Benutzer) der elektronischen Vorrichtung 1001 visuell bereitstellen. Die Anzeigevorrichtung 1060 kann z. B. eine Anzeige, eine Hologrammvorrichtung oder einen Projektor- und Steuerungsschaltungsaufbau zum Steuern einer entsprechenden Anzeige, Hologrammvorrichtung und eines entsprechenden Projektors beinhalten. Die Anzeigevorrichtung 1060 kann einen Tastschaltungsaufbau, der dafür eingerichtet ist, eine Berührung zu erfassen, oder einen Sensorschaltungsaufbau (z. B. einen Drucksensor), der dafür angepasst ist, die Intensität der durch die Berührung erfahrenen Kraft zu messen, beinhalten.
• Das Audiomodul 1070 kann einen Ton in ein elektrisches Signal umwandeln und umgekehrt. Das Audiomodul 1070 kann den Ton über die Eingabevorrichtung 1050 erhalten oder den Ton über die Tonausgabevorrichtung 1055 oder einen Kopfhörer von einer externen elektronischen Vorrichtung 1002 direkt (z. B. verdrahtet) oder drahtlos gekoppelt mit der elektronischen Vorrichtung 1001 ausgeben.
• Das Sensormodul 1076 kann einen Betriebszustand (z. B. eine Leistung oder Temperatur) der elektronischen Vorrichtung 1001 oder einen Umgebungszustand (z. B. einen Zustand eines Benutzers) außerhalb der elektronischen Vorrichtung 1001 erfassen und dann ein elektrisches Signal oder einen Datenwert erzeugen, der dem erfassten Zustand entspricht. Das Sensormodul 1076 kann z. B. einen Gestensensor, einen Gyrosensor, einen Atmosphärendrucksensor, einen Magnetsensor, einen Beschleunigungssensor, einen Griffsensor, einen Näherungssensor, einen Farbsensor, einen Infrarot- (IR-) Sensor, einen biometrischen Sensor, einen Temperatursensor, einen Feuchtigkeitssensor oder einen Beleuchtungsstärkensensor beinhalten.
• Die Schnittstelle 1077 kann ein oder mehrere spezifizierte Protokolle unterstützen, die für die elektronische Vorrichtung 1001, die mit der externen elektronischen Vorrichtung 1002 direkt (z. B. drahtgebunden) oder drahtlos gekoppelt werden soll, verwendet werden sollen. Die Schnittstelle 1077 kann z. B. eine High-Definition-Multimedia-Interface (HDMI), eine Universal-Serial-Bus- (USB-) Schnittstelle, eine sichere digitale (SD-) Kartenschnittstelle oder eine Audioschnittstelle beinhalten.
• Ein Verbindungsanschluss 1078 kann einen Verbinder beinhalten, über den die elektronische Vorrichtung 1001 mit der externen elektronischen Vorrichtung 1002 physikalisch verbunden werden kann. Der Verbindungsanschluss 1078 kann z. B. einen HDMI-Verbinder, einen USB-Verbinder, einen SD-Kartenverbinder oder einen Audioverbinder (z. B. einen Kopfhörerverbinder) beinhalten.
• Das haptische Modul 1079 kann ein elektrisches Signal in einen mechanischen Reiz (z. B. eine Vibration oder eine Bewegung) oder einen elektrischen Reiz umwandeln, der durch einen Benutzer über eine Tastempfindung oder kinästhetische Empfindung erkannt werden kann. Das haptische Modul 1079 kann z. B. einen Elektromotor, ein piezoelektrisches Element oder einen elektrischen Stimulator beinhalten.
• Das Kameramodul 1080 kann ein Standbild oder Bewegtbilder erfassen. Das Kameramodul 1080 kann eine oder mehrere Linsen, Bildsensoren, Bildsignalprozessoren oder Blitzlichter beinhalten.
• Das Leistungsverwaltungsmodul 1088 kann eine der elektronischen Vorrichtung 1001 zugeführte Leistung verwalten. Das Leistungsverwaltungsmodul 1088 kann als zumindest ein Teil von z. B. einer integrierten Leistungsverwaltungsschaltung (PMIC) implementiert sein.
• Die Batterie 1089 kann zumindest einer Komponente der elektronischen Vorrichtung 1001 Leistung zuführen. Die Batterie 1089 kann z. B. eine Primärzelle, die nicht wiederaufladbar ist, oder eine Sekundärzelle, die wiederaufladbar ist, oder eine Brennstoffzelle beinhalten.
• Das Kommunikationsmodul 1090 kann ein Einrichten eines direkten (z. B. drahtgebundenen) Kommunikationskanals oder eines drahtlosen Kommunikationskanals zwischen der elektronischen Vorrichtung 1001 und der externen elektronischen Vorrichtung (z. B. der elektronischen Vorrichtung 1002, der elektronischen Vorrichtung 1004 oder dem Server 1008) und ein Durchführen einer Kommunikation über den eingerichteten Kommunikationskanal unterstützen. Das Kommunikationsmodul 1090 kann einen oder mehrere Kommunikationsprozessoren, die unabhängig von dem Prozessor 1020 (z. B. dem AP) betreibbar sind, beinhalten und unterstützt eine direkte (z. B. drahtgebundene) Kommunikation oder eine drahtlose Kommunikation. Das Kommunikationsmodul 1090 kann ein drahtloses Kommunikationsmodul 1092 (z. B. ein zellulares Kommunikationsmodul, ein drahtloses Kommunikationsmodul mit kurzer Reichweite oder ein globales Satellitennavigations- (GNSS-) Kommunikationsmodul) oder ein drahtgebundenes Kommunikationsmodul 1094 (z. B. ein lokales Netzwerk- (LAN-) Kommunikationsmodul oder ein Powerline-Kommunikations- (PLC-) Modul) beinhalten. Ein entsprechendes von diesen Kommunikationsmodulen kann mit der externen elektronischen Vorrichtung über das erste Netzwerk 1098 (z. B. ein Kommunikationsnetzwerk mit kurzer Reichweite, wie z. B. Bluetooth®, Wireless-Fidelity- (Wi-Fi-) Direct oder einen Standard der Infrared-Data-Association (IrDA)) oder das zweite Netzwerk 1099 (z. B. ein Kommunikationsnetzwerk mit großer Reichweite, wie z. B. ein zelluläres Netzwerk, das Internet oder ein Computernetzwerk (z. B. LAN oder ein Weitverkehrsnetz (WAN)) kommunizieren. Diese verschiedenen Arten von Kommunikationsmodulen können als eine einzelne Komponente (z. B. eine einzelne integrierte Schaltung) implementiert sein, oder sie können als mehrere Komponenten (z. B. mehrere integrierte Schaltungen), die voneinander getrennt sind, implementiert sein. Das drahtlose Kommunikationsmodul 1092 kann die elektronische Vorrichtung 1001 in einem Kommunikationsnetzwerk, wie z. B. dem ersten Netzwerk 1098 oder dem zweiten Netzwerk 1099, unter Verwendung von Teilnehmerinformationen (z. B. einer internationalen mobilen Teilnehmeridentität (IMSI)), die in dem Teilnehmeridentifikationsmodul 1096 gespeichert sind, identifizieren und authentifizieren.
• Das Antennenmodul 1097 kann ein Signal oder eine Leistung nach oder von außerhalb (z. B. der externen elektrischen Vorrichtung) der elektronischen Vorrichtung 1001 übertragen oder empfangen. Das Antennenmodul 1097 kann eine oder mehrere Antennen beinhalten, und von diesen kann zumindest eine Antenne, die für ein Kommunikationsschema geeignet ist, das in dem Kommunikationsnetzwerk, wie z. B. dem ersten Netzwerk 1098 oder dem zweiten Netzwerk 1099, verwendet wird, durch z. B. das Kommunikationsmodul 1090 (z. B. das drahtlose Kommunikationsmodul 1092) ausgewählt werden. Das Signal oder die Leistung kann dann zwischen dem Kommunikationsmodul 1090 und der externen elektrischen Vorrichtung über die ausgewählte zumindest eine Antenne übertragen oder empfangen werden.
• Zumindest einige der vorstehend beschriebenen Komponenten können miteinander gekoppelt sein und untereinander über ein interperiphäres Kommunikationsschema (z. B. einen Bus, eine Allzweckeingabe-/ausgabe (GPIO), eine serielle periphere Schnittstelle (SPI) oder eine mobile Industrieprozessorschnittstelle (MIPI)) Signale kommunizieren.
• Befehle oder Daten können zwischen der elektronischen Vorrichtung 1001 und der externen elektronischen Vorrichtung 1004 über den Sensor 1008, der mit dem zweiten Netzwerk 1099 gekoppelt ist, übertragen und empfangen werden. Die elektronischen Vorrichtungen 1002 und 1004 können jeweils eine Vorrichtung eines identischen Typs wie die oder eines unterschiedlichen Typs zur der elektronischen Vorrichtung 1001 sein. Die Gesamtheit der an der elektronischen Vorrichtung 1001 auszuführenden Vorgänge oder ein Teil davon kann an einer oder mehreren der externen elektronischen Vorrichtungen 1002, 1004 oder 1008 ausgeführt werden. Wenn die elektronische Vorrichtung 1001 z. B. eine Funktion oder einen Dienst automatisch oder in Reaktion auf eine Anforderung von einem Benutzer oder einer anderen Vorrichtung ausführen soll, kann die elektronische Vorrichtung 1001, anstelle des Ausführens der Funktion oder des Dienstes, oder zusätzlich dazu, die eine oder die mehreren elektronischen Vorrichtungen auffordern, zumindest einen Teil der Funktion oder des Dienstes auszuführen. Die eine oder die mehreren elektronischen Vorrichtungen, die die Anforderung empfangen, kann/können den zumindest einen Teil der angeforderten Funktion oder des angeforderten Dienstes oder eine zusätzliche Funktion oder einen zusätzlichen Dienst, die auf die Anforderung bezogen sind, durchführen und ein Ergebnis der Durchführung an die elektronische Vorrichtung 1001 übertragen. Die elektronische Vorrichtung 1001 kann das Ergebnis, mit oder ohne eine weitere Verarbeitung des Ergebnisses, als zumindest einen Teil einer Antwort auf die Anforderung bereitstellen. Zu diesem Zweck kann eine Cloud-Computing-, eine verteilte Rechen- oder eine Client-Server-Rechentechnologie verwendet werden.
• Eine Ausführungsform kann als eine Software (z. B. das Programm 1040) implementiert sein, die eine oder mehrere Anweisungen beinhaltet, die in einem Speichermedium (z. B. einem internen Speicher 1036 oder einem externen Speicher 1038) gespeichert sind, das durch eine Maschine (z. B. die elektronische Vorrichtung 1001) lesbar ist. Ein Prozessor der elektronischen Vorrichtung 1001 kann z. B. zumindest eine von der einen oder den mehreren in dem Speichermedium gespeicherten Anweisungen unter oder ohne Verwendung von einer oder mehreren anderen Komponenten unter der Steuerung des Prozessors aufrufen und diese ausführen. Somit kann eine Maschine betrieben werden, um zumindest eine Funktion gemäß der zumindest einen aufgerufenen Anweisung durchzuführen. Die eine oder die mehreren Anweisungen können einen Code, der durch einen Compiler erzeugt wird, oder einen durch einen Interpreter ausführbaren Code beinhalten. Ein maschinenlesbares Speichermedium kann in der Form eines nichttransitorischen Speichermediums bereitgestellt sein. Der Begriff „nichttransitorisch“ weist darauf hin, dass das Speichermedium eine dinghafte Vorrichtung ist und kein Signal beinhaltet (z. B. eine elektromagnetische Welle), j edoch unterscheidet dieser Begriff nicht zwischen dem Ort, wo Daten in dem Speichermedium semipermanent gespeichert sind, und dem Ort, wo die Daten in dem Speichermedium vorübergehend gespeichert sind.
• Gemäß einer Ausführungsform kann ein Verfahren der Offenbarung in einem Computerprogrammprodukt enthalten und bereitgestellt sein. Das Computerprogrammprodukt kann als ein Produkt zwischen einem Verkäufer und einem Käufer gehandelt werden. Das Computerprogrammprodukt kann in der Form eines maschinenlesbaren Speichermediums (z. B. einem Compact-Disc-Read-Only-Memory (CD-ROM)) oder online über ein Application-Store (z. B. Play Store®) oder direkt zwischen zwei Benutzervorrichtungen (z. B. Smartphones) vertrieben werden. Beim Onlinevertrieb kann zumindest ein Teil des Computerprogrammprodukts vorübergehend erzeugt werden oder zumindest vorübergehend in dem maschinenlesbaren Speichermedium, wie z. B. einem Speicher des Servers des Herstellers, einem Server des Application-Store oder einem Relay-Server, gespeichert sein.
• Gemäß einer Ausführungsform kann jede Komponente (z. B. ein Modul oder ein Programm) der vorstehend beschriebenen Komponenten eine einzelne Entität oder mehrere Entitäten beinhalten. Es kann auf eine oder mehrere der vorstehend beschriebenen Komponenten verzichtet werden, oder es können eine oder mehrere weitere Komponenten hinzugefügt werden. Alternativ oder zusätzlich kann eine Mehrzahl von Komponenten (z. B. Module oder Programme) zu einer einzelnen Komponente integriert sein. In diesem Fall kann die integrierte Komponente immer noch eine oder mehrere Funktionen von einer jeweiligen von der Mehrzahl von Komponenten auf die gleiche oder eine ähnliche Weise durchgeführt werden wie sie durch eine entsprechende von der Mehrzahl von Komponenten vor der Integration durchgeführt werden. Durch das Modul, das Programm oder eine andere Komponente durchgeführte Operationen können sequentiell, parallel, wiederholt oder heuristisch ausgeführt werden, oder es können eine oder mehrere Operationen in einer unterschiedlichen Reihenfolge ausgeführt oder ausgelassen werden, oder es können eine oder mehrere Operationen hinzugefügt werden.
• Selbst wenn bestimmte Ausführungsformen der vorliegenden Offenbarung in der ausführlichen Beschreibung der vorliegenden Offenbarung erläutert worden sind, kann die vorliegende Offenbarung verschiedenartig modifiziert werden, ohne vom Schutzbereich der vorliegenden Offenbarung abweichen. Der Schutzbereich der vorliegenden Offenbarung ist somit nicht nur im Zusammenhang mit den beschriebenen Ausführungsformen, sondern vielmehr auf Basis der beigefügten Ansprüche und deren Entsprechungen zu bestimmen.
• ZITATE ENTHALTEN IN DER BESCHREIBUNG
• Diese Liste der vom Anmelder aufgeführten Dokumente wurde automatisiert erzeugt und ist ausschließlich zur besseren Information des Lesers aufgenommen. Die Liste ist nicht Bestandteil der deutschen Patent- bzw. Gebrauchsmusteranmeldung. Das DPMA übernimmt keinerlei Haftung für etwaige Fehler oder Auslassungen.
• Zitierte Patentliteratur
• US 63/192488 [0001]

Claims (20)

1. Verfahren zum Durchführen einer visuellen Trägheitsodometrie (VIO) an einer Benutzervorrichtung (UE), wobei das Verfahren folgende Schritte aufweist: Verarbeiten von Messungen von einer Trägheitsmessungseinheit (IMU), einer Kamera und einem Tiefensensor der UE; Bestimmen eines Keyframe-Restwertes, der zumindest einen Tiefenrestwert aufweist, basierend auf den verarbeiteten Messungen; Erzeugen und Optimieren eines Gleitfenstergraphen basierend auf Faktoren, die von dem Keyframe-Restwert abgeleitet werden; und Schätzen von einer Objektpose der UE basierend auf dem optimierten Gleitfenstergraphen.
2. Verfahren nach Anspruch 1, wobei das Verarbeiten der Messungen aufweist: Empfangen eines Datenstroms von der IMU und Durchführen einer Vorintegration für den Datenstrom, um einen IMU-Vorintegrationsterm zu erzeugen; Aufnehmen eines Frames durch die Kamera, und Durchführen einer Merkmalserfassung und -nachverfolgung des aufgenommenen Frames, um eine zweidimensionale (2D-) Merkmalsspur zu erzeugen; und Erzeugen einer Tiefenmessung, für ein erfasstes Merkmal des aufgenommenen Frames, durch den Tiefensensor.
3. Verfahren nach Anspruch 2, wobei das Bestimmen des Keyframe-Restwerts aufweist: Bestimmen eines IMU-Restwerts unter Verwendung des IMU-Vorintegrationsterms; Bestimmen eines 2D-Merkmalsrestwerts von der 2D-Merkmalsspur; und Bestimmen des Tiefenrestwerts von der Tiefenmessung.
4. Verfahren nach Anspruch 3, wobei das Erzeugen und Optimieren des Gleitfenstergraphen aufweist: Bestimmen eines IMU-Faktors basierend auf dem IMU-Restwert; Bestimmen eines Ankerframe-Visionsfaktors basierend auf zumindest dem Tiefenrestwert, falls der aufgenommene Frame ein Ankerframe ist; Bestimmen eines Nicht-Ankerframe-Visionsfaktors basierend auf dem 2D-Merkmalsrestwert und dem Tiefenrestwert, in dem Fall, dass der aufgenommene Rahmen ein Nicht-Ankerframe ist; und Optimieren des Gleitfenstergraphen basierend auf dem IMU-Faktor und einem von dem Ankerframe-Visionsfaktor und dem Nicht-Ankerframe-Visionsfaktor.
5. Verfahren nach Anspruch 4, wobei der Ankerframe-Visionsfaktor basierend auf dem 2D-Merkmalsrestwert und dem Tiefenrestwert bestimmt wird.
6. Verfahren nach Anspruch 4, wobei der Ankerframe-Visionsfaktor und der Nicht-Ankerframe-Visionsfaktor auf einem Zeitversatz basieren, der Zeitstempeln von der Kamera, dem Tiefensensor und der IMU entspricht.
7. Verfahren nach Anspruch 2, ferner aufweisend ein Durchführen einer Keyframe-Initialisierung unter Verwendung des IMU-Vorintegrationsterms und der 2D-Merkmalsspur, um einen initialen Keyframe zu erzeugen, wobei der Gleitfenstergraph basierend auf dem initialen Keyframe erzeugt wird.
8. Verfahren nach Anspruch 2, ferner aufweisend ein Synchronisieren einer Merkmalserfassung und -nachverfolgung des aufgenommenen Frames mit einer Tiefenmessung des erfassten Merkmals.
9. Verfahren nach Anspruch 2, ferner aufweisend ein Berechnen einer Merkmalsgeschwindigkeit in Bild- und Tiefenbereichen basierend auf der Merkmalserfassung und -nachverfolgung und Tiefenmessung.
10. Verfahren nach Anspruch 4, wobei ein Optimieren des Gleitfenstergraphen aufweist: Durchführen einer Marginalisierung von Orientierungspunkten in dem aufgenommenen Frame; und Durchführen einer Marginalisierung von einer Keyframe-Pose, Geschwindigkeit und Bias-Termen.
11. Benutzervorrichtung (UE), aufweisend: eine Trägheitsmessungseinheit (IMU); eine Kamera; einen Tiefensensor; einen Prozessor; und ein nichttransitorisches computerlesbares Speichermedium, das Anweisungen speichert, die, wenn sie ausgeführt werden, bewirken, dass der Prozessor: Messungen von der IMU, der Kamera und dem Tiefensensor verarbeitet; einen Keyframe-Restwert, der zumindest einen Tiefenrestwert aufweist, basierend auf den verarbeiteten Messungen bestimmt; einen Gleitfenstergraphen basierend auf Faktoren erzeugt und optimiert, die von dem Keyframe-Restwert abgeleitet werden; und eine Objektpose der UE basierend auf dem optimierten Gleitfenstergraphen schätzt.
12. UE nach Anspruch 11, wobei beim Verarbeiten der Messungen die Anweisungen ferner bewirken, dass der Prozessor: einen Datenstrom von der IMU empfängt und eine Vorintegration für den Datenstrom durchgeführt wird, um einen IMU-Vorintegrationsterm zu erzeugen; einen Frame durch die Kamera aufnimmt, und eine Merkmalserfassung und -nachverfolgung des aufgenommenen Frames durchgeführt wird, um eine zweidimensionale (2D-) Merkmalsspur zu erzeugen; und eine Tiefenmessung für ein erfasstes Merkmal des aufgenommenen Frames durch den Tiefensensor erzeugt.
13. UE nach Anspruch 12, wobei, beim Bestimmen des Keyframe-Restwerts, die Anweisungen ferner bewirken, dass der Prozessor: einen IMU-Restwert unter Verwendung des IMU-Vorintegrationsterms bestimmt; einen 2D-Merkmals-Restwert von der 2D-Merkmalsspur bestimmt; und den Tiefenrestwert von der Tiefenmessung bestimmt.
14. UE nach Anspruch 13, wobei, beim Erzeugen und Optimieren des Gleitfenstergraphen, die Anweisungen ferner bewirken, dass der Prozessor: einen IMU-Faktor basierend auf dem IMU-Restwert bestimmt; einen Ankerframe-Visionsfaktor basierend auf zumindest dem Tiefenrestwert bestimmt, in dem Fall, dass der aufgenommene Frame ein Ankerframe ist; einen Nicht-Ankerframe-Visionsfaktor basierend auf dem 2D-Merkmalsrestwert und dem Tiefenrestwert bestimmt, in dem Fall, dass der aufgenommene Frame ein Nicht-Ankerframe ist; und den Gleitfenstergraphen basierend auf dem IMU-Faktor und einem von dem Ankerframe-Visionsfaktor und dem Nicht-Ankerframe-Visionsfaktor optimiert.
15. UE nach Anspruch 14, wobei der Ankerframe-Visionsfaktor basierend auf dem 2D-Merkmalsrestwert und dem Tiefenrestwert bestimmt wird.
16. UE nach Anspruch 14, wobei der Ankerframe-Visionsfaktor und der Nicht-Ankerframe-Visionsfaktor auf einem Zeitversatz basieren, der Zeitstempeln von der Kamera, dem Tiefensensorsund der IMU entspricht.
17. UE nach Anspruch 12, wobei die Anweisungen ferner bewirken, dass der Prozessor eine Keyframe-Initialisierung unter Verwendung des IMU-Vorintegrationsterms und der 2D-Merkmalsspur durchführt, um einen initialen Keyframe zu erzeugen, wobei der Gleitfenstergraph basierend auf dem initialen Keyframe erzeugt wird.
18. UE nach Anspruch 12, wobei die Anweisungen ferner bewirken, dass der Prozessor die Merkmalserfassung und -nachverfolgung des aufgenommenen Frames mit der Tiefenmessung des erfassten Merkmals synchronisiert.
19. UE nach Anspruch 12, wobei die Anweisungen ferner bewirken, dass der Prozessor eine Merkmalsgeschwindigkeit in Bild- und Tiefenbereichen basierend auf der Merkmalserfassung und -nachverfolgung und der Tiefenmessung berechnet.
20. UE nach Anspruch 14, wobei, beim Optimieren des Gleitfenstergraphen, die Anweisungen ferner bewirken, dass der Prozessor: eine Marginalisierung von Orientierungspunkten in dem aufgenommenen Frame durchführt; und eine Marginalisierung von einer Keyframe-Pose, Geschwindigkeit und Bias-Termen durchführt.

Images