Chapter 4 Navigation Strategy in Outdoor Environments
4.3 Detailed Algorithm of Navigation Process
In this section, we introduce the detailed algorithm proposed in this study for vehicle navigation in the navigation process. With the learned information, the vehicle navigates along the learned path by the way of visiting each recorded node consecutively and finishes works at specific positions until reaching the destination point. The flowchart of the whole navigation process is shown in Figure 4.8. The whole navigation process is described in the following algorithm.
V1(PX1, PY1, PTh1) V2(PX2, PY2, PTh2)
VX
VY
VY
VX
GCS
Vehicle position of orientation reading
calibration Vehicle position of position
readings calibration Landmark
Figure 4.6 Process of odometer calibration position is far off the curb line. The vehicle detects the curb line at V1 to calibrate the orientation and then navigates to V2 to calibrate the position using landmark.
Algorithm 4.3 Navigation Process.
Input: a learned navigation path Npath with relevant guidance parameters, and learned Fixed Obstacle Nodeavoid3.for obstacle avoidance in the original navigation path.
Output: none.
Steps.
Step 1. Read from Npath a navigation node Nnext and relevant guidance parameters.
Step 2. Rotate the vehicle toward the next node Nnext..
Step 3. If a curb line following mode is adopted, modify the vehicle direction after localizing the curb landmark using the dynamic threshold adjustment technique as described in Section 6.1.1 [23].
Step 4. Check if the next node Nnext is reached by the mentioned two principles in Section 4.2.1: if not, go to Step 3; otherwise, continue.
Step 5. If a fixed obstacle node is read from Npath, insert obstacle avoidance nodes into the navigation path and go to Step 8.
Step 6. If a landmark node is read from Npath, take the following steps and then go to Step 8.
(1) Adjust the exposure value to the desired illumination in the relevant environment windows in the image.
(2) Detect the landmark and calculate the landmark position as described in Chapters 5 and 6.
(3) Utilize the position of the landmark to localize the vehicle position and modify the odometer reading as described in Section 4.2.
Step 7. If a curb line calibration node is read from Npath, modify the orientation reading of the odometer after detecting and localizing a curb line segment, as described in Section 4.2.
Step 8. Repeat Steps 1 through 7 until there exists no remaining node in Npath.
Read a new node
Figure 4.8 Flowchart of detailed proposed navigation process.
Chapter 5
Natural Landmark Detection in Images Using New Space Line Detection Technique
5.1 Idea of Proposed Space Line Detection Technique
In this study, it is desired to develop a space line detection method to localize landmarks on the navigation path for vehicle navigation. However, compared to the result of using the traditional projective camera, the projection of a space line on an omni-image using an omni-camera is not a line shape but a conic-section curve [25].
Wu and Tsai [25] proposed a line detection method which detects lines in an H-shaped landmark for use in automatic helicopter landing, as illustrated in Figure 5.1. Firstly, they proved that the projection of a space line in the omni-image is a conic-section curve. Then, by using a 2D Hough transform, they extracted the conic section curve in the omni-image and localized the boundary lines of the H shape for conducting the helicopter landing.
However, Wu and Tsai’s method is based on the fact that the parameters of the hyperboloidal mirror are known. It is known that the parameters of a hyperboloidal mirror cannot be calibrated easily. As mentioned in Chapter 3, it is more convenient for us to utilize the pano-mapping table to calibrate the camera. Thus, we propose new space line detection techniques in this study by the use of the pano-mapping table. We utilize the space plane which goes through the space line and the center of the mirror, instead of trying to obtain directly the conic section curve in the
omni-image. By the use of the two-mirror camera system, the proposed line detection technique is introduced in Section 5.2.1. Besides, we derive a method to obtain 3D information of three vertical lines based on the method for line detection proposed in this study. The derivation of 3D information is introduced in Section 5.2.2.
Finally, by the use of the proposed space line detection technique, many types of landmarks can be detected and utilized for vehicle navigation. We introduce the proposed tree trunk detection method in Section 5.4 and the proposed lawn corner detection technique in Section 5.5. Artificial landmark localization also utilizes these methods, which will be introduced in Chapter 6.
5.2 Proposed Technique for Space Line Detection
omni - camera
(a) (b)
Figure 5.1 Wu and Tsai [25] proposed a line detection method for the omni-image to conduct automatic helicopter landing. (a) Illustration of automatic helicopter landing on a helipad with a circled H shape. (b) An omni-image of a simulated helipad.
5.2.1 Line detection using pano-mapping table
If we want to detect a space line which is projected onto the omni-image, it costs much time to calibrate the camera parameters. In this section, a space line detection technique [23] for use on omni-images which are taken by the two-mirror omni-camera used in this study is proposed. Instead of directly obtaining the conic section curve, we detect the space plane which goes through a specified line and the mirror center. In the following, we describe the proposed detection process.
Assume that a pano-mapping table has been established in advance. Also, assume that the space line L to be detected is projected by Mirror B onto the omni-image, and that G is any space point on L. At first, we propose a way to represent a vector which goes through G and the center of mirror OB used in this study.
A light ray going through the space point G is projected by Mirror B onto an image point I as shown in Figure 5.2. The mirror center OB and G together form a vector VG, denoted as (Gx, Gy, Gz) in the CCSlocal. This vector VG can be described using the azimuth and elevation angles and α by the following equations:
cos cos
G 'x ; G 'y cossin; G 'z sin. (5.1) Also, as mentioned previously, to increase the front FOV of the camera, we make the camera system slant up for a specific angle, denoted as . By the use of the rotation matrix introduced in Equation (2.5), the transformation function between the coordinates (X′, Y′, Z′) of the original CCSlocal and the coordinates (X, Y, Z) of the rotated CCS can be described as follows:
1 0 0
According to the coordinate transformation described by Equation (5.2), we can convert the vector VG into a new one VG, which represents the vector with an azimuth angle and an elevation angle α going through the mirror center in the rotated CCS and may be described by the following equations:
cos cos projecting the space line L onto the omni-image. Also, let Q be the space plane which goes through L and the mirror center OB. Assume that the coordinates (X, Y, Z) of a point is on the space plane Q, and let NQ = (l, m, n) be the normal vector of the space plane Q. Then, the space plane Q may be described by:
0
lXmYnZ . (5.4)
Besides, because the normal vector NQ and the vector VG are perpendicular to each other, the coefficients in (5.4) actually are related to the elements of the vector VG of Q by the equality NQVG = (l, m, n)(Gx, Gy, Gz) = 0 which we describe in the
Figure 5.2 A space point with an elevation angle α and an azimuthangle .
omni-image
Figure 5.3 A space line L projected on IL in an omni-image.
Then, substituting Equation (5.3) into Equation (5.5), we get
(cos cos ) (cos sin cos sin sin ) reduction, we assume that n is not equal to zero. Then, we may divide Equation (5.7) by n to get an alternative form as follows:
(cos sin cos sin sin ) ( cos sin sin cos )
(5.8)
We may reduce Equation (5.8) further to get the following result:
(cos sin cos sin sin ) ( cos sin sin cos )
Algorithm 5.1 Space line detection.
Input: an input edge-point image Iedge which includes the conic section projection L’
in an image Iedge of a space line L, and the pano-mapping table for Mirror B.
Output: the values of the two parameters Amax and Bmax, representing a normal vector of the space plane described by Equation (5.10).
Steps.
Step 2. For an edge point I at coordinates (u, v) in Iedge, and look up the pano-mapping table to obtain a corresponding azimuth-elevation angle pair ( α).
Step 3. Compute the parameter values A and B by Equation (5.9) using and α; and if A, B, , and α satisfy Equation (5.9), then increment the count in the cell (A, B) of the Hough space S by one.
Step 4. Repeat Steps 2 and 3 until all the edge points in Iedge are computed.
Step 5. Take the cell (Amax, Bmax) with a maximum count in S as output.
We can obtain the normal vector (l, m, n) from the above-presented algorithm;
however, it costs much time to calculate the 2D Hough transform. Chou and Tsai [23]
proposed a method which detects a vertical line by using a 1D Hough transform and the normal vector of the space plane. The vertical line, called LY line hereafter, is parallel to the Y-axis line in the GCS. In this study, we propose a method for detecting two specific lines. One line of the two is parallel to the X-axis, called the LX line. The other is parallel to the Z-axis, called the LZ line. These three specific lines are illustrated in Figure 5.4. We will describe them, respectively
At first, we review the method for LY line detection. Note that the direction vector of LY is DY = (dYx, dYy, dYz) = (0, 1, 0). Therefore, Equation (5.5) leads to 0×l + 1
×m+ 0×n = 0. Then, it is easy to figure out that m is equal to zero. Thus, Equation (5.7) can be reduced to be the following equation:
( cos sin sin cos ) By Equation (5.10), we may reduce Equation (5.11) to be
1 0 space plane through the LY line and the mirror center.
Then, it can be that the direction vector of the LX is DX = (dXx, dXy, dXz) = (1, 0, 0).
Therefore, Equation (5.5) leads to 1×l + 0×m+ 0×n = 0. Then, it is easy to figure out that l is equal to zero. Thus, Equation (5.7) can be reduced to the following equation:
(cos sin cos sin sin ) ( cos sin sin cos ) By Equation (5.10), we can reduce Equation (5.14) to be
0 1 0,
So we can also use Algorithm 5.1 to find the parameter A, which represents a normal vector of the specific plane through the LX line and the mirror center.
The last one is about LZ line detection in the space. The direction vector of the
By Equation (5.10), we can reduce Equation (5.17) to be
0 0
Figure 5.4 Three specific space lines.
parameters K. The detailed algorithm is introduced in the following.
Algorithm 5.2 LZ space line detection.
Input: an input edge-point image Iedge which includes the conic section projection L’
in an image Iedge of a LZ space line L, and the pano-mapping table for Mirror B.
Output: the values of the parameter Kmax representing a normal vector of the space plane described by Equation (5.16).
Steps.
Step 1. Set up a 1D Hough space S with the parameter K and set all cell values to be zero.
Step 2. For an edge point I at coordinates (u, v) in Iedge, look up the pano-mapping table and obtain a corresponding azimuth-elevation angle pair ( α).
Step 3. Compute the parameter K by Equation (5.19) using and α, and if K, , and α satisfy Equation (5.19), then increment the count in the cell K of the Hough space S by one.
Step 4. Repeat Steps 2 and 3 until all the edge points in Iedge are processed.
Step 5. Take the cell Kmax with a maximum count in S as output.
5.2.2 3D data computation using three space lines
In this section, based on the proposed space line detection technique described in the previous section, we can derive the 3D data from three vertical space lines from the omni-image, as described subsequently.
regions of Mirrors B and S, respectively. The center OB of Mirror B is located at coordinates (0, 0, 0) in the CCS as we previously assumed. We can calculate the position of the center OS of Mirror S by the slant angleand the length of baseline which is denoted as b to be (0, bsin, bcos) in the CCS coordinates system.
According to Equation (5.4), two space planes Q1 and Q2 going through LY and the center of mirror, OB and OS, respectively, can be described by the following equations:
lBX + mBY + nBZ = 0; (5.20) equations can be reduced into the following forms:
lBX + nBZ = 0; (5.22)
Figure 5.5 A space line projected onto IL1 and IL2 on two mirrors in the used two-mirror omni-camera.
B1X + Z = 0; (5.24)
B2X+ (Z － bcos) = 0, (5.25)
where B1 = lB/nB and B2 = lS/nS.
Solving Equations (5.24) and (5.25), we can obtain the following desired solution for X and Z: parallelism between the two space planes Q1 and Q2.
(B) 3D data computation using a LX line. equations can be reduced into the following forms:
mB1Y + nB1Z = 0; (5.29)
mS1(Y － bsin + nS1(Z － bcos) = 0, (5.30)
A1Y + Z = 0; (5.31) A2 ( Y － bsin + (Z － bcos) = 0, (5.32) where A1 = mB1/nB1 and A2 = mS1/nS1.
Solving Equations (5.31) and (5.32), we can obtain the following desired solution for Y and Z: parallelism between the two space planes Q3 and Q4.
(C) 3D data computation using a LZ line.
Figure 5.6 A space line projected onto IL3 and IL4 on two mirrors in the used two-mirror omni-camera.
that of Q6.
The direction vector of the LZ line is DZ = (dZx, dZy, dZz) = (0, 0, 1). Thus, we know that nB2 and nS2 are both zero. Thus, the above two space plane equations can be reduced into the following forms: obtain the following desired solution for X and Y:
2 1 2
Figure 5.7 A space line projected onto IL5 and IL6 on two mirrors in the used two-mirror omni-camera.
5.3 Proposed Method for Tree Trunk Detection
In this section, we introduce the proposed method to localize a tree trunk. At first, we introduce the used method to describe a tree trunk contour and the learning of the tree trunk contour in Section 5.3.1. Next, we extract a feature of the tree trunk by moment-preserving thresholding [26] in Section 5.3.2. Then, estimating the position of tree trunk is described in Section 5.3.3. Finally, some experimental results for tree trunk detection by the proposed method are given in Section 5.3.4. The process of the tree trunk localization is illustrated in Figure 5.8.
5.3.1 Tree trunk contour description
After conducting tree trunk segmentation in the omni-image, we want to ensure that the result of segmentation is correct. In this study, we use the center of a group of feature points and a simple description with two specific parameters obtained by principal component analysis (PCA) to ensure that the object which we want is existing. The method proposed is based on Chou and Tsai [23]. When it conducts the segmentation results, it utilizes the feature point to compute the covariance matrix Cx
in the image. After obtaining the two eigenvalues and the two corresponding eigenvectors of the matrix Cx, we calculate the center of the feature points of the tree
Figure 5.8 Proposed method for tree trunk localization.
between the ICS and the principal component, respectively. Then, we utilize and to describe the tree trunk contour as shown in Figure 5.9. The detailed algorithm is described as the follows.
Algorithm 5.3 Tree trunk contour parameter computation.
Input: an input bi-level image Iinput which includes the feature points of a tree trunk appearing in an omni-image.
Output: Three tree trunk contour parameters, the center mx of all the feature points using their coordinates, a rotational angle , and a length ratio .
Steps.
Step 1. Scan each feature point p with coordinates (u, v) in Iinput, compute the center mx = (ux, vx) of all the feature points using their coordinates, and calculate the covariance matrix Cx of these feature points using their coordinates and mx.
Step 2. Compute the eigenvectors e1 = (u1, v1) and e2 = (u2, v2) and the two corresponding eigenvalues1 and 2 of matrix Cx, where e1 represents the first principal component and e2 the second.
Step 3. By the two eigenvectors e1 and e2, and the two eigenvalues 1 and ,
e1(u1, v1)
Additionally, because the vehicle cannot arrive at the same position to detect the tree trunk, the projections of the tree trunk on omni-images cause a little difference of their positions. To solve this problem, we guide the vehicle to take many of omni-images from different positions and directions in the learning process. After conducting the process of extracting the tree trunk feature points, we use the images to compute three parameters i, i, and mx, the center of the entire group of feature points. Collecting all of parameters, we set the tree trunk parameters in a range from a minimum angle min to a maximum angle max, and set a range from a minimum ratio
min to a maximum ratio max. Also, we calculate the average of the center of tree trunk, and then set a threshold value Vheight in the ICS to check that the height of the center of tree trunk is true. Finally, we record the five parameters Vheight, min, max,
min, and max as the tree contour thresholds. In the navigation process, if the computed the height of the center of the tree trunk, the rotational angle , and the ratio in tree truck detection are not in the learned ranges, we decide that the result of detection is not a pre-selected tree trunk.
5.3.2 Moment-preserving thresholding for tree trunk segmentation
In outdoor environments, it is difficult to detect the natural landmark like trees because the natural landmark does not have obvious color information. So we should find a strategy to detect it. In this section, we utilize the moment-preserving thresholding technique to extract feature points of the tree trunk.
In outdoor environments, varying lighting conditions will influence the image intensity. The moment-preserving thresholding approach to automatic threshold selection for segmenting desired object out of a given image is adopted in this study.
We segment the tree trunk from an omni-image by thresholding the difference image f into a bi-level image by the use of a threshold value Th. The details are described in the following.
Given an environment window in the image f with n pixels whose gray value at a pixel with coordinates (x, y) is donated by f(x, y), the i-th moment mi is defined as
The moments can be computed by the use of the gray-level histogram in the following way, where nj is the total number of pixels in f with gray value zj and pj = nj/n:
Assume that the image resulting from thresholding only contains two gray values z0 and z1, with z1 being larger than z0. The method is to choose a threshold value to judge the pixel value in the gray-level in the environment window to be zo or z1. To find the desired Th, we can solve two equations described by Equations (5.41) and
2 1 0 2
To obtain the threshold value Th, we need to accumulate the probability values from the smallest gray value until the accumulated value reaches p0, as described by the
Finally, conducting the thresholding work, all pixels in the environment window in the image f are scanned and their values are compared to the threshold value Th. If a pixel value in f is larger than Th, the corresponding pixel in the bi-level image b is labeled by “0”; else, it is labeled by “1.” We regard the region labeled by “0” as a tree trunk region.
5.3.3 Tree trunk localization
By using the tree trunk contour extraction process described above, we want to find the vertical axis line of the tree trunk to localize the tree trunk. We assume that the desired vertical line goes through both centers of the tree trunk appearing in the regions of Mirrors S and B in the omni-image. After extracting the two center positions of the tree trunk in the regions of Mirrors S and B, we can obtain further the two space planes which go through the axis line and the two mirror centers, respectively, by the use of the proposed vertical line detection method [23]. Finally,
we can obtain the tree trunk position by the located axis line using the information of the two space planes. The detailed process is described as follows.
Algorithm 5.4 Tree trunk location computation.
Input: an input bi-level image Ibi which includes tree trunk feature points, and an environment window Wintt.
Output: a tree trunk position Gtt in the CCS.
Steps.
Step 1. Compute the center CB with coordinates (uB, uB) of the tree trunk feature points in winB of Wintt and the center CS with coordinates (uS, uS) of the tree trunk feature points in winS of Wintt.
Step 2. Look up the pano-mapping table to obtain the corresponding elevation angle αB and azimuth angle of CB and the corresponding elevation angle αS and azimuth angle S of CS.
Step 3. By Equation (5.13), compute the parameter value BB corresponding to CB
Step 3. By Equation (5.13), compute the parameter value BB corresponding to CB