3D Obstacles Modeling for drones Sense&Avoid using Node.js and Spatialite

How would you model 3D objects like buildings, bridges and electricity wires? How would you query this data to support your Sense&Avoid solution for your drone?

In this post, we'll discuss an approach to model and query obstacles. Using their real-time video processing capabilities, drones can update the model when identifying new objects or mapping an area. In addition, drones can query for potential obstacles in their pre-compiled route, and reroute accordingly.

To make this discussion relevant to real world scenarios, I chose to work with Toronto's publicly available model, called 3D Massing. You can read about its release earlier this year here. Also, a great interactive visualization of this model can be found here.

In terms of technology, I'm going to work with Node.js and Spatialite, which is a version of Sqlite with extensions for manipulating geometry data. I chose these technologies because it's easy to write a REST API service and run it everywhere. We can host this service in the cloud and we can run it locally on the device (the drone).

A possible scenario for drones will be to run the same service locally with a subset of the data on the device. That way, drones can download part of the data for the area they operate in before taking off. Let's assume that during flight a drone identifies that a specific part of the route is blocked by an unrecorded obstacle. In this case, a short response time is essential and the re-route should be calculated as fast as possible. Response time will be faster when querying the local service than querying a remote REST API.

As mentioned earlier, for the modeling part I'm going to use Toronto's publicly available shape file. In order to use this model with Spatialite, I used a tool called QGIS which is a free and open source geographic information system. I opened the downloaded shape file, and exported it to a Spatialite DB file format.

Toronto 3D Massing Model

Exporting the shape file

Exporting the shape file

Saving file as a Spatialite DB file

Saving file as a Spatialite DB file

The provided shape file uses a Coordinate Reference System (CRS) to map geographical entities. You can read more about how the globe is split to different zones here.

I'm not going to go too much into details, but just a short explanation about how shapes are being stored in a Spatialite Geometry column type. So a Geometry value can be a Point, a Line (which is a collection of points), a Polygon (which is a collection of lines), etc. A point is composed of 2 values, for X and Y which are the location of the point on the area, relative to the CRS.

The objects in the shape file are basically Polygons or MultiPolygons describing the layout of the shape (building, bridge, etc). In addition to the 2D layout, each shape also includes an altitude. For the purpose of this post I used the Elevation, which is the building height from Mean Sea Level.

Spatialite provides APIs to do manipulations and calculations on Geometry type fields, like finding out which geometries intersect others,  which cross others, etc. When we want to find all objects in a specific area we can use these functions, but this will result in a full-table scan. Spatialite will go over all records and for each - it'll do the specific calculation to find if it satisfies the query criteria.

We want to be able to query for objects in specific areas so we need the ability to query based on a specific location. Sqlite index cannot be used against a Geometry type column. This is because regular index tables in Sqlite are based on BTree (i.e. Binary Tree) which can usefully be applied on numbers and / or strings, but not on geometries. For geometries, we need a different kind of index, i.e. the so called RTree (Rectangle Tree). Spatialite uses a RTree implementation to create Spatial Index on Geometry type columns.

As part of exporting the shape file to Spatialite, it also generated a Spatialite RTree index table. We can see that for each object in the data table, there is a referencing record in the index table describing its "boundary box": (xmin, ymin) and (xmax, ymax). Using this info, we are able to filter only objects in the area we're interested in by a simple "inner join". We can now apply the Spatialite APIs on this subset of shapes to get those we were looking for.

The following is a tool called spatialite-gui which you can use to manage Spatialite DBs:

Spatialite GUI Tool

Spatialite GUI Tool

Before we dive into the code, let's go over the approach. This is just an example and you can tune it the way you feel right. Let's assume that we would like to know if the following route at the height of 100 meters is free of obstacles.

This route is composed of the following 3 points:

Route Line: [-8848015, 5425525], [-8845603, 5426088], [-8845543, 5425149]

Route Line: [-8848015, 5425525], [-8845603, 5426088], [-8845543, 5425149]

The route's layout in Spatialite Geometry Explorer

The route's layout in Spatialite Geometry Explorer

We can see that it is composed of 3 vertices. Since we want some safe distance between our drone to the objects around it, we'd like to create some kind of a tunnel around this route. We'll use the Buffer function which adds extra space around the route (50 meters in this case):

Using the Buffer function to pad the route with a "safe distance"

Using the Buffer function to pad the route with a "safe distance"

Adding the extra buffer resulted in a 158 vertices tunnel.

Since the calculations get more complex the more vertices we have, let's try to reduce the complexity of the Geometry, but still keep the necessary info. We can do this using the SimplifyPreserveTopology function that decreased our shape's "resolution" to 15 vertices and still keeps its topology:

Simplified Geometry

Simplified Geometry

In our query, we're looking for objects that overlap this shape, but we don't want to full-scan all shapes in our table. We want to filter only those shapes that are in the area we're flying added to some buffer that we get in the input parameters.

To find the relevant area, we'll use the following query:

Find the relevant area

Find the relevant area

Envelope gets a bounding box for the original route. We add some buffer (100 meters in this case) and then we're getting another envelope to reduce the number of vertices. Now when we author our query, we'll be able to first look for all objects in this area, and apply the actual Overlap function on them. In addition, we'll also add the height/altitude condition to the mix.

 

It's time to make our hands dirty. The related code can be found here.

Most of the Node.js code is just a wrapper around the Spatialite query, exposing it as a POST /obstacles REST API.

 

The API gets the following JSON object:

{
  "droneHeight": 203,
"safetyDistanceHeight": 10,
"safetyDistanceSides": 10,
"buffer": 500,
"lineRoute": [
  [-8848015, 5425525], 
  [-8845603, 5426088], 
  [-8845543, 5425149]
  ],
"useSpatialIndex": true,
"debug": true
}

and returns the following output:

{
"obstacles": [
{ "id": 94904, "Z":193.3188, 
  "centerX": -8848353.258156676, 
  "centerY": 5425583.826544509 
  },
{ "id": 125365, "Z":203.0068, 
  "centerX": -8847628.696021229, 
  "centerY": 5425971.979974609 
  }
],
"debugInfo": { 
"request": { the request parameters },
"query": { the SQL query executed }",
"duration": { time in msec to execute the query }
}
}

Use the debug parameter to get the debugInfo block in the response. The debugInfo block includes the original request parameters, the query that was generated and the execution time.

This is the actual query:

 

Example request:

We can see that the route is free of obstacles. Now let's change our safe distance from 10 to 315, and also enable debugging information:

This time we can see that there's an obstacle in that route. If we look carefully, we can see that there's a small object (in the red circle) that is relatively close to the route. The more we increase the safe distance- the more objects we will get.

Obstacle identified within safe-distance

Now just for the sport, let's try to not use the spatial index table in our query, and see how long it takes for the query to run:

Same query without using the spatial index

Same query without using the spatial index

We get the same result, only this time it takes nearly 10 (!!) seconds because Spatialite performs a full table scan without using the Spatial index table.

 

Next step will be to use this service as part of a routing algorithm that calculates obstacles-free routes.