[0001] This application claims the benefit of priority of U.S. provisional application serial no. 62/535,038, filed on July 20, 2017 the disclosure of which is herein incorporated by reference in its entirety.

FIELD

[0002] The device and method disclosed in this document relates to image processing and, more particularly, to dense visual SLAM (Simultaneous Localization and Mapping) methods.

BACKGROUND

[0003] Unless otherwise indicated herein, the materials described in this section are not prior art to the claims in this application and are not admitted to the prior art by inclusion in this section.

[0004] Three-dimensional (3D) position tracking is one of the fundamental techniques required to align the virtual and real world together in Augmented Reality applications, but is also applicable to many other useful application. Visual SLAM (Simultaneous Localization and Mapping) is the dominant technology for reconstruction of camera trajectory and the environment, especially in absence of any prior knowledge on the environments. A visual SLAM system typically consists of a front-end and a back- end. Visual odometry is the front-end that estimates the per-frame camera pose, which is essential in Augmented Reality applications to seamlessly align virtual objects with the real world. In the back-end, normally, camera poses are refined through a global optimization. Some methods also reconstruct the model of the environment which can also be further used for many other purposes, such as 3D mapping, physically-based simulation, and animation. The majority of existing visual SLAM techniques can be grouped into two categories, frame -to -frame and frame -to-model approaches, based on how the back-end map information is maintained and utilized for front-end pose estimation.

[0005] Frame -to -frame approaches are typically keyframe -based and rely on pose- pose constraints between a frame and a keyframe for pose estimation (e.g., DVO and σ- DVO). Specifically, a set of keyframes are identified during the visual odometry. For each frame associated with a keyframe, a relative camera pose is computed with respect to this keyframe. When loop closure is detected, the current keyframe can be associated with previous keyframes to create more pose-pose constraints. Considering all the frames, a pose graph can be constructed that represents the pose-pose constraints across frames. Then a pose-graph optimization can be performed to refine the camera pose of each frame. However, frame -to -frame approaches do not maintain a global representation of the scene (e.g., point cloud) and suffer from accumulated camera drift.

[0006] In the frame -to-model approaches, such as PTAM, KinectFusion, and ElasticFusion, a global map of the scene is usually maintained and updated. Popular map representations include point clouds, surfel clouds, and volumetric fields. These approaches can provide accurate models of the environment. However, camera pose estimation is typically performed in a frame -to-model fashion, where only the pose-point constraint between the global map and the current frame observation is considered. Normally, the back-end optimization is performed to only optimize the keyframe poses and the global point cloud with pose-point constraints, which means that the other frames between keyframes are never optimized. Some frame -to-model approaches do not perform any form of global optimization at all. Consequently, the accuracy of the final camera trajectory is limited. For the approaches that only maintain sparse point clouds, generating accurate dense meshes is very challenging. Moreover, approaches using dense map representations, especially volumetric representation, typically suffer from low spatial resolution and rely heavily on GPU acceleration.

[0007] RGBD dense Visual SLAM approaches have shown their advantages in robustness and accuracy in recent years. However, there are still several challenges such as sensor noise and other inconsistences in RGBD measurements across multiple frames that could jeopardize the accuracy of both camera trajectory and scene reconstruction. It would be beneficial to provide a dense visual SLAM method that properly accounts for sensor noise and other inconsistences in sensor measurements. It would be further advantageous, the method utilized a both pose-pose and pose-point constraints in an efficient manner for back-end optimization.

SUMMARY

[0008] A method for visual simultaneous localization and mapping is disclosed. The method comprises: storing, in a memory, a first data structure having a plurality of surfels representing a 3D environment, each surfel having a 3D position, an uncertainty of the 3D position, an intensity, an uncertainty of the intensity, and a surface normal, the 3D position of each surfel being in a first coordinate space; receiving, with a processor, a first image of the 3D environment from a camera, the first image having an array of pixels, each pixel having an intensity and a depth; estimating, with the processor, a first camera pose from which the camera captured the first image based on the first image and the first data structure; and updating, with the processor, at least one surfel in the plurality of surfels of the first data structure based on the first image and the first camera pose.

[0009] A visual simultaneous localization and mapping system is disclosed. The system comprises: a camera configured to capture a plurality of images of a 3D environment, each image having an array of pixels, each pixel having an intensity and a depth; a memory configured to store a first data structure having a plurality of surfels representing the 3D environment, each surfel having a 3D position, an uncertainty of the 3D position, an intensity, an uncertainty of the intensity, and a surface normal, the 3D position of each surfel being in a first coordinate space; and a processor. The processor is configured to: receive a first image of the plurality of images of the 3D environment from the camera; estimate a first camera pose from which the camera captured the first image based on the first image and the first data structure; and update at least one surfel in the plurality of surfels of the first data structure based on the first image and the first camera pose.

BRIEF DESCRIPTION OF THE DRAWINGS

[0010] The foregoing aspects and other features of the method and system are explained in the following description, taken in connection with the accompanying drawings. [0011] FIG. 1 shows an exemplary embodiment of a three-dimensional localization and mapping system.

[0012] FIG. 2 shows exemplary components of the three-dimensional localization and mapping system of FIG. 1.

[0013] FIG. 3 shows a logical flow diagram for a method for performing dense visual SLAM using probabilistic surfel maps.

[0014] FIG. 4 shows the generation of a new keyframe probabilistic surfel maps using a new keyframe and a global probabilistic surfel map.

[0015] FIG. 5 shows a pose-graph having pose-pose constraints.

[0016] FIG. 6 shows a pose-graph having pose-pose constraints and pose-point constraints.

[0017] FIG. 7 shows an alternating pose-pose and pose-point iterative optimization strategy.

DETAILED DESCRIPTION

[0018] For the purposes of promoting an understanding of the principles of the disclosure, reference will now be made to the embodiments illustrated in the drawings and described in the following written specification. It is understood that no limitation to the scope of the disclosure is thereby intended. It is further understood that the present disclosure includes any alterations and modifications to the illustrated embodiments and includes further applications of the principles of the disclosure as would normally occur to one skilled in the art which this disclosure pertains. Three-Dimensional Localization and Mapping System

[0019] With reference to FIGS. 1 -2, exemplary embodiments of a three-dimensional localization and mapping system 10 are shown. As shown in FIG. 1 , the localization and mapping system 10 at least includes a camera 20. The camera 20 is configured to capture a plurality of images of a scene 30 as it is moved around the scene 30. The scene 30 comprises a real-world environment and, as such, may contain a variety of objects or structures therein. As shown, at a given camera position and orientation (camera pose), the camera 20 has a restricted field of view 40 such that an image captured by the camera 20 at the given camera pose only includes a corresponding portion 50 of the scene 30.

[0020] The camera 20 is configured to generate images of the scene 30 having both photometric information (intensity, color, and/or brightness) and geometric information (depth and/or distance). Particularly, in at least one embodiment, the camera 20 is an RGB-D camera configured to generate RGB-D images. Each RGB-D image comprises two-dimensional array of pixels. In at least one embodiment, each pixel includes RGB color intensity values and a depth value. However, it will be appreciated that the camera 20 may comprises a monochromatic and/or greyscale camera which captures images having pixels with only an intensity value and a depth.

[0021] FIG. 2 shows exemplary components of one embodiment of the three- dimensional localization and mapping system 10. Particularly, in addition to the camera 20, the localization and mapping system 10 further includes at least one processor 12 and memory 14. The memory 14 is configured to store program instructions that, when executed by the processor 12, enable the three-dimensional localization and mapping system 10 to perform various operations described elsewhere herein, including visual simultaneous localization and mapping (SLAM). The memory 14 may be of any type of device capable of storing information accessible by the processor 12, such as a memory card, ROM, RAM, hard drives, discs, flash memory, or any of various other computer- readable medium serving as data storage devices, as will be recognized by those of ordinary skill in the art. Additionally, it will be recognized by those of ordinary skill in the art that a "processor" includes any hardware system, hardware mechanism or hardware component that processes data, signals or other information. The processor 12 may include a system with a central processing unit, graphics processing units, multiple processing units, dedicated circuitry for achieving functionality, or other systems.

[0022] The processor 12 is configured to process the captured images of the scene 30 to perform visual odometry to estimate the position, orientation, and trajectory of the camera 20 with respect to the scene 30 across several frames. Additionally, the processor 12 is configured to generate a three-dimensional model or map representation of the scene 30.

[0023] It will be appreciated that such mapping of the scene 30 and localization of the camera 20 within the scene 30, has many useful applications such as augmented reality, three-dimensional environment mapping or object scanning, autonomous robot navigation (e.g. vehicles, vacuum cleaners, flying drones), and other similar robotics applications. Accordingly, the three-dimensional localization and mapping system 10 may take a variety of different forms such as a smartphone, a tablet computer, a handheld camera, a head mounted display, or a sensor/processing system of a robot and/or vehicle.

[0024] In some embodiments, particularly for augmented reality applications, the three-dimensional localization and mapping system 10 further includes a display screen 16. The display screen 16 may comprise any of various known types of displays, such as an LCD or OLED screen. In some embodiments, the display screen 16 may comprise a touch screen configured to receive touch inputs from a user. In augmented reality applications, the processor 12 may be configured to operate the display screen 16 to display real-time images/video captured by the camera 20 with virtual objects superimposed thereon.

Method for Dense Visual SLAM using Probabilistic Surfel Maps

[0025] Methods for operating the augmented reality system 10 are described below. In particular, methods for dense visual SLAM are described. In the description of the methods, statements that a method is performing some task or function refers to a controller or general purpose processor executing programmed instructions stored in non- transitory computer readable storage media operatively connected to the controller or processor to manipulate data or to operate one or more components in the three- dimensional localization and mapping system 10 to perform the task or function. Particularly, the processor 12 of the three-dimensional localization and mapping system 10 above may be such a controller or processor. Alternatively, the controller or processor may be implemented with more than one processor and associated circuitry and components, each of which is configured to form one or more tasks or functions described herein. It will be appreciated that some or all of the operations the method can also be performed by a remote server or cloud processing infrastructure. Additionally, the steps of the methods may be performed in any feasible chronological order, regardless of the order shown in the figures or the order in which the steps are described. [0026] With continued reference to FIG. 2, the memory 14 of the three-dimensional localization and mapping system 10 is configured to store program instructions corresponding to a visual SLAM program 60 which implements the methods described herein. The visual SLAM program 60 incorporates a map representation referred to herein as a "Probabilistic Surfel Map" (PSM). A probabilistic surfel map is a data structure which advantageously provides a globally consistent map representation having a plurality of surfels or points that are encoded with both photometric (intensity) uncertainties and geometric (depth) uncertainties in order to address inconsistency and measurement noise issues with the RGB-D camera 20. We note that the terms "surfel" and "point" are used interchangeably herein with respect to the PSM data structure.

[0027] In conventional map representations, a scene point encodes a set of attributes representing different properties. For SLAM, typical attributes include 3D position, intensity and surface normal. However, due to various factors such as sensor measurement noise, occlusion and change of lighting conditions, there are usually mismatches across multiple observations of a single scene point. Those mismatches are the major sources of the accumulated drift in visual odometry.

[0028] To reduce the camera drift and construct a more accurate and consistent map, the PSM data structure disclosed herein encodes both the attributes as well as their uncertainties. Mathematically, we denote each point in the PSM according to the equation:

which represents the (accumulated) measurement of a scene point. It includes a 3D position £ ³ , a positional uncertainty represented via a 3 X 3 covariance matrix M , an intensity value M\, ε R ¹ , an intensity uncertainty M e M ¹ , and a 3D normal

M £ M ³ . In at least one embodiment, the distribution of 3D positional uncertainty

independently.

[0029] The PSM data structure is utilized in both the front-end visual odometry and the back-end map construction. Particularly, as will be discussed in greater detail below, the memory 14 is configured to store a global PSM 62 and a keyframe PSM 64. The global PSM 62 is a map representation of all portions of the scene 30 that have been captured in the images from the camera 20 and is used during the back-end optimization to provide pose-point constraints that are properly weighted through the encoded uncertainties. In contrast, the keyframe PSM 64 is a map representation of only a portion of the scene 30 that was captured in a most recent keyframe K _j and is used during visual odometry to provide a more accurate reference for relative camera pose estimation. As used herein, M ^I = will be used to denote the global PSM 62

and M ¹ = IM , J fj , M\, Μ^ , Μ^ } will be used to denote the keyframe PSM 64.

[0030] FIG. 3 shows a logical flow diagram for a method 100 for performing dense visual SLAM using probabilistic surfel maps. The method 100 includes a front-end 105 which performs visual odometry using the keyframe PSM 64 and a back-end 1 10 which constructs and optimizes the global PSM 62, from which the keyframe PSM 64 is derived. The method 100 improves upon the functioning of the three-dimensional localization and mapping system 10 and, more particularly, the functioning of the processor 12 thereof, by advantageously utilizing the PSM data structure for visual odometry in the front-end 105 and for optimization in the back-end 1 10. Compared to a volumetric representation, the PSM data structure advantageously maintains only discrete points and avoids the tremendous memory consumption that would result in limited spatial resolution. Additionally, the PSM data structure enables proper modeling and updating of globally consistent measurement uncertainties. As discussed in further detail below, instead of integrating massive number of points using a weighted average, the method 100 considers the distribution of fused points as the intersection of the distributions from the individuals. Using this approach, the photometric and geometric uncertainties of reliable points drop rapidly and these points be easily identified. With a sparse and reliable map, the method 100 requires less memory and computation during SLAM. Additionally, when a dense mesh is requested, the sparse map can be used as control points to deform dense point cloud from keyframes to produce a consistent dense mesh.

[0031] The method 100 begins with a step of receiving a current image frame from a camera (block 115). Particularly, with respect to the embodiments described in detail herein, the processor 12 of the three-dimensional localization and mapping system 10 is configured to execute instructions of the visual SLAM program 60 to receive an image frame from the camera 20. Particularly, in at least one embodiment, the processor 12 is configured to receive an RGB-D image frame from an RGB-D camera 20. As discussed above, each RGB-D image frame comprises two-dimensional array of pixels. In at least one embodiment, each pixel includes RGB color values and a depth value. [0032] The method 100 continues with a step of estimating a camera pose for the current frame using the keyframe PSM (block 120). Particularly, with respect to the embodiments described in detail herein, the processor 12 is configured to execute instructions of the visual SLAM program 60 to estimate a relative camera pose between the current RGB-D image frame and the keyframe PSM 64. More particularly, the processor 12 is configured to calculate the optimal relative camera pose that minimizes photometric and geometric errors between the points M ¹ of the keyframe PSM 64 and measurements of the current RGB-D image frame, when the points M ¹ of the keyframe PSM 64 are transformed into the camera coordinate space of the current RGB-D image frame using the relative camera pose.

[0033] Particularly, each RGB-D pixel of the current RGB-D image frame can be treated as a measurement and is modeled in a similar way as a point in the PSM data structure. Particularly, each measurement is modeled as two Gaussians distributions and one normal denoted as p ^L ,∑p, I ¹ , σ} ² , nj, where p ^L is a 3D point in the camera coordinate space generated from the corresponding RGB-D pixel,∑p is the positional uncertainty in the camera coordinate space and is computed based-on a sensor noise model for the

RGB-D camera 20; I ¹ is the intensity of the corresponding RGB-D pixel, σ\ is the intensity uncertainty and is assigned a large constant (e.g., 256); and n is the normal of the point p ^L and is approximated with a cross product of two vectors, one from point p ^L to its neighboring pixel in the right and another from p ^L to its neighboring pixel below. As used herein the term "camera coordinate space" refers to the 3D coordinate space relative to the current RGB-D image frame. [0034] In at least one embodiment, the processor 12 is configured compute the positional uncertainty∑p with a sensor noise model that models the uncertainty. For example, zero-mean multivariate Gaussian distributions can be used for structure light- based RGB-D camera 20. In many embodiments, the RGB-D camera 20 emits infra-red patterns and recovers depth from correspondences between two image views with a small parallax. During this process, the disparity is quantized into sub-pixels, which introduces a quantization error in the depth measurement. The noise due to quantization error is defined as:

where q _pix is the sub-pixel resolution of the device, b is the baseline, and is the focal length.

[0035] This error increases quadratically with range Z _t , thus preventing the use of depth observations from far objects. The 3D sensor noise of RGB-D cameras can be modeled with a zero-mean multivariate Gaussian distribution whose covariance matrix has the following as diagonal components: Κη Ζο ai ₂ = tan z σ| ₃ = η{Ζ{) ² (3), where the σ| ₃ is directed along the ray, and β _χ and β _γ denote the angular resolutions in x andy directions. Note that the error in the ray direction increases quadratically.

[0036] Therefore, the positional uncertainty∑p for each 3D point p ^L can be expressed as:

∑ _P = σ ₂ ² ₂ , ff| ₃ )R _ay (4), where R _ray denotes the rotation matrix between the ray and camera coordinates, and R _ray is the transpose of R _ray .

[0037] A camera pose is a rigid body transformation in special Euclidean group SE(3). Thus, the relative camera pose between the current RGB-D image frame and the keyframe PSM 64 can be expressed as a 4 x 4 homogeneous transformation matrix T, which includes a 3 X 3 rotation matrix R £ SO (3) and a 3D translation vector t £ I ³ . Given a point M ¹ in the in keyframe PSM 64 and a camera pose T corresponding to a certain frame, we can transform the 3D position M of the point M ¹ from the PSM coordinate space to the camera coordinate space as Pj : Pj = #(>f _p ; T) = Rpi + t. The inverse transformation T ^_1 transforms p; from the camera coordinate space back to the PSM coordinate space. Generally, for pose optimization, the transformation T is represented as a six dimensional twist vector ξ given by Lie algebra se(3). The transformation T and ξ are used interchangeably to denote a camera pose. As used herein the term "PSM coordinate space" (which may also be referred to herein as the "world coordinate space" or "world space") refers to the 3D coordinate space of the keyframe PSM 64 and the global PSM 62.

[0038] Projection of a point pj in the camera coordinate space onto the 2D image space of the an RGB-D image frame is denoted herein by the projection function X _j =

7r(pj). Similarly, back-projection of a 2D point X _j with depth value (X _j ) into the camera coordinate space is denoted herein as p; = π ^-1 (xj. Z(Xj)) . For a projected point 2D point

Xj , the corresponding intensity and depth measurements based on the respective RGB-D image are denoted herein as /(x;) and (X _j ) , respectively, and may be determined as an interpolation of neighboring pixels in the RGB-D image. [0039] As mentioned above, the processor 12 is configured to estimate a relative camera pose ξ between the current RGB-D image frame and the keyframe PSM 64. Particularly, the processor 12 is configured to calculate the optimal relative camera pose that minimizes photometric and geometric errors between the points M ¹ of the keyframe PSM 64 and the measurements of the current RGB-D image frame, when the points M ¹ of the keyframe PSM 64 are transformed into the camera coordinate space of the current RGB-D image frame using the relative camera pose. Given the camera projection model and rigid body motion, the photometric and geometric errors of each point are defined by a residual error term r¾ according to the equation:

where r/ is the photometric error, r is the geometric error, the 2D point X; (< ) represents a projection of a respective point in the most recent keyframe K _j onto the current RGB-D image frame with the relative camera pose ξ, /( j(( )) represents a photometric measurement at the 2D point X; (< ) in the current RGB-D image frame, T ^l denotes the corresponding photometric measurement in the keyframe K _j , Z(x _j (( )) represents a geometric measurement at the 2D point X;(< ) in the current RGB-D image frame, and Z ^l denotes the corresponding geometric measurement in the keyframe K _j . [0040] However, with the Keyframe PSM 64, we can re-define the residual term of equation (5) with the following changes:

¾( ) = i (^(¾())

/ ^" = ΜΪ (6), where n g M^, ( )J represents the represents a projection of a respective point M ^l in the keyframe PSM 64 onto the current RGB-D image frame with the relative camera pose ξ, M, ¹ denotes the photometric intensity of the respective point M ^l , M _p denotes the 3D position of the respective point M ^l in the PSM coordinate space, and [,g ( f _p , ^)] _z denotes the z component of the respective point M ^l transformed into the camera coordinate space.

[0041] Using the notations described above, the calculation of the optimal relative camera pose ξ can be expressed mathematically according to the energy function:

where n is the total number of points M ¹ or number of reliable points M ¹ in the keyframe PSM 64, r is the transpose of the residual error term r¾, and W _j is a 2 X 2 weighting matrix which leverages the importance of each residue term. In at least one embodiment, the optimization of this energy function can be solved through an image- warping optimization framework such as the Lucas-Kanade algorithm with an inverse- composition loss function.

[0042] As the energy function from the equation (7) is non-linear with respect to the camera pose ξ, the least squares optimization is sensitive to outliers. So the matrix Wj plays a vital role in rejecting/under-weighting outliers to achieve robust camera pose estimation. In at least one embodiment, the distribution of photometric residuals is modeled as being close to a zero-mean student-t distribution with five degrees of freedom (i.e., v = 5):

[0043] In contrast, in one embodiment, the sensor noise model described above is used to model the distribution of geometric residuals. Thus, in at least one embodiment, a hybrid weighting strategy is used, which applies student-t distribution for the photometric residuals and the sensor noise model for the geometric residuals. Such a hybrid weighting advantageously achieves a significant boost in robustness and accuracy. In the hybrid weighting strategy, the weight matrix Wj is modeled as

1/2

W _/ = W; AW; (9),

1

W ² = diag(a)[, of (10),

= ( r¾ ¹ r ^T + [¾] _{3 3} ) - ¹ (12).

[0044] In equations (9)-(12), ω[ is a weight based on the intensity student-t distribution, while cof is propagated from the sensor noise model via first-order Taylor expansion.∑ _p . and∑ _p are the covariance of the 3D point pj and its corresponding point in current image ρ· . Λ here is a normalizer of all the weights in Wj. By combining the equations (6)-(12) with equation (7), the calculation of the optimal relative camera pose with hybrid approach can be described as:

h _ybr i _d = r (13).

[0045] The method 100 continues with a step of updating the keyframe PSM using the current frame (block 125). Particularly, with respect to the embodiments described in detail herein, once the relative camera pose ξ for the current RGB-D image frame is calculated, the processor 12 is configured to execute instructions of the visual SLAM program 60 to update the keyframe PSM 64 with the observation points of the current RGB-D image frame. More particularly, once the relative camera pose ξ for the current RGB-D image frame is calculated, the processor 12 is configured to update the 3D positions M^, the positional uncertainties M _∑ , the intensities M, ¹ , and the intensity uncertainties J f^ ₂ of each point in the keyframe PSM 64 having an associated point in the current RGB-D image frame.

[0046] As discussed above, due to various factors including sensor noise, changes of lighting conditions, etc., the sensor measurements in any particular RGB-D image frame are considered to be biased observations with modeled with uncertainties. However, over several frames, the processor 12 is configured to update the uncertainties via a fusion of their respective probabilistic density functions. Through the gradual update of the keyframe PSM 64 during visual odometry, the method 100 builds a consistent and reliable model of the environment.

[0047] Once the relative camera pose ξ for the current RGB-D image frame is calculated, the processor 12 is configured to update the keyframe PSM 64 with the observations from the current RGB-D image frame. First, the processor 12 is configured to calculate a plurality of observation points ρ ^ι ,∑ _ρ , / ^ι , σ ² , η| in the PSM coordinate space corresponding to a plurality of 2D points Xj in the 2D image space of the current RGB-D image frame, each of which are the interpolation of neighbor RGB-D pixels at a projection of the keyframe PSM 64 onto the current RGB-D image frame. Particularly, in at least one embodiment, the processor 12 is configured to calculate the observation points according to the equations: =

. ^v (14),

n = g ((Pxn -≠') X (Pyn - P ¹ ')' ^{7 '1} ) where p ^l is the 3D position of the respective observation point in the camera coordinate space, p ^l is the 3D position of the respective observation point in the PSM coordinate space,∑p is the positional uncertainty of the respective observation point transformed into the PSM coordinate space, I ¹ is the intensity of the respective observation point, σ\ is the intensity uncertainty of the respective observation point, and n is the normal of the respective observation point transformed into the PSM coordinate space. [0048] It will be appreciated that each of these observation points is associated with a corresponding point M ¹ in the keyframe PSM 64. Therefore, given a new observation point, the processor 12 is configured to update the corresponding point M ^l in the keyframe PSM 64 with a fusion of two Gaussian distributions for both the position and the intensity according to the equations:

Jv (jS¾, ^j ¾) x J -(p )

[0049] We denote this update operation with the symbol φ. With the Gaussian distribution modeling, the update operation can be described in more detail as follows:

J ¾ © [ρ Σ , , σ^, η)≡ [Μ^, Μ^, Μί, Μ^, Μ^) (16), and

= >¾

where J f^ is the updated 3D position of the respective point M ¹ in the keyframe PSM 64, ¾ _p is t ^ne updated positional uncertainty of the respective point M ¹ in the keyframe PSM 64, Mj is the updated intensity of the respective point M ¹ in the keyframe PSM 64, M _ff ^l 2 is the updated intensity uncertainty of the respective point M ¹ in the keyframe PSM

64, and f„ is the normal of the respective point M ¹ in the keyframe PSM 64.

[0050] By updating the keyframe PSM 64 after each image frame using the above operation, the uncertainties J f^ and ¾ _p ^w iU drop quickly if observation points are correctly associated with the points M ¹ of the keyframe PSM 64. This is because the method 100 takes the intersection between Gaussian distributions during fusion, which is the major difference from conventional visual SLAM methods. In at least one embodiment, the normal is only used to eliminate incorrect associations, we simply keep the original normal instead of updating it with new measurements. In other words, if the normal of the generated observation point in the PSM coordinate space is not equal to or at least within a predetermined threshold closeness of the normal of a respective point M ¹ of the keyframe PSM 64, the association is considered to be incorrect and the respective point M ^l is not updated with the observation point. [0051] The steps 1 15, 120, and 125 collectively comprise the front-end 105 of the method 100, and are performed for each RGB-D image frame received by the processor 12 from the RGB-D camera 20. Accordingly, the keyframe PSM 64 is continually updated a refined until a new keyframe is selected, thereby improving the visual odometry performed with respect to each intermediate RGB-D image frame. Below we describe how the PSM data structure, in particular the global PSM 62, is utilized in the back-end 1 10 to improve performance in keyframe selection and global optimization.

[0052] The method 100 continues with a step of determining whether the current frame should be selected as a new keyframe (block 130). Particularly, with respect to the embodiments described in detail herein, the processor 12 is configured to execute instructions of the visual SLAM program 60 to determine whether the current RGB-D image frame should be treated as a new keyframe Kj ₊₁ based on at least one criterion.

[0053] In one embodiment, the processor 12 is configured to select the current RGB- D image frame to be a new keyframe Kj ₊₁ in response to the current RGB-D image frame being captured more than a predetermined threshold amount of time since the most recent keyframe Kj was captured.

[0054] In one embodiment, the processor 12 is configured to select the current RGB- D image frame to be a new keyframe Kj ₊₁ in response to the calculated relative camera pose ξ for the current RGB-D image frame being greater than a predetermined spatial distance from a camera pose ξ _κ . of the most recent keyframe Kj .

[0055] In one embodiment, the processor 12 is configured to select the current RGB-

D image frame to be a new keyframe Kj ₊₁ in response to the entropy of the calculated relative camera pose ξ between the most recent keyframe Kj and the current RGB-D image frame being lower than a predetermined threshold ratio of the entropy of the camera pose between the most recent keyframe K _j and its adjacent frame K _j + 1.

[0056] In one embodiment, the processor 12 is configured to select the current RGB- D image frame to be a new keyframe K _{j +1} in response to a number of points in the keyframe PSM 64 being active points and/or visible points in the current RGB-D image frame being lower than a predetermined threshold.

[0057] In one embodiment, the processor 12 is configured to select the current RGB- D image frame to be a new keyframe K _{j +1} in response to an average uncertainty of

∑"=o¾ . ∑?=o ¾

intensity or an average uncertainty of depth within the keyframe PSM of current keyframe K _j being higher than a respective predetermined threshold that indicates significant mismatch between the map and current frame.

[0058] We note that if the current RGB-D image frame is a first image frame and no keyframes have yet been selected, then the processor 12 is configured to automatically select the current RGB-D image frame as a new keyframe K _j+1 .

[0059] If a new keyframe is selected, the method 100 continues with a step of updating the global PSM based on the keyframe PSM (block 135). Particularly, with respect to the embodiments described in detail herein, once a new keyframe K _j+1 is selected, the processor 12 is configured to execute instructions of the visual SLAM program 60 to update the global PSM 62 with the points M ¹ of the keyframe PSM 64 of the previous keyframe K _j . More particularly, once a new keyframe K _j+1 is selected, the processor 12 is configured to update the 3D positions M , the positional uncertainties

-M ^' i _p , the intensities M, ¹ , and the intensity uncertainties J f^ ₂ of each point M ^l in the global PSM 62 having an associated point M ¹ in the keyframe PSM 64. Additionally, in some embodiments, the processor 12 is configured to add any new points M ^l of the keyframe PSM 64 that do not have an associated point M ¹ in the global PSM 62 to the global PSM 62.

[0060] The processor 12 is configured to update the global PSM 62 with the points of the keyframe PSM 64 in a similar manner as described above with respect to block 125 of FIG. 3, in which the keyframe PSM 64 was updated based on each intermediate image frame. Particularly, the processor 12 is configured to treat the points M ¹ of the keyframe

PSM 64 as a plurality of observation points p ^L ,∑p, / ^L , σ} ² , nj and update the positional uncertainties M _∑v and the intensity uncertainties J f^ ₂ of each point M ¹ in the global

PSM 62 having an associated point M ¹ in the keyframe PSM 64 using the equations (15)-(17), in the manner discussed above.

[0061] The method 100 continues with a step of generating a new keyframe PSM for the new keyframe (block 140). Particularly, with respect to the embodiments described in detail herein, the processor 12 is configured to execute instructions of the visual SLAM program 60 to generate a new keyframe PSM 64 based on the global PSM and any new observations in the new keyframe K _{j +1} .

[0062] During visual odometry, the keyframe PSM 64 typically covers only part of scene. With a new keyframe K _j+1 identified, it is very likely there will be new observations of the scene that does not exist in the global PSM 62 yet. The processor 12 is configured to generate the new keyframe PSM 64 for new keyframe K _{j+ 1} by combining existing parts in the global PSM 62 that are visible in new keyframe K _j+1 with the new observations from new keyframe Kj _{+ 1} . As shown in FIG 4, we project the global PSM 62 onto the image space of the new keyframe Kj ₊₁ and identify active points from of global PSM 62 (shaded with a diagonal pattern) that are visible in the new keyframe Kj ₊₁ . Additionally, for the new regions (shaded with a cross-hatch pattern) in the keyframe that have no overlap with the global PSM, the processor 12 is configured back- project them into the camera coordinate space of the new keyframe Kj ₊₁ to obtain another set of observation points. For these points, the variance for intensity uncertainty

2

is set to a default value σ° , while the covariance matrix for positional uncertainty is derived from the sensor noise model, discussed above. Thus, the processor 12 is configured to generate the new keyframe PSM 64 with the active/visible points of the global PSM 62 and with any new observation points that do not overlap with the global PSM 62.

[0063] In at least one embodiment, the processor 12 is configured to prune and/or remove the points M ¹ of the new keyframe PSM 64 having with positional uncertainty -Μ-Σ beyond a predetermined threshold. Thus, only the reliable points are included in the keyframe PSM 64. Additionally, in some embodiments, after the initial pruning, the processor 12 is configured to keep only a ratio of the rest points past pruning. Thus, the keyframe PSM 64 is generally more sparsely populated than the global PSM 62, which improves processing speed.

[0064] Returning to FIG. 3, once the new keyframe PSM 64 is generated, the method 100 returns to the processes of the front-end 105 to perform visual odometry for new RGB-D image frames with respect to the new keyframe Kj ₊₁ according the blocks 1 15, 120, and 125. [0065] In addition to returning the to the processes of the front-end 105, the method 100 continues in the back-end 1 10 with a step of detecting whether there is a loop closure (block 145) and, if a loop closure is detected, with steps of adding pose-pose constraints (block 150) and performing local pose-graph optimization (block 155). Particularly, with respect to the embodiments described in detail herein, the processor 12 is configured to execute instructions of the visual SLAM program 60 to detect whether the newly selected keyframe Kj ₊₁ comprises a loop-closure with respect to a previous keyframe K ₀ , ... , Kj and, if loop closure is detected, generate pose-pose constraints based on the detected loop-closure, and perform local pose-graph optimization based on the pose-pose constraints.

[0066] Particularly, in the back-end 1 10, the processor 12 is configured to maintain and/or store in the memory 14 a pose-graph having nodes and edges. As used herein, a "pose-graph" refers to a data structure having a plurality of nodes which define values or measurements and a plurality of edges connecting pairs of nodes which define a relationship between the values or measurements of the connected nodes.

[0067] As new keyframes Kj are selected, the processor 12 is configured to add their keyframe camera poses ξ _κ . as new nodes in a pose-graph and their relative transformations as edges in the pose-graph, thereby creating a chain of keyframe camera poses ξ _κ linked by pose-pose constraints. As used herein, a "pose-pose constraint" is a relative transformation between two camera poses. FIG. 5 shows a pose-graph 200 which is used for local optimization based on pose-pose constraints. Each pose (shown as nodes/circles) of the pose-graph 200 is a keyframe camera pose ξ _κ . of a keyframe Kj . Each pose-pose constraint (shown as solid edges/lines) of the pose-graph 200 represents relative transformations between the two respective keyframe camera poses ξ _κ .

[0068] To correct for the accumulated error in the trajectory, the processor 12 is configured to search for loop-closures to previously visited keyframes. As used herein, a loop-closure refers to when a keyframe pose is determined to be the same as a previous keyframe pose or within a predetermined closeness to a previous keyframe pose. To identify candidate previous keyframes that might form a loop closure with the new keyframe, the processor 12 is configured to search nearest neighbor keyframe poses in a kd-tree. In one embodiment, in order to validate a loop-closure, the processor 12 use the entropy ratio test discussed above for keyframe selection. However, instead of the entropy of the transformation of the first frame to the keyframe, the processor 12 is configured to use the average entropy of all successful matches of intermediate frames to the keyframe. If a valid loop closure is detected, the processor 12 adds a new edge (pose- pose constraint) to the pose-graph. As shown in FIG. 5, the pose-graph 200 includes an additional pose-pose constraint between the keyframe camera poses ξ _κ and ξ _κ , indicating a loop closure.

[0069] Afterwards, the processor 12 is configured to correct errors in the keyframe poses ξ _κ . and/or optimize the keyframe poses ξ _κ . based on the pose-pose constraints by solving a non-linear least squares optimization problem. In one embodiment, the processor 12 uses a graph optimization framework to correct the errors such as the framework described in "G2o: A general framework for graph optimization," in 201 1 IEEE International Conference on Robotics and Automation (ICRA) by R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige and W. Burgard. [0070] Returning to FIG. 3, the method 100 continues with a step of determining whether global optimization should be performed (block 160). Particularly, the local pose-graph optimization based on the pose-pose constraints, discussed above, can be performed in real-time while the method 100 continues to receive new RGB-D image frames from the camera 20. However, further global optimization can be performed by establishing pose-point constraints using the global PSM 62. In at least one embodiment, the processor 12 is configured to determine that global optimization is to be performed once the method 100 has stopped receiving new RGB-D image frames. However, in some embodiments, global optimization may be performed periodically in real-time.

[0071] If global optimization is to be performed, the method 100 continues with a step of iteratively alternating between pose-pose optimization and pose-point optimization (block 165). Particularly, with respect to the embodiments described in detail herein, the processor 12 is configured to execute instructions of the visual SLAM program 60 to add pose-point constraints to the pose-graph and perform global optimization based on the pose-point constraints and the pose-pose constraints.

[0072] Particularly, as discussed above, the processor 12 is configured to maintain and/or store in the memory 14 a pose-graph having a plurality of nodes which define values or measurements and a plurality of edges connecting pairs of nodes which define a relationship between the values or measurements of the connected nodes. As described above, a plurality keyframe camera pose ξ _κ of a keyframes Kj were added as nodes of the pose-graph and a plurality of pose-pose constraints were added as edges of the pose- graph. [0073] When global optimization is performed, the global PSM 62 provides pose- point constraints that are properly weighted through the encoded uncertainties, which are be used to improve both trajectory and map accuracy. Particularly, the processor 12 is configured to add points M ¹ from the global PSM 62 to the pose-graph and add pose- point constraints indicating the consistencies and/or differences between the PSM points M ^l and their local observations from a transformation/projection to the camera coordinate space of each keyframes K _j in which they are visible. FIG. 6 shows a pose- graph 250 which is used for global optimization based on pose-pose constraints and pose- point constraints. In addition to the a keyframe camera poses ξ _κ . of the keyframes K _j , the pose-graph 250 includes PSM points M ^l (shown as nodes/circles) and pose-point constraint (shown as dashed edges/lines), which represents a transformation/projection of the PSM points M ¹ in to the keyframes K _j . In this way, during global optimization, the method 100 enforces both pose-pose constraints and pose-point constraints.

[0074] For the pose-point constraints, the measurements from PSM point M ¹ are the intensity Mf and the depth value of the point's projection in the keyframe [^(^fp, < )] ; the local measurements are I _j ( ;(< )) ^an d 2y(xj0 )) from keyframes K _j . The processor 12 is configured to optimize an error function similar to equation (13) using a residual term similar to equation (5), but with the following definition changes: = ΜΪ (18).

[0075] Note that, in the equations (18), ξ represents the transformation from the PSM coordinate space of the global PSM 62 to the camera coordinate space of the corresponding keyframe K _j instead of a relative camera pose. By minimizing the error function, both camera poses and 3D positions of the points can be optimized.

[0076] Given the complete pose-graph having poses, points, pose-pose constraints, and pose-point constraints, the processor 12 is configured to correct errors in and/or optimize the keyframe poses ξ _κ . and the points M ¹ based on the pose-pose and pose- point constraints by solving a non-linear least squares optimization problem. In one embodiment, the processor 12 uses a graph optimization framework to correct the errors such as the framework described in "G2o: A general framework for graph optimization," in 201 1 IEEE International Conference on Robotics and Automation (ICRA) by R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige and W. Burgard.

[0077] However, the error function for pose-point constraints is non-linear and sensitive to noises and outliers. Accordingly, in at least one embodiment, the processor 12 is configured to perform an iterative optimization process which alternates between optimizing only pose-pose constraints and optimizing pose-point constraints. This alternating strategy is summarized in FIG. 7, and can be more efficient and robust compared to a joint optimization strategy. At the beginning of each iteration, the processor 12 is configured to perform a pose-pose graph optimization to correct errors in the camera poses and update the PSM points according to the new poses. The relative motion between the new and old pose of the frame will transform each PSM point for next iteration of optimization. Then, the processor 12 is configured to check the pose- point constraints to remove the outliers constraints before performing the pose-point graph optimization to correct errors in the camera poses, with fixed PSM points. As PSM points are only registered to keyframes, only keyframe poses are updated during this optimization step. Therefore, the processor 12 is configured to update the local chain of intermediate frames adjacent to each keyframe to align the whole camera trajectory, and remove outlier constraints between keyframe poses to reduce the oscillation during alternative optimization. Usually, the optimization will converge quickly within 10 iterations. After the optimization of the camera poses and PSM points converges, the processor 12 is configured to perform a final pose-point graph optimization to correct errors in the PSM points, with fixed camera poses.

[0078] Returning to FIG. 3, if a dense mesh construction is requested, the method 100 continues with a step of generating dense mesh using the global PSM (block 170). Particularly, with respect to the embodiments described in detail herein, the processor 12 is configured to execute instructions of the visual SLAM program 60 to, in response to a request for a dense mesh construction, generate a dense mesh using the global PSM 62.

[0079] A common task for visual SLAM is to reconstruct the environment, and generate a dense mesh as an output. In conventional dense visual SLAM approaches, the map is represented either in a volumetric form which suffers from limited spatial resolution, or with 3D point cloud or surfels.

[0080] If dense mesh reconstruction is requested, the processor 12 is configured to generate a consistent dense mesh by deforming the depth map of each keyframe Kj towards the global PSM 62 and fusing the deformed depth maps together to generate the dense mesh. More particularly, for each keyframe Kj , the processor 12 is configured to project the points that have registered pose-point constraints with the keyframe to the 2D image coordinate space. Each projected point x _t has a depth value [g (Mp, < )] _z and its uncertainty σ = R ¾ _p R ^T in the respective keyframe Kj . Then, for each pixel Xj in the respective keyframe Kj, the processor 12 is configured to find a group of projected map points by KNN-search in the 2D image space. Given the projected map points, we adjust depth for each pixel according to the equation:

[0081] Next, the processor 12 fuses the deformed depth maps together to generate the dense mesh. Using this approach, the method 100 is able to produce consistent dense mesh once requested and also save memory and computations during visual SLAM.

[0082] While the disclosure has been illustrated and described in detail in the drawings and foregoing description, the same should be considered as illustrative and not restrictive in character. It is understood that only the preferred embodiments have been presented and that all changes, modifications and further applications that come within the spirit of the disclosure are desired to be protected.