Obstacle Classification and 3D Measurement in Unstructured Environments Based on ToF Cameras

2.1.1. Laser Sensors

Sensors using lasers to acquire 3D data can be classified into actuated laser ranger finders and 3D laser ranger finders [1]. Typically, an actuated ranger finder is a 2D sensor attached to a platform rotated by a servomotor [2–5]. Some groups use two 2D laser rangefinders to enhance performance [6,7]. In order to receive consistent 3D data with such sensors, a stop-and-go mode for traveling is necessary, and multiple consequent scans are taken and merged. The precision of 3D data points depends on the resolution of servomotor and on the precision of the scanner. The main disadvantage of actuated 2D laser ranger finders is that they are slow and the scan is time-consuming.

The 3D laser range finders can provide high-resolution data at a very high sampling rate. Examples of this type of range finders include RIEGL, CYRAX, zoller + fröhlich scanner, Velodyne 3D-LRF, and space ground LiDAR, which all generate consistent 3D data points within a single 3D scan. The major benefits of these sensors are high resolution and large field of view. Unfortunately, these sensors are usually heavy in weight, with high power consumption and high cost. In contrast to expensive 3D laser sensors, the FX 3D LIDAR sensor is relatively lightweight and has no rotational mechanical parts, but its resolution is low.

2.1.2. Stereo Vision Systems

Stereo vision systems acquire 3D data with two or more cameras by passive sensing. Some commercial stereo vision systems are available, such as those manufactured by PointGray and VidereDesign. Comparing to laser ranger sensors, the range data resolution of a stereo camera is usually lower. However, it can provide high-resolution images, texture and color information, not just a 3D geometry. Now, stereo cameras have been widely applied to map-building [8–12], natural environment navigation, and rescue missions [13]. Nevertheless, stereo vision has limitations, especially in environments with few features and limited visibility due to undesirable light conditions. Furthermore, stereo vision is effective only in the near field as its range precision and maximum depth is limited because its range resolution usually decreases rapidly as the depth increases.

2.1.3. 3D Range Camera

3D range cameras have been developed for 3D data gathering providing both an intensity image and a range image. A TOF camera has an active sensor that determines both the range and intensity information at each pixel by measuring the time taken by infrared light beam traveling to the object and reflected back to the camera [14,15]. Now several manufactures have begun marketing of these cameras, such as MESA Imaging, PMD Technologies, and 3DV Systems. Microsoft Kinect is another kind of popular range camera based on the light coding technology. Its ability to fast and reliable access to 3D data at high frame rates has enormous potential applications in mobile robotic mapping [16–20], navigation [21,22] and localization [23], industrial robot collision avoidance [24,25], human tracking [26], special measurement [27].

In general, the distance accuracy and image quality of TOF cameras is still relatively low compared to that of other 3D sensors. External as well as internal factors which affect the camera performance are integration time, target reflectivity, object distance, movement artifacts, environmental conditions (temperature, light, multiple reflections, non-ambiguity range) and so on [28–30]. Signal noise reduction is usually conducted by either averaging information over time or by using spatial smoothing filters [30,31]. Some calibration methods have been presented for range error rectification [31–33] caused by object reflectivity, distance and etc. Low integration time is often used for reducing the blurring effect caused by motion, but it is necessary to compensate for the low SNR of a single image. One possible approach is to fuse data from multiple frames recorded at a higher frame rate to improve SNR of the averaged signal [28]. Lindner et al. introduced a solution for motion artifacts of ToF cameras based on optic flow techniques [34].

Meanwhile, researchers have proposed lots of methods to enhance the resolution in either the 3D depth image or the 2D image. Some of the methods are based on the combination of higher resolution image and depth image acquired by a 2D camera and a ToF camera [35–37], while other methods are based on the fusion of a stereo camera and a ToF camera [38,39]. Yuan et al. [40] proposed a laser-based navigation system enhanced with 3d time-of-flight data. With the development of 3D range cameras, this has a great future in an enormous range of applications.

3.2.1. 3D Information Acquiring and Preprocessing

(1)

Camera parameters setting and 3D Information acquiring

Since the integration time of ToF cameras has a direct effect on signal-to-noise (SNR) of measurement, accuracy of measurement, movement artifacts, and so on, it should be selected based on the required acquisition speed, the measurement quality and measured objects. In our case, we select integration time based on the following facts:

First, as we focus on obstacle detection and classification, the environment is simplified in our study. Only static obstacles in the scene are processed. Furthermore, the robot operates in stop-go mode when capturing a scene by ToF camera. Therefore, movement artifact in our measurement can be ignored.

Since ToF camera measurement is based on the received modulation signal, the accuracy of distance measurement of a ToF camera depends heavily on the signal-to-noise of the received signal. Longer integration time allows one to capture a larger amount of reflected light, which results in lower noise. In addition, the requirements for integration time are different for different objects. To reach the same measurement quality, a longer integration time needs to be set for low-reflective and further objects than for high reflective and closer objects. Consequently, long integration time is necessary to ensure the accuracy of measurement with less noise. However, longer integration time also induce higher temperature of the sensor. Furthermore, very long integration time leads to saturation.

Based on the above mentioned facts and the existing analysis [28–32], different integration times were tested and compared in our test environments, and 80 ms was finally selected for SR3000 in our case to get measurement data with constant accuracy and less noise.

Furthermore, to reduce the amount of heat generated, the camera is operated in triggered mode rather than continuous mode. The focus of SR3000 camera is fixed during the whole navigation process. In our situations, as shown in Section 4, interesting obstacles are usually in the range of 2 m–5 m. As a result, we set the focus in the distance of 3.5 m. Additionally, in order to improve the distance accuracy, we calibrate the measurement data according to the neural network based approach described in reference [33].

(2)

Noise reduction based on spatial smoothing

Besides the integration time, noise in images obtained by the SR3000 camera is reduced using a spatial smoothing filter. Compared with median filter and Gaussian filter, the bilateral filter [63,64] takes into account the difference in value within the neighbors to preserve edges while smoothing. Therefore, it can filter out random noises of distance information inside the object region while keeping edges of the object. Consequently, the bilateral filter is introduced for distance measurement denoising before obstacle detection and measurement.

(3)

“Back folding” measurement filtering

Measurements by the SR3000 are also subject to a so-called “ambiguity” or a “back-folding” phenomenon. When the distance of object is further than the ambiguity range (here 7.5 m), the measurement is usually a random value in range of 0∼4 m with a low amplitude instead of a true value. The possible solution is to filter those values out by setting an amplitude threshold as objects situated outside the non-ambiguity range. However the amplitude threshold is difficult to define due to the difference of scene and object characteristics.

In the robot developed by us the ToF camera is mounted horizontally. Consequently, along the vertical direction in ToF images, pixels with a longer distance usually appear in the upper area compared with pixels with a shorter distance in an open or obstacle free environment. If pixels do not hold this feature, they should belong to a valid obstacle or a “back-folding” region. In addition, valid object region should satisfy the principle of image spatial continuity and deep information continuity, and its range or image information should be continuous instead of random. Based on this knowledge and the characteristics of amplitude, we have presented a method to remove those noisy measurement caused by “back folding” phenomenon [65]. As shown in Figures 4 and 5, regions with ambiguity measurements are filtered out using our proposed method, which effectiveness has been verified.

3.2.2. Irrelevant Region Detection

(1)

Far background detection

According to the above definition, a pixel (i,j) on intensity image GI falls within a far-background region if its distance value Z(i,j) is more than a distance threshold D_FB. As a result, the point is removed from the intensity image by setting its intensity value to zero:

$$\begin{array}{cccc}\text{If}& Z(i,j)>D_{FB}& \text{then}& GI(i,j)=0\end{array}$$

(1)

(2)

Ground plane detection

In order to calculate the accurate ground plane, a simple and fast histogram based method is presented. The TOF camera is mounted horizontally on the robot. As a result, the ground plane means the points on the plane have a constant height.

As illustrated in Figure 6a, generally the area adjacent to the robot in the forwarding direction is the safest area for safe navigation, and the lower center region in the intensity image will also likely be a safe region for the robot. Consequently, as shown Figure 6b, we select a sample region (SR) in the intensity image to calculate the parameters of ground plane because ground plane will appear in this area with the highest probability.

For a given pixel (i,j) on the intensity image of the SR-3000 camera, we can draw its corresponding 3D information provided by P(x,y,z). Therefore, we express the height of any point in the SR region as Y(m,n). However, the ground is not perfectly planar in most terrain environments. Small variations in height on uneven terrains are commonly encountered. Consequently, we assume that the height of points on the ground plane has a Gaussian distribution $N(H_g,{\sigma }_H^2)$, where H_g is the average ground plane height, and σ is the standard deviation of the ground height. The parameters of ground plane are calculated as follows:

(a)

As the camera is mounted horizontally on the robot with height H₀ (the distance between the center of camera to the ground), we defined the heights of points in the SR region in the range of [H₀ −10, H₀+10] (in cm), for all possible points in a ground plane set, denoted by PG.

(b)

For a point in PG, we calculate its height distribution histogram. The horizontal axis X in this histogram stands for the height with step of 2 cm, and the vertical axis Y stands for the number of points drops in the height range [H(i),H(i+1)] represented as PNum(H(i)).

(c)

We select the top three scores in the histogram as PNum(H(i)), PNum(H(j)) and PNum(H(k)), calculate the sum of points in one neighborhood and select the maximal one. Then, the more accurate height range of ground plane is defined as follows:

$$\begin{array}{ll}if\hfill & \text{∑}_{w=-1}^1\mathit{\text{Pnum}}\left(H(j+w)\right)=\mathit{\text{Max}}\left(\text{∑}_{m=-1}^1\mathit{\text{Pnum}}\left(H(i+w)\right),\text{∑}_{m=-1}^1\mathit{\text{Pnum}}\left(H(j+w)\right),\text{∑}_{m=-1}^1\mathit{\text{Pnum}}\left(H(k+w)\right)\right)\hfill \\ \mathit{\text{then}}\hfill & \mathit{\text{GRange}}=[H_0-10+2\ast (j-1),H_0-10+2\ast (j+1)]\hfill \end{array}$$

(2)

(d)

The parameters of ground plane are calculated as follows:

$$H_g=\frac{\text{∑}_{i=1}^M\text{∑}_{j=1}^{N}Y(m,n)}{K},\forall Y(m,n)\in \mathit{\text{GRange}}$$

(3)

$$\sigma =\frac{\text{∑}_{m=1}^M\text{∑}_{n=1}^N{\left(Y(m,n)-H_g\right)}^2}{K},\forall Y(m,n)\in \mathit{\text{GRange}}$$

(4)

where K is the number of points with height in the range of GRange.

(e)

As the parameters of ground plane are calculated, the points on the ground plane are defined. Pixel (i,j) of intensity image GI with height value H(i,j) falls in the range of [H_g−2σ,H_g+2σ] will be considered as a ground plane point. In order to remove the ground points from the intensity image for further obstacle detection, the corresponding intensity value of ground plane point is set to 0, as illustrated in Equation (5):

$$\begin{array}{cccccc}If& Y(i,j)\in [H_g-2\sigma ,H_g+2\sigma ]& \mathit{\text{then}}& (i,j)\in \mathit{\text{GPange}}e& \mathit{\text{and}}& GI(i,j)=0\end{array}$$

(5)

For different complex environments, the ground plane can be detected with high robustness and accuracy, and most points on the ground plane are removed from the scene. However, there are still some undetected ground points with considerable height variance in the image, which need to be removed by further processing (to be described in Section 3.2.3).

(3)

Sky detection

The sky region is defined as the region where which height is far beyond what the robot can reach. As the ground plane is detected, we define the sky region according to the height difference with ground plane height. A pixel (i,j) in the intensity image GI with height Y (i,j) elongs to the sky region if its height is higher than the ground plane with threshold δ_S, and then the corresponding intensity value of the pixel is set to zero:

$$\begin{array}{cccc}\text{If}& Y(i,j)-H_g>{\delta }_S& \text{then}& GI(i,j)=0\end{array}$$

(6)

3.2.3. Obstacle Detection Based on Intensity Image and 3D Information

(1)

Segmentation of regions of interest

According to the definition in Table 1, the remaining pixels in the intensity image GI after removing irrelevant regions are regions of interest. This fact immediately suggests the use of binary images to represent these regions. A binary image has an intensity value 0 for the irrelevant regions and 1 otherwise:

$$\begin{array}{cccc}\text{If}& GI(i\cdot j)>0& \text{then}& GI(i\cdot j)=1\end{array}$$

(7)

In practice, the detected ground plane often contains unwanted “noisy parts” because of the coarseness and surface irregularity of the ground. A morphological filtering method is introduced to suppress noises. As given in Equations (8) and (9), erosion followed by dilation with the same structuring element B is used to remove isolated small objects or noises from image GI:

$$GI\circ B=\left(GI\text{Θ}B\right)\oplus B$$

(8)

$$GI•B=\left(GI\oplus B\right)\text{Θ}B$$

(9)

(2)

Region of interest clustering and obstacle detection

After extracting obstacles, some parts of the obstacles are often overlapped with each other in the intensity image. Naturally, obstacles should be individual objects (such as rocks, slope, and so on). For any individual obstacle, we know that adjacent points in the intensity image must also be adjacent in spatial relation. Consequently, detected interested points can be clustered into separate obstacles on the basis of their geometric information (i.e., depth and horizontal information) and connectivity.

Comparing adjacent points located in the obstacle region, if the distance between two points stays within a certain range, we assign them to the same category. Otherwise, we assign them to different categories. Then, a current point is regarded as starting point of new category and compared with the rest points. In this paper, the Manhattan Distance is selected to calculate distance between two points as follows:

$$D_{i,j}=c_W\left|X(i_1,j_1)-X(i_2,j_2)\right|+c_H\left|Y(i_1,j_1)-Y(i_2,j_2)\right|+c_D\left|Z(i_1,j_1)-Z(i_2,j_2)\right|$$

(10)

Where c_W, c_H and c_D represent contributions to clustering, which is determined by spatial changes in horizontal, vertical and depth directions. For the obstacles, change of distance in the depth direction is usually more obvious, thus we have c_W < c_H < c_D.

3.3.1. Obstacles to be Recognized

In Section 3.2, obstacles are clustered and detected. To realize navigation in a natural environment, it is important to recognize obstacles and plan different efficient navigation strategies accordingly. It is assumed that the robot can climb step-like obstacles with a height threshold H_rc and slope obstacles with a slope angle threshold θ_rc. However, the robot can walk across obstacles higher than H_rc if the obstacle region has a certain slope angle. In this paper obstacles are expected to be fall into four categories for navigation purposes, based on the movement performance of the robot.

(1)

Rock: Small obstacles (rocks or steps) can be traversed by robot without avoiding them according to movement performance of robot. In addition to help distinguishing them from other obstacles, the maximum height of a rock obstacle is defined as H_r which is usually higher than H_rc.

(2)

Stone: Compared with rocks medium obstacles such as stones, pillars and trees must be avoided by robots. The Width and D_depth of stone obstacles should be smaller than the width threshold of slope (W_sl) while its height is above the threshold H_rc.

(3)

Slope: huge obstacles, such as a slope, wall or a megalith (certainly, whether a slop can be climbed is determined by its slope angle and mechanical properties of the robot). Its height is higher than H_r while its minimum width is defined as W_sl which is usually wider than the robot's width.

(4)

Ditch: obstacles below the ground plane, such as a ditch or a small water path.

3.3.2. Features Used for Obstacle Recognition

Each obstacle is characterized by its geometry features. However, natural obstacles may have an irregular shape. Thus, a feature vector {Width,Height,R_LH,D_depthDepth,Area} is selected to describe width, height, ratio between length and height, the maximal depth difference, and the average depth and area of the obstacle, as illustrated in Figure 7 and defined as follows.

Width: The maximum horizontal width between the rightmost and the leftmost points in each row within the region:

Height: The maximum height difference between the ground and each point within the obstacle is the region's height:

Ratio between Width and Height:

D_depth: The maximum difference of depth value of obstacle or object region, which stands for the obstacle's distance range in the direction of depth. As for ditch obstacle, D_depth is defined as the range difference between two regions where ground and ditch meeting respectively:

Depth: the distance between robot and obstacle. The average value of distances of all pixels in the object region stands for the object Depth. In addition, the Depth definition is a little different for a slope obstacle compared with other obstacles. Depth of slope is defined as the average distance of the region where there is a slope and ground effect since slope regions usually have a big range of distance distributions:

Area: It is clear that the same object would have different amounts of pixels in its image if the object is at different distances to the camera. The relationship between the object area and its depth is generally linear. Consequently, we define the area as a feature related to the true area instead of the ground truth area:

Similarly, other features about obstacles can also be calculated to describe its location and spatial relationships:

Distance between obstacles: For neighboring obstacles, D_obs is defined as the distance between the most right point (i_R,j_R) of the left obstacle and the most left point (i_L,j_L) of the right obstacle without consideration of the height difference:

Obstacle center: Obstacle's location is defined as its gravity center (i_c,j_c) in the intensity image:

3.3.3. Obstacle Recognition Based on Multi-RVM

According to the definition in Section 3.3.1, possible obstacles are classified into four classes. As obstacles like ditches are under the ground plane, this kind of obstacles can be distinguished easily from other three classes of obstacles based on the height difference with the ground plane. As obstacles in the terrain or field environment vary greatly in shape, it is difficult to acquire enough samples for all possible obstacles. Essentially, obstacles recognition subjects to the problem of small sample and nonlinear pattern recognition.

RVM is a machine learning technique that uses Bayesian inference to obtain solutions for classification. This method depends only on a subset of the training data and is able to solve nonlinear pattern recognition problems. RVM has an identical functional form to the support vector machine, but provides probabilistic classification. Compared to SVM, the Bayesian formulation of the RVM avoids the set of free parameters of the SVM (that usually require cross-validation-based post-optimizations), and has less relevant vectors than support vectors given the same prediction performance. Based on these advantages, RVM is used as the obstacle classifier in this work.

Since we must solve a multi-class problem, the system needs a multi-RVM classifier. We adopt “one-versus-one” method of multi-RVM. In this method, we will construct all the possible two-class classifiers for three kinds of training samples. Every classifier will be trained on two-class training samples of three classes of obstacles. Therefore, six sub-RVM classifiers for two-class classification are needed. Decision is made by a max-wins voting strategy, in which every classifier assigns the instance to one of two classes. Then, the vote for the assigned class is increased by one vote, and finally the class with the most votes determines the instance classification. A detailed scheme is shown in Figure 8.

4.1.1. Experiment in Scene 1: Stones and Rocks with Occlusion

This environment was a specific scene with a ground with sands, stones and a back wall. The ground was not very flat, and the color difference was not obvious with obstacles. In addition, some obstacles were overlapped in the intensity image.

According to the proposed method, the whole obstacle detecting process is shown in Figure 10. The original intensity image of the scene is shown in Figure 10a. The maximum distance of this scene is smaller than far background threshold (7 m).Therefore no pixels in this scene can be removed in this step of far background elimination. Following this step, the far background region, ground, and sky region respectively were removed, as shown in Figure 10b,c and regions of interest were segmented. The binary image of the region of interest is illustrated in Figure 10d. Consequently, image erosion and dilation were applied to the binary image of the region of interest to filter out small objects or suppress noise. The result is shown in Figure 10e. Finally, based on the 3D geometric information, the detected regions of interest are clustered into separate obstacles as shown in Figure 10f–h. The detected obstacles are shown in Figure 10i.

The result shows that five obstacles were completely detected, even for the very small and overlapped stones. Obstacle feature parameters were also computed in order to test the accuracy of the presented method. The ground truth data were measured manually. The relative error between the measurement value and human labeled data value is shown in Table 2. The length measurement of the 3rd obstacle had a very big relative error with human labeled data, as it was partly occluded by the 2nd obstacle in the horizontal direction.

4.1.2. Experiment in Scene 2: Small Stones and Convex Sand Region

As mentioned in the above section, the ground is often imperfect and not planar in most terrain environments. If the ground height is defined with a narrow range, some ground regions will be detected as obstacles. However, if the ground height is set with a big range, some obstacles will probably not be detected correctly if their height is smaller than the height range. This scene was designed to evaluate the performance of the proposed method in small obstacles detection which height is below 30 cm.

As shown in Figure 11, the presented method gives a satisfactory detection result. Firstly, far background, ground plane and sky region were detected and removed from the scene as shown in Figure 11b. Two small obstacles and two convex regions with little slope angle were detected correctly as shown in Figure 11f. The measurement error was also in acceptable range, as shown in Table 3.

4.1.3. Experiment in Scene 3: Trees and Grass in Natural Environment

This scene is a typical natural outdoor environment. The uneven ground is randomly covered with grass and trees. As shown in Figure 12, firstly the house is separated from the image as its distance is far beyond the Far-Background threshold, and then tree is detected as obstacle after ground detection region clustering.

As shown in Table 4, the height measured by robot looks a bit different from its manual reference value. The reason consisted of two sides, the first is that the sky threshold was just selected as the reference ground truth; the other reason was that the ground is uneven and the detected region became a little shorter after ground deletion and region clustering.

4.1.4. Experiment in Scene 4: Slope

There were three parts in this scene, a wall (Far-background), a ground and a slope made of sands. As the texture and color for ground and slope were same, it was difficult for a robot to distinguish each other depending on 2D images. However, it was very easy to detect slope region based on the proposed method, the detail process is shown in Figure 13. The computed parameters of obstacle and its relative error with human labeled data are shown in Table 5.

4.1.5. Experiment in Scene 5: Ditch in Field Environment

In this scene there was a ditch under the ground. Once again, it was easily detected by the proposed method, the detail steps and results are shown in Figure 14. D_depth in Table 6 is the range difference between two regions where ground and ditch are meeting, respectively, which stands for the ditch's width in the depth's direction. Depth is still defined as the average distance of all points in region. As for the human labeled data, we sampled the distance along the ditch evenly, and made an average as a reference value. This definition is coherent with the definition of other obstacles. In addition, the computed parameters of obstacle and its relative error with human labeled data are shown in Table 6.

4.2.1. Multi-RVM Training

Five hundred scenes with different obstacles were selected as training samples. Based on the proposed obstacle detection method, obstacles and their feature were detected in every scene. As shown in Tables 2, 3, 4, 5 and 6, every sample includes obstacle features and its class type which is denoted by vector {Width, Height, R_LH, D_depth, Area, Class}. The number of extracted training set and testing set from 500 scenes is listed in Table 7. In addition, the performances of the presented method and SVM based method were compared and analyzed.

The RBF kernel function was applied for multi-RVM based obstacle classification, as shown in Equation (19):

4.2.2. Obstacle Classification Results and Analysis

When the RVM classification method was trained, obstacles were classified and recognized by imputing detected obstacle regions. For test samples with γ = 0.1, the classification results are shown in Table 8.

The test set recognition results are shown as in Tables 9 and 10 in which the classifier was trained with different parameters, Table 9 shows the results of the SVM-based classifier with the same training parameters. Table 10 presents the test set recognition result with the RVM-based classifier, where different parameters were used to train the classifier. The experiment results show that the RV number was greatly reduced compared to the SV number. As a result, the processing time of the proposed method was also shorter than that of the SVM-based method. The recognition rate of proposed method was just slightly lower than that of the SVM-based method.

4.2.3. Experiment Result Analysis of Obstacle Detection and Measurement

Experiments have been done to test the accuracy of the proposed method on obstacle classification and geometrical feature measurement. Experimental results in the presented five typical scenes showed that the proposed method can detect obstacles as well as provide valid and dense 3D geometrical measurement with high efficiency and robustness, which is critical for intelligent robot navigation. In addition, experimental results are analyzed in detail as follows:

(1)

This method has a very high robustness in complex and unstructured environments. Firstly, in our tested environments, the obstacle classification accuracy of the proposed method maintains a high level. As shown in Table 10, the recognition accuracy of obstacles reaches 93%. Secondly, the relative error illustrated in Tables 2, 3, 4, 5 and 6 is usually lower than 5% except for some special overlapped or very irregular cases in Tables 2 and 3. On the other hand, since the ground truth is provided by human labeling, it has some inherent measurement error, especially for irregular or overlapped obstacles in complex environments. As a result, the real relative accuracy of the presented method may be higher or lower than the data in above experiments, which reflects a trend of accuracy.

(2)

The accuracy of the proposed method can be improved in the future. As the ToF camera has low resolution either in the 2D intensity image output or 3D depth image, compared with the enhancement on range accuracy [24,28–34], it is also very necessary to improve the accuracy on obstacle segmentation. As for our proposed method, the accuracy of obstacle geometrical parameters, such as Width, Height, D_obs, are greatly affected by the object segmentation accuracy. In the future, we will consider the fusion of ToF camera with a 2D high-resolution camera. This will produce high quality color image as well as corresponding depth image, which will improve the segmentation accuracy of obstacles further.

(3)

The proposed method is highly efficient and easy to implement. It can finish the process of obstacle recognition and 3D measurement within the time of a single frame of the ToF camera data without any prior knowledge. In addition, the method is easy to implement as the whole process consists of only simple operations.