I have a drone that can fly autonomously and I want it to land safely.. I have a depth camera underneath it and now I can't find a way to detect flat regions to tell the drone to land on it. I'm using realsense D435i camera..this is a sample of depth image from the downwards facing camera.
can u help, pleaase?
According to this D435i infosheet
However I see no info about the depth frame if it is post processed with linear output or with perspective. You can infer it from images if the objects get smaller with distance it is still in perspective I do not know but will assume its the case.
your image is 848x480 so I will assume its just linearly scaled by
1/1.5
so the FOV is the same.linearize depth into regular grid sampled points/mesh
so each pixel must be converted into 3D position relative to the camera. So simply cast ray from the camera focal point going through pixel and stop at its distance. The resulting position is the 3D position we want. Doing this for each point will result in 2D grid of connected points (surface mesh). However we do not know if the depth is euclid or perpendicular distance (
cos
corrected) so we need to try both combinations and chose the one which has better result (building are boxes and not pyramids...)From the info I got at hand This is the best I could get from your input image:
so I took all the pixels from your image, converted x,y position into spherical
longitude,latitude
angles (linear interpolation from FOV of depth camera and image resolution) then converted pixel intensity into depth and converted that to Cartesian.where
pnt[][]
is 2D array of 3D points stored as consequentx,y,z
and:The image resolution is
xs,ys
.for more info see:
compute normal for each point (surface) from its neighbors
simple cross product will do, however its important to use correct order of operands so the resulting normals points the same direction and not opposing ... otherwise we would need to deal with it later by using
abs
ofdot
result.Do a histogram on the normals (aligned to limited set of directions)
Histogram is relation between some value of variable and its probability or occurrence. For example image histogram tells us how many pixels are colored with each color (range/bins). For our purposes flat areas (planes) should have the same normal (in your depth image is too much noise but I assume your gradient function that converts the depth into color is weirdly scaled and non linear emphasizing the noise as standard depth cameras does not have such big noises) so normals of a flat zones should be a very occurrent hence it should be represented by peaks in histogram. Firs we should convert 3D normal into single integer value (for simplifying the code) and also limit possible directions to only some fixed ones (to rule out noise and compress the output data size).
The normalized normal is 3D vector with coordinates in range
<-1,+1>
so we can convert that range to unsigned integer number of some bitwidth and stack up 3 of those into single integer. For example 5bit per coordinate:so any direction will be converted to integer
<0,2^15)
so<0,32767>
which is nicely manageable. So simply create some arrayint his[32768]
and reset all its values to zero. Then for each pixel convert its normal to such integerni
and increment its counterhis[ni]++
; beware if you got big resolution of image you need to take in mind possible overflow of counters. So for 32 bit int the reslution must be less than 2^31 pixels... otherwise you use bigger bitdepth or add condition to avoid incrementing whet max value is reached already.remove points with very low occurrence of their normal
this will allow us to ignore parts of image which are small surfaces and or are not flat enough. We can set those points to some specific normal like
(0,0,0)
or have a separate array with flags for this.find big enough continuous area with the same normals
You can brute force search a box (size of your drone + some margin) in the data (so 2 nested for loops for position and another 2 for the test) and if all points have the same normal you found possible landing zone.
normal
n1,n2
s are the "same" if:where margin is allowed angular difference between normal in
deg
orrad
depends on yourcos
implementation.[Edit1] Your data is too noisy for normal approach
here image (after smoothing of data to reduce noise) showing the occurrence of individual normal directions (with this gradient) so blue is lowest occurrence, red is highest, green is in between...
As you can see the most occurrence has pixels out of range or invalid values (hard to say) and the top of boxes that where measured correctly. However most of the surface of your view has weird very big noise (in both amplitude and radius ... those blobs) which I never saw on depth images before. My bet is your conversion from raw depth to "color" gradient is non linear and emphasize noise (that is most likely also the reason why I had so hard time to reconstruct your scene back to 3D) + JPEG lossy compression artifacts.
So unless you provide clear equation for conversion between color of pixel and depth this is a no go. So instead you need different slower approach. I would do a window "brute force" search instead:
define a window
So your drone has some size so we need some rectangle of defined size (slightly bigger than your drone area projected on ground, how much depends on the landing trajectory and flight control stability & precision ...)
So we need some rectangle of size
wx * wy
in meters (or what ever units you reconstruct the scene with).test "all" possible positions of window
so simply start at top left corner test if window of our pointcloud is fla enough and if yes you found your solution, if not test position slightly moved to the right ... until end of pointcloud reached. Then start again from left but moved slightly to the bottom. So this will be just 2 nested for loops incrementing by some value smaller than window size (does not need to be just pixel)
for each window location detect if the area is flat
so obtain avg position of each corner of window. That will define a plane so simply test all points in the window, by computing their perpendicular distance to the plane. if bigger than some threshold the area is not flat enough. If all points where OK then you found your landing zone.
Here a preview:
Of result of this approach. The red means invalid color (black on your original image) and green is the first found landing zone. I modified the code for the normal approach to do this so there is quite some code remnant that is not necessary anymore like color, normals etc ... You only need the pointcloud point positions... Here the relevant C++/OpenGL code for this:
First simple depth image to pointcloud class:
And usage:
The image loader is from mine OpenGL engine (partially based on VCL and PNGDelphi) so you need to use whatever you got in your environment.
The only important stuff (apprat fromthe 3D reconstruction) is the function
find_flat_rect
which works exactly as described in this approach.The only thing left to add is a test that landing zone is not too big slope (by dot product between plane normal
n
and down vector (from accelerometer or gyros). In case your drone is always in horizontal orientation you can usez
axis instead.Here the function that converts 3D point into 2D depth image cooridnates:
However due to not perfect 3D reconstruction (due unknown depth encoding used) is this not very accurate so the result is slightly distorted. Beware results might be slightly out of image bounds so you need to to limit it to
<0,xs)
and<0,ys)
if you want to use it for pixel access ...