Overview

In computer vision, it is a common task to map pixels in an image to a coordinate system in the real world and vice versa. The geolocation module in the airside system repository identifies the relative positioning of ground targets from the given sensory data.

Note: Throughout this document bolded lowercase variables represent vector quantities and bolded UPPERCASE variables represent matrices.

Intrinsics

An image is a grid of pixels. Pixels are the smallest component in a digital image. The resolution of an image is the number of pixels in an image. The top-left pixel in the image is the origin. The positive x-direction points in the right direction and the positive y-direction points downwards.

The camera coordinate system follows the North-East-Down (NED) coordinate system. In the NED system, the x-axis is the forward direction, the y-axis is the rightward direction, and the z-axis the downward direction. In the camera coordinate system, c corresponds to the x-direction, u corresponds to the y-direction, and v corresponds to the z-direction. The optical center corresponds to the origin in the camera coordinate system. See more information about the NED system here: https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates.

Image to Camera Space

If you are not familiar with the Field of View of Cameras, I suggest reading this document to understand the necessary background for this section https://uwarg-docs.atlassian.net/wiki/spaces/CV/pages/2237530135/Cameras#Field-of-View.

To calculate the vectors c, u and v, we assume that the magnitude of the c vector is 1 (The magnitude of c can be arbitrary as u and v will scale, we assume the magnitude c is 1 for convenience). Knowing that the magnitude of the c vector is 1 and the Field of View we can calculate the magnitude of the u and v vectors (the magnitude of u and v correspond to a and b in the diagram above respectively). This is done with basic trigonometry (we can take the tangent of half of the FOV angle since the magnitude of the c vector is 1).

If the ratio of a:b is not the same as the ratio of r_x:r_y then the pixels are not square, which doesn’t really matter but it is useful to note. Usually the pixels will be square.

To map a pixel in the image space to its corresponding vector in the camera space, we can apply the scaling function f(p) = 2(p/R) - 1 where p is the pixel location (in x or y axis), and R is the resolution (in x or y axis). This function is chosen since it maps the domain of the image space which is [0, R] to the codomain [-1, 1]. This is achieved by scaling the value by 2 and translating it down by 1.

We can multiply the value of the scaling function with the vectors u and v to get the horizontal and vertical axis of the pixel vector (u_pixel = f(p) * u and v_pixel = f(p) * v). Thus, the pixel vector, p, is equal to p = c + u_pixel + v_pixel.

Extrinsic

Three-Dimensional Rotation Matrices

It is useful to think about matrices as a transformation of space. When we multiply a vector with a matrix, we can visually see this as transforming a vector from one coordinate space to another. To better understand matrices as transformations, you can look at the resources below.

• https://www.khanacademy.org/math/precalculus/x9e81a4f98389efdf:matrices/x9e81a4f98389efdf:matrices-as-transformations/a/matrices-as-transformations

• https://www.khanacademy.org/math/precalculus/x9e81a4f98389efdf:matrices/x9e81a4f98389efdf:matrices-as-transformations/a/practice-associating-matrices-with-transformations

A rotation matrix is a special type of matrix, as it transforms the coordinate space by revolving it around the origin. A visualization of a rotation matrix can be seen below.

Rotation matrices are useful since it allows us to model the orientation of an object in 3D space. Multiplying a vector with a rotation matrix allows us to rotate a vector and change its orientation. The rotation matrices for 3D space are shown below. For more information on rotation matrices, see here: https://en.wikipedia.org/wiki/Rotation_matrix.

It is important to note that matrices are not commutative. This means that the product of two matrices A and B are not equal if I switch the order of the matrices (AB != BA).

The orientation of a rigid-body in three-dimensional space can be described with three angles (Tait-Bryan Angles). The name of these three angles in aviation are yaw, pitch, and roll. Yaw describes the angle around the z-axis, pitch describes the angle around the y-axis, and roll describes the angle around the x-axis (the coordinate system abides the NED system).

Intrinsic rotations are elemental rotations that occur about the axes of a coordinate system attached to a moving body. In contrast, extrinsic rotations are elemental rotations that occur about the axes of the fixed coordinate system. The rotations in geolocation are intrinsic. There are 6 different intrinsic rotations that can be performed (since there are 6 permutations to rotate an object around the x, y, and z axis). The order matters because as mentioned above, matrices are not commutative. The intrinsic rotation that is performed in the geolocation module is z-y-x or 3-2-1. If we have X as the rotation matrix for roll, Y as the rotation matrix for pitch, and Z for the rotation matrix for yaw, the overall transformation matrix T would be T = ZYX. The diagram below showcases the intrinsic rotation for z-y-x. For more information you can check out the link here: https://www.wikiwand.com/en/Euler_angles#Tait%E2%80%93Bryan_angles .

Camera to Drone Space

Once we are able to describe a pixel as a vector in the camera space, we need to convert it to a vector in the drone space. This vector will point to the object in the image from the perspective of the drone. To convert the vector from the camera space to the drone space, we need to know how the camera is positioned and oriented with respect to the drone.

When calculating the position of the camera in relation to the drone, the measurements must be reported from the center of the drone to the center of the camera. Any unit of measurement can be used as long as the units are consistent. Once we have the measurements of the camera in the x, y, and z axis, we can generate a vector to model the position of the camera in the drone space.

When calculating the orientation of the camera in relation to the drone, the measurements must be reported in radians when the drone is on a flat surface. The camera’s yaw, pitch, and roll with respect to the drone are used to generate a rotation matrix that models how the camera is oriented with respect to the drone.

Let’s say R is the rotation matrix describing the camera’s orientation in relation to the drone, t is the vector representing the position of the camera with respect to the drone, and p is a vector in the camera space. If we want convert the vector p into a vector in the drone space (let’s call this p') we can perform the following calculation: p' = Rp + t.

TODO: Redraw diagrams. Not sure what was the issue with the old ones.

World

Two-Dimensional Rotation and Translation

TODO: Not sure what to put in this section or why it is needed.

Drone to World Space

Once we know the vector pointing to the object is in the drone space, we need to convert it into a vector in the world space. To do this we can get the drone’s yaw, pitch, and roll in the world space and create a rotation matrix. This provides us with a vector that points to the detected object in the world coordinate system.

Let’s say W is the rotation matrix describing the drone’s orientation in the world space, and p' is the vector pointing to the object in the drone space. To get the vector pointing to the object in the world space (let’s call this p''), we can perform the following calculation: p'' = Wp'.

Projective Perspective Transform Matrix

The goal of the geolocation module is to map where a particular pixel in an image maps to the ground. The following sections dives into the theory on how geolocation works.

The diagram above displays the vectors used in the geolocation algorithm. The world space is the coordinate system used to describe the position of an object in the world. The world space is shown in the diagram with the black coordinate system. The camera space is the coordinate system used to describe the position of an object in relation to the camera. The camera space is showing in the diagram with vectors c, u, v. Note that bolded variables are vector quantities.

The table below outlines what each variable represents.

The geolocation module works under the following assumptions.

• Geolocation assumes the ground is flat. To be clear I am not saying the Earth is flat, but we can assume the ground is flat at small distances because the radius of the earth is so large. The image below displays this assumption. This assumption allows us to assume a planar coordinate system for our calculations. We make this assumption because incorporating GIS data is difficult.

World to Ground Space

Given a vector pointing to the object in the world space, we can compute where the object is on the ground by finding the intersection between the vector with the ground. Since we are assuming a planar coordinate system, the ground is the plane z = 0. We can calculate the scalar multiple, t, that extends the vector to the ground. Knowing the t value, we can then compute the x and y values and determine the ground location of the object.

Using this calculation, we can now get ground locations of target. However, these calculations become costly if we need a large number of pixels translated into ground locations.

To resolve this issue, we can compute a perspective transform matrix. A perspective transform matrix is a 3x3 matrix that maps a 2D point in one plane to another 2D point in a different plane. We use the perspective transform matrix to map a point in the image plane to a point in the world space. We use the OpenCV library to calculate the transform matrix. To get the matrix, the library needs 4 points in the image plane and the corresponding 4 points on the ground plane. Among these 4 points, 3 of the points should not be collinear (3 of the points should not be on the same line). Below is a diagram of the 4 points used to calculate the perspective transform matrix in the geolocation module.

Using the algorithm from above, we can find the corresponding ground locations for the 4 pixels above. We can then send the image points and ground points to the getPerspectiveTransform function in OpenCV and compute the perspective transform matrix to map any pixel in the image to a location on the ground.

Let’s say P is the perspective transform matrix and a pixel in an image are p = (x, y).

Break ---------------------------------------------------------------

Overview

In computer vision, it is a common task to map pixels in an image to a coordinate system in the real world and vice versa. The geolocation module in the airside system repository identifies the relative positioning of ground targets from the given sensory data.

maps pixels in an image to geographical coordinates using the drone's position and orientation.

Theory

The diagram above displays the vectors used in the geolocation algorithm. The world space is the coordinate system used to describe the position of an object in the world (e.g. latitude and longitude). The world space is shown in the diagram with the black coordinate system. The camera space is the coordinate system used to describe the position of an object in relation to the camera. The camera space is showing in the diagram with vectors c, u, v. Note that bolded variables are vector quantities.

The table below outlines what each variable represents.

To compute the geographical coordinates (latitude and longitude) of a pixel in an image, we need to convert a pixel in the image (p₁, p₂) → vector in the camera space (p) → vector in the world space (a).

We can compute the scaling factor (let’s call the scaling factor t) with the equation below.

Rotation Matrix

It is useful to think about a matrix as a transformation in space. A matrix can warp the coordinate system into a different shape. When we multiply a vector with a matrix, we can visually see this as transforming a vector from one coordinate space to another. To better understand matrices as transformations, you can look at the resources below.

• https://www.khanacademy.org/math/precalculus/x9e81a4f98389efdf:matrices/x9e81a4f98389efdf:matrices-as-transformations/a/matrices-as-transformations

• https://www.khanacademy.org/math/precalculus/x9e81a4f98389efdf:matrices/x9e81a4f98389efdf:matrices-as-transformations/a/practice-associating-matrices-with-transformations

A rotation matrix is a special type of matrix, as it transforms the coordinate space by revolving it around the origin. A visualization of a rotation matrix can be seen below.

Rotation matrices are useful since it allows us to model the orientation of an object in 3D space. Multiplying a vector with a rotation matrix allows us to rotate a vector and change its orientation. The rotation matrices for 3D space are shown below. For more information on rotation matrices, see here: https://en.wikipedia.org/wiki/Rotation_matrix

Intrinsic Camera Properties

Pixels and the Camera Space

You can think of an image as a grid of pixels. Pixels are the smallest component in a digital image. The resolution of an image is the number of pixels in an image. The top-left pixel in the image is the origin (the point (0, 0)). The positive x-direction points in the right direction and the positive y-direction points downwards.

The camera coordinate system follows the North-East-Down (NED) coordinate system. In the NED system, the x-axis is the forward direction, the y-axis is the rightward direction, and the z-axis the downward direction. In the camera coordinate system, c corresponds to the x-direction, u corresponds to the y-direction, and v corresponds to the z-direction. See more information about the NED system here: https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates.

Calculating Camera Space Vectors

Pixel to Vector in Camera Space

To map a pixel to its corresponding vector in the camera space we use the scaling function shown below.

From the following calculations we can see that the codomain of the scaling function is [-1, 1] (the maximum value of the function is 1 and the minimum value of the function is -1). We can multiply the value of the scaling function with the vectors u and v to get the horizontal and vertical axis of the pixel vector (u_pixel = f(p, r) * u and v_pixel = f(p, r) * v). Thus the pixel vector (p) is equal to c + u_pixel + v_pixel.

Extrinsic Camera Properties

Before we can convert vectors in the camera space to vectors in the world space, we need to know how the camera is positioned in relation to the drone. We need to know the position of the camera in relation to the drone and the orientation of the camera in relation to the drone. The position and orientation of the camera is reported in the NED coordinate system.

When calculating the position of the camera in relation to the drone, the measurements must be reported in meters from the center of the drone. When calculating the orientation of the camera in relation to the drone, the measurements must be reported in radians when the drone is on a flat surface.

Geolocation

The Geolocation class in the airside system repository is responsible for converting a pixel in the image space to coordinates in the NED system in the world space. Geolocation works by creating a perspective transform matrix that maps pixels to coordinates in the real world. Below outlines the functions used in the `Geolocation`

 drone position local from global

The geolocation algorithm assumes a planar coordinate system. We can make the assumption that the earth is a planar coordinate system at small distances (because the radius of the earth is so large). From the FlightControllerWorker we get the latitude and longitude of the drone in the WGS84 coordinate system. With the WGS84 coordinate system, the origin lies in the coast of Africa so assuming a planar coordinate system would be completely wrong. Instead, we can take the home location and make that the origin. This allows us to make the assumption that the world coordinate system in planar. We can convert latitude, longitude, and altitude to the NED coordinate system using the pymap3d library. To do this we need to use the drone's current location and the home location.

 ground intersection from vector

Given the camera’s location in the world space and a vector pointing down in the world space, this function can calculate where the vector pointing down will intersect the ground. The calculation for doing so is shown below.

In this diagram, o is camera’s location in the world space and a is the vector pointing down. We can calculate the scaling factor (t in the diagram) and use it to calculate the x and y coordinate in the world space.

 get perspective transform matrix

A perspective transform matrix is a 3x3 that maps a 2D point in one plane to another 2D point in a different plane. We use the perspective transform matrix to map a point in the image plane to the a point in the world space. We use the OpenCV library to calculate the transform matrix. To get the perspective transform matrix, 4 points on the image are needed. Among these 4 points, 3 of the points should not be collinear (3 of the points should not be on the same line). Below is a diagram of the 4 points used to calculate the perspective transform matrix.

Let’s call these 4 points the image sources. The 4 image sources can be converted into vectors in the camera space using the scaling function. The sources will then be multiplied by the rotation matrix of the camera in relation to the drone. This will result in the source vectors being oriented in the world space in relation to the drone. Finally, the source vectors are multiplied with the drone orientation matrix to get the source vectors in the world space. We can then send the source vectors to the __ground_intersection_from_vector function to get the corresponding the ground points in the world space.

We now have four pairs of tuples in the image space and the world space. We can pass that to the getPerspectiveTransform function that OpenCV provides, and we now have our matrix!

Now that we have described how the functions in the Geolocation class works, we can go through the run function and see how the entire algorithm works.

1. Get the MergedOdometeryDetection object from the queue and create the rotation matrix that models the drone’s orientation with the drone’s yaw, roll, and pitch

2. Get the home location from the home_location_queue and the drone’s location from the MergedOdometryDetection object and pass it into the drone_position_local_from_global function to get the drone’s location in the NED coordinate system.

3. Pass in the drone’s rotation matrix and the drone’s position in the NED system to the __get_perspective_transform_matrix function to get the perspective transform matrix.

4. Pass in the perspective transform matrix and Detection object into the __convert_detection_to_world_from_image to convert the detected object into coordinates in the world frame.

5. Create a DetectionInWorld object and output it in the output queue.

Logs

Geolocation saves logs locally for the detections it makes in logs/{date+time}/geolocation_worker.log.

Below is an example of what it could look like:

12:36:56: [INFO] [/home/warg/computer-vision-python/modules/geolocation/geolocation_worker.py | geolocation_worker | 39] Logger initialized
12:37:02: [INFO] [/home/warg/computer-vision-python/modules/geolocation/geolocation.py | run | 320] <class 'modules.detection_in_world.DetectionInWorld'>, vertices: [[0.6552691643408014, 9.98856225523965], [0.6482836937744504, 9.986302393051126], [0.6578720594692677, 9.980787791068765], [0.6508864409775312, 9.97852507854267]], centre: [    0.65308      9.9835], label: 0, confidence: 1.0
12:37:05: [INFO] [/home/warg/computer-vision-python/modules/geolocation/geolocation.py | run | 320] <class 'modules.detection_in_world.DetectionInWorld'>, vertices: [[0.4275230900336949, 10.152804162814556], [0.2980973314064277, 10.114202092691663], [0.46949445525840316, 10.023504609670422], [0.340985020395882, 9.982833647293372]], centre: [    0.38457      10.068], label: 0, confidence: 1.0
12:37:08: [INFO] [/home/warg/computer-vision-python/modules/geolocation/geolocation.py | run | 320] <class 'modules.detection_in_world.DetectionInWorld'>, vertices: [[0.49811098655414815, 10.170383875702877], [0.36995788863253354, 10.095773204556759], [0.5752676468673406, 10.046568235880605], [0.44786116828996236, 9.969197821890818]], centre: [    0.47329      10.071], label: 0, confidence: 1.0

If the operation is successful, the [INFO] tag is used to log the results; otherwise, an [ERROR] tag will be present. The input is a bounding box (with 4 corners and a center), so the output also has 4 corners and a center. The vertices are in the order: (top left, top right, bottom left, bottom right).

Each point will have 2 or 3 coordinates, in the NED format (in meters). If there are 2 coordinates, it would be (North, East), and if there are 3 coordinates, it would be (North, East, Down).

All of the distances are relative to the home location, so [0.65, 9.98] indicates that the point is 0.65 meters north and 9.98 meters east of the home location (take-off position).