Chapter 4 Human Localization in Indoor Environments by
4.5 Algorithm of Human Localization and Tracking
In this section, we will describe the complete steps for human localization and tracking. Under the assumption that there is only one user in the indoor environment taken care of by the system, we can obtain one foreground region representing a user for each processing cycle. Therefore, the server-side system will send the location and orientation to the user’s client-side system. The following algorithm illustrates the complete steps for this task.
Algorithm 4.9 Algorithm of human localization and tracking.
Input: The foreground region hu of a user in the previous processing cycle, the omni-images I1, I2, …, In captured from cameras C1, C2, …, Cn, respectively, where n is the number of the cameras.
Output: The user’s location p, the user’s orientation d , and the foreground region hu of the user.
Steps
Step 1. Find foreground regions R(, ) in each of I1, I2, …, In by Algorithm 4.1, where R i( , ) means all foreground regions in omni-image Ii, and R i j( , ) means the jth foreground region inR i( , ).
Step 2. Track the foreground regions R(1,), R(2,), …, R(n,) by Algorithm 4.7, resulting in the tracks T(1,), T(2,), …, T(n,), where T i( , ) means all tracks in omni-image Ii, and T i j( , ) means the jth track inT i( , ).
Step 3. If hu is set, then find hu by Algorithm 4.8; else, take the following steps.
3.1. Detect the color edge mark by Algorithm 4.5 for R(1,), R(2,), …, R(n,).
68
3.2. If the color edge mark is detected in any foreground region, apply Algorithm 4.8 to anyone of the foreground regions with color edge mark detected to find the human region hu and go to Step 4.
3.3. If the color edge mark is not detected, apply Algorithm 4.8 to the foreground region with the largest region size in R to find the human region hu and go to Step 4.
Step 4. Compute the location p of hu as the output by Algorithm 4.2.
Step 5. Compute the orientation d of hu as the output by Algorithm 4.6.
In Step 3, if we have found a human region in the previous processing cycle, we can find the same human region by performing the camera-off algorithm. Otherwise, we find the human region by the color edge mark or the region size. Finally, we compute the location and the orientation of the human region as output. Furthermore, the human region will be used in the next processing cycle.
4.6 Experimental Results
In this section, we show some experimental results of both human location detection and orientation detection. Figure 4.12 shows the results of the human location detection at four different locations. It shows that the proposed method can actually find the foot point of a human and transform the point from the ICS to the MCS.
Figure 4.13 shows the results of the human orientation detection by the color edge mark, where the approximating lines are shown as light green color. It also shows that the proposed method is feasible.
69
(a)
(b)
(c)
(d)
Figure 4.12 Human location detection at four different locations.
70
(a)
(b)
(c)
(d)
Figure 4.13 Human orientation detection by color edge mark at four different locations.
71
Chapter 5
Path Planning for Navigation
5.1 Ideas of Proposed Techniques
When a user want to reach a certain destination, the server-side system will find the location of the user at first, and then find the location of the destination in the environment map we constructed in the learning stage. Next, the server-side system will begin to plan a path starting from the user and ending at the destination, and sends the result to the client-side system.
Here we use an obstacle image obtained in the learning stage to determine whether a planned path collides with any obstacle or not. If the path starting from the location of a user and ending at the location of the destination does not collide with any obstacle, the server-side system directly sends the two locations to the client-side system, which means that the user may now walk forward to the desired destination.
However, if the path collides with obstacles, we have to determine the immediate collision points to avoid the obstacles. Here we use an obstacle avoidance map constructed in the learning stage for this purpose. A more detailed description of the obstacle avoidance process will be described in Section 5.2. Next, we follow the avoidance directions to find the immediate points, and finally we will obtain a new path starting from the user’s location to the destination. The path finding scheme will be described in Section 5.3. However, the planned path may not be in the simplest form; in other words, there may exist two non-connected points on the path that can instead be connected together without any obstacle between the two points. Therefore,
72
we propose a scheme to simplify the planned plan, which will be described in Section 5.4.
After a path is completely planned, it will be send to the client-side system. A user can follow the path to reach the desired destination. However, if the user moves to a location which is not on the planned path, the planned path must be updated. A path update scheme is also proposed in this study, which will be described in Section 5.5.
Finally, we will describe a complete path planning process in Section 5.6. Then some experimental results will be presented in the last section.
5.2 Obstacle Avoidance
An obstacle avoidance map is created by splitting the MCS into small blocks. A block is a processing unit in the obstacle avoidance process; in other words, the planned path “walks” a block at a time, if the planned path walks to a block containing obstacles, it will find another block to go. Here we introduce a block coordinate space (BCS), which can be used to locate the position of a block. The BCS coordinates of a block are denoted as (i, j). A block with the BCS coordinates (0, 0) means that the block is at the top-left corner; a block with the BCS coordinates (1, 0) means that it is the one on the right side of the one with coordinates (0, 0), and so on.
The MCS coordinates (Mx, My) of a block with the BCS coordinates (i, j) can be
73
Each element in an obstacle avoidance map represents two opposite avoidance directions, so we only store the one of the two directions in degrees. As shown in Figure 5.1, the region of all angle degrees can be divided to 8 parts all with an equal degree range of 45o. Every region part is assigned an index from 0 to 7, where the degree region of part 0 is from 337.5o to 22.5o; the degree region of part 1 is from 22.5o to 67.5o, and so on. Therefore, we can determine the region part of an avoidance direction by the angle of the direction. The region part of an avoidance direction is called “avoidance region” in the sequel.
337.5∘
Figure 5.1 The whole direction region is divided to 8 parts, and each part is assigned an index.
As the 3 3 blocks shown in Figure 5.2, if a planned path walks to the central block and cannot directly walk to the destination from the block, the proposed system tries to find the next immediate block in the 7 neighborhoods by the avoidance direction of the block. Here, we can apply the avoidance regions to the 3 3 blocks, and then assign each of the 7 neighborhoods an index as shown in Figure 5.2.
74
Therefore, each avoidance direction can map to an avoidance block by the index of the avoidance region. However, we do not find the immediate point just at one avoidance block. More specifically, we will assign three avoidance blocks for each avoidance range. As shown in Figure 5.3, each avoidance range (shown as semi-transparent regions) is assigned three blocks, which include one primary avoidance block (shown as red regions) of the same avoidance range and the two neighborhoods, which are called secondary avoidance blocks (shown as blue regions).
0
2 1 3
4
5 6 7
Figure 5.2 Apply the direction region parts to the neighborhoods of one block, and each neighborhood is assigned an index.
0
75
Figure 5.3 Avoidance blocks of 8 avoidance ranges, where the avoidance regions are shown as semi-transparent regions. Each avoidance region is assigned three blocks, which include the primary avoidance block (shown as red regions) of the same avoidance range and two secondary avoidance blocks (shown as blue regions).
We can find the next immediate point in the three avoidance blocks by the avoidance direction. More specifically, we have two avoidance directions in one block, so we will have six avoidance blocks for one block.
The following algorithm describes the processes to find the avoidance points of a block. It results in a set of avoidance points, which is sorted by the priority of the avoidance points. The priority of an avoidance point is based on the distance from the avoidance point to the destination point. An avoidance point with a higher priority should be considered first as the next immediate point in the path finding process. The path finding process scheme will be described in the next section.
Algorithm 5.1 Finding avoidance points.
Input: The current position p of a planned path in the MCS, the final destination position d of the planned path in the MCS, an obstacle image Io, and an obstacle avoidance map A, where A i j( , ) means the angle of the avoidance direction of the block with BCS coordinates ( , )i j .
Output: A set of avoidance points, which is sorted by the priority of the avoidance block.
Steps
Step 1. Initialize an empty set Sresult for the resulting points.
Step 2. Compute the coordinates (i, j) of p in the BCS by Equation 5.2.
Step 3. If A(i, j) is non-negative, obtain the two avoidance directions d and 1 d 2 from A(i, j); otherwise, take the following steps.
76
3.1. Compute the direction v pd.
3.2. Find three avoidance blocks of the avoidance direction v . 3.3. Go to Step 5.
Step 4. Find six avoidance blocks B of d and 1 d , which include two primary 2 avoidance blocks and four secondary blocks.
Step 5. Initialize two empty ordered sets Sp and Ss.
Step 6. Take the following steps for each avoidance block b in B.
6.1. Compute the MCS coordinates b of b by Equation 5.1.
6.2. If the line segment pb collides with any obstacle, skip to the next avoidance block b and go to Step 6 again.
6.3. If b is of a primary avoidance block, add b into Sp; otherwise, add b into Ss.
Step 7. If Sp is not empty, take the following steps.
7.1. Sort each element b of Sp in the ascending order by the Manhattan distance from b to d:
|bxdx||bydy|.
7.2. Add the first element of Sp into Sresult, and add the remainder into Ss. Step 8. Sort Ss by the same scheme of 7.1.
Step 9. Add each element of Ss into Sresult orderly.
Step 10. Take Sresult as the output.
In Step 3, if the current block contains no avoidance direction, the avoidance direction is set to the one from the current position to the destination. We add each of the avoidance blocks which are reachable from the current block into two sets. The primary avoidance blocks will be added into one set, and the secondary will be added
77
into another. Then we sort the sets based on the distance to the final destination in Step 7 and Step 8. We choose the closest one of the primary avoidance block as the first priority avoidance block, and add it into a resulting set. Finally, the remainders are added into the resulting set orderly.
5.3 Path Finding
We introduce the processes of obstacle avoidance in the previous section. In this section, we describe the path finding scheme by use of avoidance points.
When we want to find a path from the current position to a destination position, we find the avoidance points of the current position by Algorithm 5.1 at first. Then we begin to check each avoidance point. We will record it when we check an avoidance point, and we do not check the same avoidance point twice, which means that a path do not check a point it has walked before. If an avoidance point has not been walked before, we try to find a path from the avoidance point to the destination. Therefore, the process can become recursive. However, if an avoidance point cannot reach the destination, we continue to try the next avoidance point. If there are no more avoidance points, we finish the finding process. The following algorithm always returns a path starting from the input start point and ending at the destination if a path is found; otherwise, it returns a flag indicating the failure.
Algorithm 5.2 Path finding.
Input: A start point p in the MCS, A final destination point d in the MCS, and a set Sw which includes the points we have walked before.
Output: A set of points of the found path Sresult, and a flag f indicating whether a path is found or not.
78
Steps
Step 1. Initialize an empty set Sresult to store the output points.
Step 2. Add p into Sresult and Sw.
Step 3. If p can directly go to d without colliding obstacles, add d into Sresult and then go to Step 9.
Step 4. Find the avoidance points by Algorithm 5.1, resulting in a set of avoidance points Sa.
Step 5. Take out the first avoidance point pa in Sa.
Step 6. If pa is contained in Sw, take the following steps.
6.1. If there still is any avoidance point in Sa, go to Step 5; otherwise, set f to fail and then finish this algorithm.
Step 7. Apply this algorithm recursively with the inputs pa, d, and Sw, resulting in a set of points of a found path Sad and a flag fa.
Step 8. If flag fa is a success, add each point of Sad into Sresult; otherwise, go to Step 5.
Step 9. Set Sresult as output and set f to be a success.
Because the start point should always be walked and contained in the resulting path, so we add the start point into the resulting set and the walked point set. In Step 3, we check whether the start point can be directly connected to the destination point without colliding obstacles; if so, then return the path which contains only the start point and the destination point. Otherwise, we have to find a next immediate point by applying the same algorithm recursively.
In Step 8 we check the returning flag: if the path is found successfully, then we add all the points of the returning points into the resulting set; otherwise, we try the next avoidance point again.
79
A result of the path finding process is shown in Figure 5.4. We found a path starting from the top-left point to the bottom-right point. As mentioned previously, the path is not of the simplest form. We will describe the processes to simplify the path in next section.
Figure 5.4 Path found in the path finding process.
5.4 Path Simplification
After a path is found, we begin to conduct the proposed path simplification process. The process can be decomposed into two parts: redundant point elimination and distance elimination. The goal of the redundant point elimination is to find two points which are non-connected and can instead be connected together, and then to remove the points between the two points; in other words, the goal is to find a
“shortcut” between two points (like the red line shown in Figure 5.5(a)). The goal of distance elimination is to reduce the total path length by finding two points which are on different line segments and can be connected together; in other words, the goal is to find a “shortcut” between two line segments. As shown in Figure 5.5(b), we can reduce the total length of the path by substituting P2 with two new immediate points
80
(shown as red points).
After the path finding process, we conduct redundant point elimination at first, as described in Algorithm 5.3. The algorithm iterates over all immediate points in a path, and finds the last immediate point which can be connected together for each immediate point.
P1
P2
P3
P4
P1
P2
P3
(a) (b)
Figure 5.5 The redundant point elimination and the distance elimination. The black points represent the original immediate points of a path (a) The redundant point elimination, where the two redundant points P2 and P3 can be removed. (b) The distance elimination, the path length can be eliminated by substituting P2 by the two red points.
Algorithm 5.3 Redundant point elimination.
Input: A set of points of a path P, where P(i) means the ith point in the set.
Output: A set of points of a simplified version of P.
Steps
Step 1. Initialize a variable i = 1 used to represent the index of the start point of the shortcut.
Step 2. If i is equal to size(P), go to Step 8, where size(x) means the number of the points of the set x.
81
Step 3. Initialize a variable j = size(P) used to represent the index of the end point of the shortcut.
Step 4. If i = j 1, go to Step 7.
Step 5. If P(i) can be directly connected to P(j) without colliding obstacles, remove P(i+1), P(i+2), …, P(j1) and go to Step 7.
Step 6. Decrement j by 1 and go to Step 4.
Step 7. Increment i by 1 and go to Step 2.
Step 8. Take P as the output.
A result of redundant point elimination using the above algorithm is shown in Figure 5.6, where Figure 5.6(b) shows the result of applying redundant point elimination on Figure 5.6(a).
(a) (b)
Figure 5.6 Result of path finding and redundant point elimination. (a) Result of path finding. (b) Result of applying the redundant point elimination on (a).
However, the result shown in Figure 5.6(b) is still not of the simplest form. We
82
can find a short cut in the region of the red rectangle outline of Figure 5.6(b) by the distance elimination mentioned previously.
The distance elimination process checks the points on each line segment of a path.
However, if the points on a line segment are continuous, it is impossible to check all points on the segment. Therefore, the points on a line segment are discretized into several points with equal distances Td before distance elimination is conducted.
As shown in Figure 5.7, we check two line segments at a time, which are Li and Li+1, respectively. For each discretized point pi on Li and each pi+1 on Li+1, we check whether pi and pi+1 can be connected together or not. The checking order of Li is from the start point to the end point; that of Li+1 is contrarily from the end point to the start point. We check all discretized points on Li+1 for each pi. If pi (shown as blue points) cannot be connected to a point on Li+1, it is skipped and the next pi on Li+1 is then processed. Finally, we will find a shortcut between Li and Li+1 if there exists one (shown as the red line segment).
P1
Figure 5.7 Process of distance elimination. The black points are the immediate points of a path. The gray region represents the region of an obstacle, the line between the two red points are a shortcut found by the distance elimination process.
83
The following algorithm describes the complete processes to perform the distance elimination work.
Algorithm 5.4 Distance elimination.
Input: A set of points of a path P, where P(i) means the ith point in the set.
Output: A set of points of a simplified version of P.
Steps
where Td is a predefined distance between two neighboring discretized points.
2.3 Initialize two variables pi = P(i) and pi+1 = P(i+2), which are used to represent the two end points of the shortcut, respectively.
2.4 Add di1 to pi+1.
2.5 If p P ii1 ( 1)is in the opposite direction of Li+1 or p P ii1 ( 1) = 0, then go to Step 3 to check the next two line segments.
2.6 If pi can be connected directly to pi+1 without colliding obstacles, take
84
the following steps.
2.6.1. Insert pi and pi+1 into P before the position of P(i+1).
2.6.2. Remove P(i+1) from P.
2.6.3. Go to Step 3 to check the next two line segments.
2.7 Add d to pi i.
2.8 If pi Li ,go to Step 3; otherwise, go to 2.4.
Step 3. Increment i by 1 and go to Step 2.
Step 4. Take P as the output.
The result of applying the distance elimination algorithm described above on Figure 5.6(b) is shown in Figure 5.8. However, there still exist redundant points in the path of Figure 5.8. Therefore, we have to apply Algorithm 5.3 on the resulting path to reduce redundant points again. More specifically, we apply the redundant point elimination and distance elimination processes on a path until the points of the path are not changed. Algorithm 5.5 describes the above process.
Figure 5.8 Result of applying the distance elimination on the path of Figure 5.6(b).
85
Algorithm 5.5 Path simplification.
Input: A set of points of a path P, where P(i) means the ith point in the set.
Input: A set of points of a path P, where P(i) means the ith point in the set.