Dense Scene Flow Estimation with the GVF Snake Model for Obstacle Detection Using a 3D Sensor in the Path-Planning Module

Figure 2 depicts a PMD CamCube 3.0 imaging model with the sources separated from the sensor. The PMD camera should ideally have two light sources mounted symmetrically on either side of the smart pixel array. To determine the depth of the obstruction, illuminate it. With a 40 [H] × 40 [V] deg FOV, the infrared light sources L_s1 and L_s2 are arranged spatially symmetrically to the camera, illuminating the obstruction with incident light rays. The light beams from the sources incident on a point (P(x,y,z)) on the obstruction in three-dimensional space. As shown in Figure 2, 1 and 2 are the angles between the surface normals of the x–y plane and the rays traveling from L_s1 and L_s2 to P(x,y,z). With respect to L_s1 and L_s2, D_opt1 and D_opt2 are the distances that the light must travel to strike point P(x, y, z) on the obstruction, such that L_s1 P = D_opt1 and L_s2 P = D_opt2. Point P's reflected light forms an angle (ζ) with respect to a surface parallel to the lens's central source (L_c). To reach the receiver pixel (p_o) on the smart pixels, the light ray is then refracted at L_c and travels LR = D_sen. At point P_c to the sensor array, this refracted ray forms an angle (ζ) with respect to the surface. The focal length (f) of the camera determines how far apart the imaging lens and sensor array are from one another. Depending on the location of each pixel in 3D space, each one has a unique travel time.

Figure 2:

The depth information (D_model(x,y,z)) can be modeled as follows: (2) Dmodel(x,y,z)=(Dopt1+Dopt22+Dref+Dsen)/2 {D_{{{model}} \left( {x,y,z} \right)}} = \left( {\frac{{{D_{opt1}} + {D_{opt2}}}}{2} + {D_{ref}} + {D_{sen}}} \right)/2

Additional depth information and depth data acquisition are possible with the TOF camera. In the experimental setup, a PMD CamCube 3.0 camera is mounted on a table's horizontal rail to allow perpendicular motion toward and away from a fixed board. The distance at which the depth and amplitude images are measured is different between the plain perpendicular board and camera, i.e., 500 mm, 750 mm, 1000 mm, 1250 mm, 1500 mm, 1750 mm, 2000 mm, 2250 mm, and 2500 mm. As seen in Figure 3, these measured depth images are plotted.

Figure 3:

The Lucas–Kanade algorithm [21] is an intensity-based differential technique that operates under the presumption that the flow vector is consistent within a pixel neighborhood. The flow vectors are determined by fitting a constant model for V→ {\rm{\vec V}} in each spatial neighborhood to a set of weighted least-squares local first-order constraints [6]. Eq. (3) is minimized to obtain the velocity estimate. (3) W2(x,y,z)=∇R(x,y,z,t)V→2+R(x,y,z,t)|2 x,y,z∈Ω {{\rm{W}}^2}\left( {{\rm{x}},{\rm{y}},{\rm{z}}} \right) = \nabla {\rm{R}}\left( {{\rm{x}},{\rm{y}},{\rm{z}},{\rm{t}}} \right){{\rm{\vec V}} ^2} + {\left. {{\rm{R}}\left( {{\rm{x}},{\rm{y}},{\rm{z}},{\rm{t}}} \right)} \right|^2}\;{\rm{x}},{\rm{y}},{\rm{z}} \in \Omega where W (x, y, z) denotes a Gaussian windowing function. The velocity estimate is given by Eq. (4). (4) V→=[ATW2A]−1ATW B {\rm{\vec V}} = {[{{\rm{A}}^T}{W^2}{\rm{A}}]^{ - 1}}{{\rm{A}}^T}{\rm{W\,B}} where (5) A=[∇R(x1,y1,z1),…,∇R(xn,yn,zn)] {\rm{A}} = \left[ {\nabla {\rm{R}}\left( {{{\rm{x}}_1},{{\rm{y}}_1},{{\rm{z}}_1}} \right), \ldots ,\nabla {\rm{R}}\left( {{{\rm{x}}_n},{{\rm{y}}_n},{{\rm{z}}_n}} \right)} \right] (6) W=diag[W(x1,y1,z1),…,W(xn,yn,zn)] {\rm{W}} = {\rm{diag}}\left[ {{\rm{W}}\left( {{{\rm{x}}_1},{{\rm{y}}_1},{{\rm{z}}_1}} \right), \ldots ,{\rm{W}}\left( {{{\rm{x}}_n},{{\rm{y}}_n},{{\rm{z}}_n}} \right)} \right] (7) B=−(Rt(x1,y1,z1),…,Rt(xn,yn,zn)) {\rm{B}} = - \left( {{{\rm{R}}_t}\left( {{{\rm{x}}_1},{{\rm{y}}_1},{{\rm{z}}_1}} \right), \ldots ,{{\rm{R}}_t}\left( {{{\rm{x}}_n},{{\rm{y}}_n},{{\rm{z}}_n}} \right)} \right)

The gradient constraints are combined with a global smoothness term in the Horn–Schunck algorithm [18]. The flow velocity can be calculated by minimizing the squared error quantity of the constraint equation and smoothness constraint. The condition for global smoothness is provided as follows: (8) ‖∇Vx‖2,‖∇Vy‖2,‖∇Vz‖2=∑i=x,y,z(∂Vi∂x)2+(∂Vi∂y)2+(∂Vi∂z)2 {\left\| {\nabla {V_x}} \right\|^2},{\left\| {\nabla {V_y}} \right\|^2},{\left\| {\nabla {V_z}} \right\|^2} = \sum\limits_{i = x,y,z} {{{\left( {\frac{{\partial {V_i}}}{{\partial x}}} \right)}^2} + {{\left( {\frac{{\partial {V_i}}}{{\partial y}}} \right)}^2} + {{\left( {\frac{{\partial {V_i}}}{{\partial z}}} \right)}^2}}

Eq. (9) specifies the error that must be minimized. (9) E2=∫D(∇R.V→+Rt)2+α2(‖∇Vx‖2+‖∇Vy‖2+‖∇Vz‖2)dxdydz {E^2} = \int_D {{{\left( {\nabla R{.}\vec V + {R_t}} \right)}^2}} + {\alpha ^2}\left( {{{\left\| {\nabla {V_x}} \right\|}^2} + {{\left\| {\nabla {V_y}} \right\|}^2} + {{\left\| {\nabla {V_z}} \right\|}^2}} \right)dxdydz where α is a term used to determine how much of an impact the smoothness constraint has. The weighted least squares fit of local constraints is applied using a locally implemented [21] combined differential approach.

To the global smoothness constraint and a constant model for flow velocity [18], the predicted velocities can be decreased as follows: (10) E2=∫D(WN2(∇R.V→+Rt)2+α2(‖∇Vx‖2+‖∇Vy‖2+‖∇Vz‖2)dxdydz {E^2} = \int_D {\left( {W_N^2{{\left( {\nabla R{.}\vec V + {R_t}} \right)}^2}} \right.} + {\alpha ^2}\left( {{{\left\| {\nabla {V_x}} \right\|}^2} + {{\left\| {\nabla {V_y}} \right\|}^2} + {{\left\| {\nabla {V_z}} \right\|}^2}} \right)dxdydz where W_N (x, y, z) denotes a Gaussian windowing function. α is a weighting term that identifies the influence of the smoothness constraint. Here, A^T W² A is calculated as follows: (11) ATW2A=(ΣW2Rx2ΣW2RxRyΣW2RxRzΣW2RxRyΣW2Ry2ΣW2RyRzΣW2RxRzΣW2RyRzΣW2Rz2) {A^T}{W^2}A = \left( {\begin{array}{*{20}{c}}{\Sigma {W^2}R_x^2}&{\Sigma {W^2}{R_x}{R_y}}&{\Sigma {W^2}{R_x}{R_z}}\\{\Sigma {W^2}{R_x}{R_y}}&{\Sigma {W^2}R_y^2}&{\Sigma {W^2}{R_y}{R_z}}\\{\Sigma {W^2}{R_x}{R_z}}&{\Sigma {W^2}{R_y}{R_z}}&{\Sigma {W^2}R_z^2}\end{array}} \right)

The velocity estimates can be solved through an iterative process [6]. (12) Vxn+1=Vxn−WN2Rx(RxVx+RyVy+RzVz+Rt)α2WN2(Rx2+Ry2+Rz2) V_x^{n + 1} = V_x^n - \frac{{W_N^2{R_x}\left( {{R_x}{V_x} + {R_y}{V_y} + {R_z}{V_z} + {R_t}} \right)}}{{{\alpha ^2}W_N^2\left( {R_x^2 + R_y^2 + R_z^2} \right)}} (13) Vyn+1=Vyn−WN2Ry(RxVx+RyVy+RzVz+Rt)α2WN2(Rx2+Ry2+Rz2) V_y^{n + 1} = V_y^n - \frac{{W_N^2{R_y}\left( {{R_x}{V_x} + {R_y}{V_y} + {R_z}{V_z} + {R_t}} \right)}}{{{\alpha ^2}W_N^2\left( {R_x^2 + R_y^2 + R_z^2} \right)}} (14) Vzn+1=Vzn−WN2Rz(RxVx+RyVy+RzVz+Rt)α2WN2(Rx2+Ry2+Rz2) V_z^{n + 1} = V_z^n - \frac{{W_N^2{R_z}\left( {{R_x}{V_x} + {R_y}{V_y} + {R_z}{V_z} + {R_t}} \right)}}{{{\alpha ^2}W_N^2\left( {R_x^2 + R_y^2 + R_z^2} \right)}}

The velocity (Vⁿ⁺¹, Vⁿ⁺¹, Vⁿ⁺¹) is calculated using the derivate estimates and the average of the previous velocity.

The spatial and temporal derivatives (R_x, R_y, R_z, R_t) are computed using the 3D GVF snake model, which is described in more detail in the following subsection, to reduce artifacts brought on by the speckle noise throughout an image frame.

For ACs, which are curves that move in an image, the GVF [30] is a new external force field. It is also referred to as a snake or deformable model. It is widely used for boundary detection and segmentation and aims to reduce their energy. The contour is then deformed iteratively to fit the object's boundary while minimizing an energy function. The energy function consists of two terms: an internal energy term that penalizes the contour's bending and stretching, and an external energy term that attracts the contour to the object's edges. The GVF snake model uses gradient vector flow to calculate the external energy term. The gradient vector flow is a vector field that points in the direction of the maximum gradient of the image. By using the gradient vector flow, the GVF snake model can track objects with complex shapes and irregular boundaries. The GVF is employed to compute the shifts in pixel values along the x, y, and z axes. The energy function that reduces to the GVF is represented by the vector field G→(x, y, z)=[u(x, y, z),v(x, y, z),w(x, y, z)] {\bar {\text G}}\left( \text{x},~\text{y},~\text{z} \right)=\left[ \text{u}\left( \text{x},~\text{y},~\text{z} \right),\text{v}(\text{x},\ \text{y},\ \text{z}),\text{w}\left( \text{x},~\text{y},~\text{z} \right) \right] [26]. (15) ∈f=ρ|∇G→|2+|∇f|2|G→−∇f|2dxdydz \in f = \rho {\left| {\nabla {\rm{\vec G}}} \right|^2} + {\left| {\nabla {\rm{f}}} \right|^2}{\left| {{\rm{\vec G}} - \nabla {\rm{f}}} \right|^2}{\rm{dxdydz}}

The trade-off between the first and second terms in the integrand is controlled by the parameter ρ, where f represents the 3D volume, ∇ stands for the gradient operator, and ρ is the regularization parameter. The following Euler equations must be resolved to determine this vector field. (16) ρ∇2u=(u−fx)|∇f|2ρ∇2v=(v−fy)|∇f|2ρ∇2w=(w−fz)|∇f|2 \begin{array}{*{20}{l}}{\rho {\nabla ^2}{\rm{u}} = \left( {{\rm{u}} - {{\rm{f}}_x}} \right){{\left| {\nabla f} \right|}^2}}\\{\rho {\nabla ^2}{\rm{v}} = \left( {{\rm{v}} - {{\rm{f}}_y}} \right){{\left| {\nabla f} \right|}^2}}\\{\rho {\nabla ^2}{\rm{w}} = \left( {{\rm{w}} - {{\rm{f}}_z}} \right){{\left| {\nabla f} \right|}^2}}\end{array} where ∇² stands for the Laplacian operator, and |∇f|² = (f² + f²), (f_x, f_y, f_z) stands for the image's edge maps in the x y z x\;\;y\;\;z appropriate directions. We obtain this by extending the Euler equations as time functions [28]. (17) u(t+δt)−u(t)+ρ ∇2u(t)+(u(t)−fx)|∇f|2=0v(t+δt)−v(t)+ρ ∇2v(t)+(v(t)−fy)|∇f|2=0w(t+δt)−w(t)+ρ ∇2w(t)+(w(t)−fz)|∇f|2=0 \begin{array}{*{20}{l}}{{\rm{u}}\left( {{\rm{t}} + \delta {\rm{t}}} \right) - {\rm{u}}\left( {\rm{t}} \right) + \rho \;{\nabla ^2}{\rm{u}}\left( {\rm{t}} \right) + \left( {{\rm{u}}\left( {\rm{t}} \right) - {{\rm{f}}_x}} \right){{\left| {\nabla f} \right|}^2} = 0}\\{{\rm{v}}\left( {{\rm{t}} + \delta {\rm{t}}} \right) - {\rm{v}}\left( {\rm{t}} \right) + \rho \;{\nabla ^2}{\rm{v}}\left( {\rm{t}} \right) + \left( {{\rm{v}}\left( {\rm{t}} \right) - {{\rm{f}}_y}} \right){{\left| {\nabla f} \right|}^2} = 0}\\{{\rm{w}}\left( {{\rm{t}} + \delta {\rm{t}}} \right) - {\rm{w}}\left( {\rm{t}} \right) + \rho \;{\nabla ^2}{\rm{w}}\left( {\rm{t}} \right) + \left( {{\rm{w}}\left( {\rm{t}} \right) - {{\rm{f}}_z}} \right){{\left| {\nabla f} \right|}^2} = 0}\end{array}

The second term will predominate in ɛ_f if the edge map of the image, ∇f, is large. If ∇f is low, the first term will dominate. ∈_f,

To deliver a smooth vector field [16], the energy function must be minimized. The vector fields give information about changes in the adjacent field. The Pixel-Mixed-Device (PMD) camera presents a simple and compact network in addition to providing adequate information about the obstacles. To determine the moving obstacle's speed, a new algorithm is used. As soon as the moving obstacle enters the camera's FOV, the path planning algorithm begins to calculate its motion from the vertices (x, y) and saves them in a temporary list. Finally, an array containing the calculated vertices of the moving obstacle is created. The range information is used to calculate the moving obstacle's speed and direction.

To evaluate the performance of the method, we have conducted a set of experiments with the Middlebury datasets [RubberW hale, Dimetrodon and Urban2], as shown in Figure 4.

Figure 4:

To compare the computed flow vectors from various methods to the ground truth Middlebury datasets [4], the AAE, ST DAE, and EPE are measured. (18) AAE(We, Wgt)=arccos(WeTWgt‖We‖⋅‖Wgt‖) AA{{E}_{\left( {{W}_{e}},\ \ {{W}_{gt}} \right)}}=\text{ }\!\!~\!\!\text{ arccos }\!\!~\!\!\text{ }\left( \frac{W_{e}^{T}{{W}_{gt}}}{\left\| {{W}_{e}} \right\|\cdot \left\| {{W}_{gt}} \right\|} \right) (19) STDAE=1n−1∑i=1n(AEi−AE¯)2 STDAE = \sqrt {\frac{1}{{n - 1}}{{\sum\limits_{i = 1}^n {\left( {A{E_i} - \overline {AE} } \right)} }^2}} where AE stands for angular error and n stands for the element's sample count. (20) AE¯=1n∑i=1n(AEi)EPE=(ue−ugt)2+(ve−vgt)2 \begin{array}{*{20}{l}}{\overline {AE} = \frac{1}{n}\sum\limits_{i = 1}^n {\left( {A{E_i}} \right)} }\\{EPE = \sqrt {{{({u_e} - {u_{gt}})}^2} + {{({v_e} - {v_{gt}})}^2}} }\end{array} where W_e = (u_e, v_e, 1) denotes the estimated flow vector and W_gt = (u_gt, v_gt, 1) is the ground truth flow vector. The errors are illustrated in Table 1 and plotted in Figure 5.

Figure 5:

Numerous tests using real Kinect depth data are conducted with various image sizes and lighting conditions to assess the effectiveness of our dense scene flow estimation method. When accessing depth information, the OpenNI library is used, and when processing image sequences, the OpenCV library is used.

By moving a paperboard in front of the sensor backward and forward while varying the lighting (light is on fully and light is on partially), the proposed dense scene flow method is evaluated in real-world scenarios. The screenshots of dense scene flow in the X–Y, X–Z, and Y–Z directions with different lighting conditions are shown in Figures 6 and 7. The findings demonstrate that the method can function effectively with objects with low texture and in any lighting situation.

Figure 6:

Figure 7:

A person is shown entering the scene from the opposite end and making their way toward the Autonomous Ground Vehicle (AGV). The objective is to identify and gauge the person's movement. Consequently, the camera is used to calculate the relative distance between the vehicle and the moving subject. The (X, Y, Z) axes of the camera frame are defined as being in the direction of the optical axis and the image axes, respectively, and (+)ve Z pointing in the direction of vision. The camera frame's origin is at the optical center. In the camera reference frame, a surface point's coordinates at time (t − 1) are (X′, Y′, Z′), and at time (t), they are (X, Y, Z). By utilizing the scene flow technique, the 3D information is used to estimate the resultant velocity as the camera detects the person's coordinates at various frame intervals. The density of the flow vectors between two various frame sequences that are computed using various techniques is shown in Figure 8.

Figure 8:

We experimented with different scenarios to gauge the performance of the suggested algorithm. The PMD camera's three-dimensional coordinates (X, Y, and Z) are used to reconstruct the three-dimensional scene flow. The dynamic obstacle is allowed to move at different known speeds within the FOV of the camera. The PMD camera is used as an external sensor, with a frame rate for the scene flow of 10 frames per second (fps).

In Experiment I, we use the ARIA library, a C++-based Pioneer mobile robot platform with the GLFW (Open Graphics Library), to create the scene flow technique to see 3D renderings of the PMD range data. The P3DX robot moves diagonally from left to right with regard to the 3D PMD camera coordinates, which are depicted as pale green lines in Figure 9 and Figure 10. These motion field estimation findings are shown for consecutive frames of moving obstacles. In these sequences, the obstacle's estimated position and orientation are represented by red lines; the obstacle's estimated speed is 200 m/s with a 10 % offset.

Figure 9:

Figure 10:

Then, a moving obstacle is allowed to move in the FOV of the camera, and different frames are captured, as shown in Figure 11. The moving obstacle's orientation can potentially be identified, and Figure 12 illustrates how its location vertices are shown in three dimensions. The results cited [15] demonstrate that the suggested method can detect moving obstacles in real-world scenarios and evaluate kinematic behavior. Figure 13 delineates the dense flow vectors that are extracted by combining scene flow and the GVF snake model from the subsequent frames to segment a moving obstacle. The gradient vector flow of the 3D image is calculated with the regularization parameter, ρ of 0.1 and α of 0.001. So, the proposed technique recognizes the orientation of the moving obstacles and provides information about the structure of the environment, and therefore, it is used for dynamic path planning in the following experiments.

Figure 11:

Figure 12:

Figure 13:

The P3DX controller's parameters are left alone during the experiment. By comparing features in succeeding images, it is possible to calculate the ego-motion of the PMD camera mounted on the P3DX. By comparing two consecutive frames, the feature detection algorithm known as “Good Features to Track” stabilizes moving P3DX. The goal of the mission is to use the graph search (Efficient D* lite [14]) algorithm on the P3DX's onboard computer to plan an P3DX's collision-free path to its desired position. To visualize 3D renderings of the PMD range data, the ARIA library, a C++-based Pioneer mobile robot platform, and the GLFW (Open Graphics Library) have been used.

As shown in Figure 14, the experiment involves performing an autonomous path planning task while exposing a PMD camera to dim night lighting conditions. The Efficient D* lite algorithm, which is implemented on the P3DX's onboard computer, is tasked with creating a path for the P3DX to travel that avoids collisions and gets it to its desired position. For the experiment, the AGV's maximum linear speed is 400 mm/s. P3DX must move from position (0, 0) to position (9 m, 0), dodging two static obstacles (i.e., tripod at (2.5, 0) and obstacle = (5.8, 0), with (x, y) being the coordinates in meters for each. When it discovers the obstacles, the P3DX adjusts its route. Real-time range data from the sensor are displayed as range intensity frames in Figures 15a–d, as well as 3D point clouds in Figures 15e,f. Tripod and moving obstacle are detected while navigating to its destination and P3DX completes this task (Figure 15g) without running into any obstacles.

Figure 14:

Figure 15:

Finally, Experiment III is carried out in a situation with a moving obstacle. With its X and Y coordinates in meters, P3DX is programmed to move from an initial position of (0, 0) to a goal position of (9.0 m, 0). The dynamic obstacle is estimated by the scene flow technique to move at a speed of 694 mm/s and be slightly oriented to the right of P3DX. The path planning algorithm alters P3DX's path to the left to avoid the moving obstacles as it anticipates the possibility of a collision at the coordinates (4.5 m, 0). Figure 16 shows the processed sensor data rendered.

Figure 16: