如何根据深度信息找到平坦区域
How to find flat regions based on depth information
我有一架可以自主飞行的无人机，我希望它能安全着陆..
我在它下面有一个深度相机，现在我找不到检测平坦区域以告诉无人机降落在上面的方法。
我正在使用 realsense D435i 相机..这是来自向下相机的深度图像示例。
你能帮忙吗？
RGB FOV(1920x1080): 64° × 41° × 77° (±3°) // HxVxD what to heck is D???
depth FOV(1280x720): 86° × 57° (±3°)
min depth(1280x720): 26cm
max depth: 3m
但是，如果 post 使用线性输出或透视处理，我看不到有关深度帧的信息。如果物体随着距离变小，你可以从图像中推断出它仍然在透视图中，我不知道，但会假设是这种情况。
您的图像是 848x480，所以我假设它只是按 1/1.5 线性缩放，所以 FOV 是相同的。
将深度线性化为采样的规则网格 points/mesh
所以每个像素都必须转换成相对于相机的 3D 位置。所以简单地从相机焦点投射光线穿过像素并在它的距离处停止。结果位置就是我们想要的 3D 位置。对每个点执行此操作将导致连接点的二维网格（表面网格）。但是我们不知道深度是欧几里得还是垂直距离（cos 更正）所以我们需要尝试两种组合并选择效果更好的组合（建筑物是盒子而不是金字塔......）
根据我手头的信息这是我能从你的输入图像中得到的最好的信息：
所以我从你的图像中取出所有像素，将 x,y 位置转换为球形 longitude,latitude 角度（深度相机的 FOV 和图像分辨率的线性插值）然后将像素强度转换为深度并将其转换为笛卡尔。
// depth -> PCL
for (y=0;y<ys;y++)
{
pd=(int*)depth->ScanLine[y]; // pointer to y-th scan line in depth image
b=depthFOVy*(((double(y)-depthy0)/double(ys-1))-0.5); // latitude
for (x=x3=0;x<xs;x++,x3+=3)
{
a=depthFOVx*(((double(x)-depthx0)/double(xs-1))-0.5); // longitude
r=pd[x]&255; // RGB from drepth (just single channel
r/=255.0; // linear scale
if (int(depthcfg&_DepthImage_invert_z)) r=1.0-r;
r=depthz0+(r*(depthz1-depthz0)); // range <z0,z1>
// spherical -> cartesian
p[0]=cos(a)*cos(b); // x
p[1]=sin(a)*cos(b); // y
p[2]= sin(b); // z
if (int(depthcfg&_DepthImage_cos_correct_z)) r/=p[0];
pnt[y][x3+0]=r*p[1];
pnt[y][x3+1]=r*p[2];
pnt[y][x3+2]=r*p[0];
}
}
其中 pnt[][] 是 3D 点的 2D 数组，存储为 x,y,zand:
pcl.depthz0=0.8;
pcl.depthz1=1.00;
pcl.depthcfg=_DepthImage_invert_z|_DepthImage_cos_correct_z;
图像分辨率为xs,ys。
有关详细信息，请参阅：
- Does Kinect Infrared View Have an offset with the Kinect Depth View
从其邻居计算每个点（表面）的法线
简单的叉积就可以了，但是使用正确的操作数顺序很重要，这样生成的法线指向相同的方向而不是相反的方向......否则我们稍后需要使用 abs 来处理它dot 个结果。
绘制法线直方图（与有限的一组方向对齐）
直方图是变量的某些值与其概率或发生之间的关系。例如，图像直方图告诉我们每种颜色有多少像素 (range/bins)。出于我们的目的，平坦区域（平面）应该具有相同的法线（在您的深度图像中有太多噪声，但我假设您将深度转换为颜色的渐变函数被奇怪地缩放并且非线性强调噪声，因为标准深度相机没有如此大的噪音）所以平坦区域的法线应该是非常常见的，因此它应该由直方图中的峰值表示。首先，我们应该将 3D 法线转换为单个整数值（为了简化代码），并将可能的方向限制为仅一些固定方向（以排除噪声并压缩输出数据大小）。
归一化法线是坐标在 <-1,+1> 范围内的 3D 向量，因此我们可以将该范围转换为某个位宽的无符号整数，并将其中的 3 个叠加成单个整数。例如每个坐标5bit：
normal = (nx,ny,nz)
nx = 31.0*(nx+1.0)*0.5; // <-1,+1> -> <0,31>
ny = 31.0*(nx+1.0)*0.5; // <-1,+1> -> <0,31>
nz = 31.0*(nx+1.0)*0.5; // <-1,+1> -> <0,31>
int ni = int(nx) + int(ny)<<5 + int(nz)<<10
所以任何方向都将转换为整数 <0,2^15) 所以 <0,32767> 这很容易管理。因此，只需创建一些数组 int his[32768] 并将其所有值重置为零即可。然后对于每个像素，将其法线转换为这样的整数 ni 并增加其计数器 his[ni]++；当心如果你有高分辨率的图像，你需要记住计数器可能溢出。因此，对于 32 位 int，分辨率必须小于 2^31 像素...否则您可以使用更大的位深度或添加条件来避免递增已经达到最大值。
删除正常出现率非常低的点
这将使我们能够忽略图像中的小表面或不够平坦的部分。我们可以将这些点设置为一些特定的法线，例如 (0,0,0) 或者有一个带有标志的单独数组。
找到具有相同法线的足够大的连续区域
你可以在数据中暴力搜索一个框（你的无人机的大小 + 一些边距）（所以 2 个嵌套 for 循环用于位置，另外 2 个用于测试）如果所有点都具有相同的法线，你发现可能着陆区。
正常 n1,n2 s 是“相同的”，如果：
dot(n1,n2)/(|n1|.|n2|) >= cos(margin)
允许边距的地方 angular deg 或 rad 之间的正常差异取决于您的 cos 实施。
[Edit1] 您的数据对于正常方法而言噪音太大
这里的图像（在对数据进行平滑处理以减少噪声后）显示了各个法线方向（this gradient）的出现，因此蓝色出现率最低，红色最高，绿色介于两者之间...
如您所见，出现次数最多的是像素超出范围或无效值（很难说）以及正确测量的方框顶部。然而，你的视图的大部分表面都有奇怪的非常大的噪音（在两个振幅上d radius ...那些斑点），我以前从未在深度图像上看到过。我敢打赌你从原始深度到“颜色”渐变的转换是非线性的并且强调噪声（这很可能也是我很难将你的场景重建回 3D 的原因）+ JPEG 有损压缩伪影。
因此，除非您为像素颜色和深度之间的转换提供明确的方程式，否则这是行不通的。因此，您需要不同的较慢的方法。我会改为 window“暴力”搜索：
定义一个window
所以你的无人机有一些尺寸，所以我们需要一些定义尺寸的矩形（比你的无人机在地面上的投影面积略大，多少取决于着陆轨迹和飞行控制稳定性和精度......）
所以我们需要一些大小为 wx * wy 的矩形（以米为单位）（或者您重建场景时使用的任何单位）。
测试 window
的“所有”可能位置
所以只需从左上角开始测试，如果我们的点云的 window 足够平坦，如果是，你找到了你的解决方案，如果没有，测试位置稍微向右移动......直到到达点云的末端.然后从左边重新开始，但稍微移动到底部。所以这将只是 2 个嵌套的循环，递增的值小于 window 大小（不需要只是像素）
每个window位置检测区域是否平坦
所以得到window每个角的平均位置。这将定义一个平面，因此通过计算它们与平面的垂直距离来简单地测试 window 中的所有点。如果大于某个阈值，则该区域不够平坦。如果所有点都正常，那么您就找到了着陆区。
这里是预览：
这种方法的结果。红色表示无效颜色（原始图像上为黑色），绿色是第一个找到的着陆区。我修改了正常方法的代码来执行此操作，因此有相当多的代码残余不再需要，例如颜色，法线等...您只需要点云点位置...这里是相关的 C++/OpenGL 代码:
第一个简单的点云深度图像class:
const int _DepthImage_invert_z =0x00000001; // brighter colros are closer
const int _DepthImage_cos_correct_z =0x00000002; // depth is perpendicular distance instead of euclid
const int _DepthImage_smooth =0x00000004; // average position to reduce noise
class DepthPoint
{
public:
double pos[3]; // position (x,y,z)
double nor[3]; // normal vector (x,y,z)
double col[3]; // color (r,g,b)
DepthPoint(){};
DepthPoint(DepthPoint& a) { *this=a; }
~DepthPoint(){};
DepthPoint* operator = (const DepthPoint *a) { *this=*a; return this; }
//DepthPoint* operator = (const DepthPoint &a) { ...copy... return this; }
};
class DepthImage
{
public:
// resoluton
int xs,ys; // resolution
// point cloud ys x xs
DepthPoint **pnt; // pnt[y][x] point for each pixel(x,y) of image
// depth camera properties
double depthFOVx,depthFOVy, // [rad] FOV angles
depthx0,depthy0, // [pixels] FOV offset
depthz0,depthz1; // [m] intensity <0,255> -> distance <z0,z1>
int depthcfg; // configuration flags
double error; // invalid depth
// color camera properties
double colorFOVx,colorFOVy, // [rad] FOV angles
colorx0,colory0; // [pixels] FOV offset
int colorcfg; // configuration flags
DepthImage()
{
pnt=NULL; _free();
depthFOVx=86.0*deg; colorFOVx=86.0*deg;
depthFOVy=57.0*deg; colorFOVy=57.0*deg;
depthx0=0.0; colorx0=0.0;
depthy0=0.0; colory0=0.0;
depthz0=0.0;
depthz1=6.0;
depthcfg=0; colorcfg=0;
}
DepthImage(DepthImage& a) { *this=a; }
~DepthImage() { _free(); }
DepthImage* operator = (const DepthImage *a) { *this=*a; return this; }
//DepthImage* operator = (const DepthImage &a) { ...copy... return this; }
void _free()
{
if (pnt)
{
if (pnt[0]) delete[] pnt[0];
delete[] pnt;
}
pnt=NULL; xs=ys=0;
}
void load(AnsiString _depth,AnsiString _color)
{
_free();
Graphics::TBitmap *depth,*color;
int i,x,y,*pd,*pc;
double p[3],q[3],a,b,r;
// load depth image
depth=new Graphics::TBitmap;
picture_load(depth,_depth,NULL);
depth->HandleType=bmDIB;
depth->PixelFormat=pf32bit;
xs=depth->Width;
ys=depth->Height;
// allocate PCL
pnt =new DepthPoint*[ ys];
pnt[0]=new DepthPoint [xs*ys];
for (y=0;y<ys;y++) pnt[y]=pnt[0]+(y*xs);
// depth -> PCL
error=depthz1*0.99;
for (y=0;y<ys;y++)
{
pd=(int*)depth->ScanLine[y]; // pointer to y-th scan line in depth image
b=depthFOVy*(((double(y)-depthy0)/double(ys-1))-0.5); // latitude
for (x=0;x<xs;x++)
{
a=depthFOVx*(((double(x)-depthx0)/double(xs-1))-0.5); // longitude
r=pd[x]&255; // RGB from drepth (just single channel
r/=255.0; // linear scale
if (int(depthcfg&_DepthImage_invert_z)) r=1.0-r;
r=depthz0+(r*(depthz1-depthz0)); // range <z0,z1>
// spherical -> cartesian
p[0]=cos(a)*cos(b); // x
p[1]=sin(a)*cos(b); // y
p[2]= sin(b); // z
if (int(depthcfg&_DepthImage_cos_correct_z)) r/=p[0];
// scale and reorder coordinates to match my view
pnt[y][x].pos[0]=r*p[1];
pnt[y][x].pos[1]=r*p[2];
pnt[y][x].pos[2]=r*p[0];
}
}
// smooth positions a bit to reduce noise (FIR averaging smooth/blur filter)
if (int(depthcfg&_DepthImage_smooth))
for (i=0;i<10;i++)
{
for (y=1;y<ys;y++)
for (x=1;x<xs;x++)
{
vector_mul(p,pnt[y][x].pos,3.0);
vector_add(p,p,pnt[y][x-1].pos);
vector_add(p,p,pnt[y-1][x].pos);
vector_mul(pnt[y][x].pos,p,0.2);
}
for (y=ys-1;y>0;y--)
for (x=xs-1;x>0;x--)
{
vector_mul(p,pnt[y-1][x-1].pos,3.0);
vector_add(p,p,pnt[y-1][x].pos);
vector_add(p,p,pnt[y][x-1].pos);
vector_mul(pnt[y][x].pos,p,0.2);
}
}
// compute normals
for (y=1;y<ys;y++)
for (x=1;x<xs;x++)
{
vector_sub(p,pnt[y][x].pos,pnt[y][x-1].pos);// p = pnt[y][x] - pnt[y][x-1]
vector_sub(q,pnt[y][x].pos,pnt[y-1][x].pos);// q = pnt[y][x] - pnt[y-1][x]
vector_mul(p,q,p); // p = cross(q,p)
vector_one(pnt[y][x].nor,p); // nor[y][x] = p/|p|
}
// copy missing edge normals
for (y=1;y<ys;y++) vector_copy(pnt[y][0].nor,pnt[y][1].nor);
for (x=0;x<xs;x++) vector_copy(pnt[0][x].nor,pnt[1][x].nor);
// set color
for (y=0;y<ys;y++)
for (x=0;x<xs;x++)
{
DepthPoint *p=&pnt[y][x];
vector_ld(p->col,0.5,0.5,0.5);
if (p->pos[2]>=error) vector_ld(p->col,1.0,0.0,0.0); // invalid depths as red
}
delete depth;
}
bool find_flat_rect(double* &p0,double* &p1,double* &p2,double* &p3,double wx,double wy,double dmax)
{
int i,x,y,x0,y0,x1,y1,dx=1,dy=1;
double d,p[3],n[3];
// whole image
for (y0=0;y0<ys;y0+=dx)
for (x0=0;x0<xs;x0+=dy)
{
dx=1; dy=1;
// first coorner
p0=pnt[y0][x0].pos; if (p0[2]>=error) continue;
// find next coorner wx distant to p0
for (x1=x0+1;x1<xs;x1++)
{
p1=pnt[y0][x1].pos; if (p1[2]>=error) continue;
vector_sub(p,p1,p0); // p = p1-p0
d=vector_len(p); // d = |p|
if (d>=wx) break;
} if (x1>=xs){ x0=xs; continue; }
// find next coorner wy distant to p0
for (y1=y0+1;y1<ys;y1++)
{
p3=pnt[y1][x0].pos; if (p3[2]>=error) continue;
vector_sub(p,p3,p0); // p = p3-p0
d=vector_len(p); // d = |p|
if (d>=wy) break;
} if (y1>=ys){ p0=p1=p2=p3=NULL; return false; }
// diagonal coorner
p2=pnt[y1][x1].pos;
// window position increments are 1/4 of window size
dx=1+((x1-x0)>>2);
dy=1+((y1-y0)>>2);
// plane normal
vector_sub(p,p1,p0); // p = p1-p0
vector_sub(n,p3,p0); // n = p3-p0
vector_mul(n,n,p); // n = cross(n,p)
vector_mul(n,n); // n = n/|n|
// test distances to plane
i=1;
for (y=y0;y<=y1;y++)
for (x=x0;x<=x1;x++)
{
if (pnt[y][x].pos[2]>=error) continue;
vector_sub(p,pnt[y][x].pos,p0); // p = pnt[y][x] - p0
d=fabs(vector_mul(p,n)); // d = |dot(p,n)| ... perpendicular distance
if (d>dmax){ i=0; x=x1+1; y=y1+1; break; }
}
if (i) return true;
}
p0=p1=p2=p3=NULL;
return false;
}
void gldraw()
{
int x,y;
DepthPoint *p;
for (y=1;y<ys;y++)
{
glBegin(GL_QUAD_STRIP);
for (x=0;x<xs;x++)
{
p=&pnt[y-1][x];
glColor3dv(p->col);
glNormal3dv(p->nor);
glVertex3dv(p->pos);
p=&pnt[y][x];
glColor3dv(p->col);
glNormal3dv(p->nor);
glVertex3dv(p->pos);
}
glEnd();
}
glColor3f(1.0,1.0,1.0);
}
};
//---------------------------------------------------------------------------
和用法：
DepthImage pcl; // pointcloud
double *q0,*q1,*q2,*q3; // pointers to landing zone rectangle points
pcl.depthz0=0.8; // some constants for reconstruction
pcl.depthz1=1.00;
pcl.depthcfg=
_DepthImage_invert_z
|_DepthImage_cos_correct_z
;
pcl.load("depth.jpg",""); // this creates the pointcloud from image
pcl.find_flat_rect(q0,q1,q2,q3,0.40,0.25,0.005); // this finds a rectangle of 0.4x0.25 m size and 0.005m deviation from flat surface (return true if found)
// the q0,q1,q2,q3 are pointers to 3D points of the found rectangle or NULL if no such has been found
图像加载器来自我的 OpenGL 引擎（部分基于 VCL 和 PNGDelphi），因此您需要使用环境中的任何内容。
唯一重要的东西（来自 3D 重建的应用程序）是函数 find_flat_rect，它完全按照此方法中的描述工作。
唯一要添加的是测试着陆区的坡度不是太大（通过平面法线 n 和向下矢量（来自加速度计或陀螺仪）之间的点积）。如果你的无人机总是在水平方向上，您可以使用 z 轴。
这里将3D点转换为2D深度图坐标的函数为：
void DepthImage::project(double &x,double &y,double* p) // p[3] -> (x,y) ... 3D to 2D perspective projection
{
x=atan2(p[0],p[2])/depthFOVx;
y=atan2(p[1],p[2])/depthFOVy;
if (int(depthcfg&_DepthImage_cos_correct_z))
y*=(double(ys)*depthFOVx)/(depthFOVy*double(xs));
x=(x+0.5)*xs;
y=(y+0.5)*ys;
}
然而，由于 3D 重建不完美（由于使用了未知的深度编码），这不是很准确，因此结果略有失真。当心结果可能略微超出图像范围，因此如果您想将其用于像素访问，则需要将其限制为 <0,xs) 和 <0,ys) ...
我有一架可以自主飞行的无人机，我希望它能安全着陆.. 我在它下面有一个深度相机，现在我找不到检测平坦区域以告诉无人机降落在上面的方法。 我正在使用 realsense D435i 相机..这是来自向下相机的深度图像示例。
你能帮忙吗？
RGB FOV(1920x1080): 64° × 41° × 77° (±3°) // HxVxD what to heck is D???
depth FOV(1280x720): 86° × 57° (±3°)
min depth(1280x720): 26cm
max depth: 3m
但是，如果 post 使用线性输出或透视处理，我看不到有关深度帧的信息。如果物体随着距离变小，你可以从图像中推断出它仍然在透视图中，我不知道，但会假设是这种情况。
您的图像是 848x480，所以我假设它只是按 1/1.5 线性缩放，所以 FOV 是相同的。
将深度线性化为采样的规则网格 points/mesh
所以每个像素都必须转换成相对于相机的 3D 位置。所以简单地从相机焦点投射光线穿过像素并在它的距离处停止。结果位置就是我们想要的 3D 位置。对每个点执行此操作将导致连接点的二维网格（表面网格）。但是我们不知道深度是欧几里得还是垂直距离（
cos更正）所以我们需要尝试两种组合并选择效果更好的组合（建筑物是盒子而不是金字塔......）根据我手头的信息这是我能从你的输入图像中得到的最好的信息：
所以我从你的图像中取出所有像素，将 x,y 位置转换为球形
longitude,latitude角度（深度相机的 FOV 和图像分辨率的线性插值）然后将像素强度转换为深度并将其转换为笛卡尔。// depth -> PCL for (y=0;y<ys;y++) { pd=(int*)depth->ScanLine[y]; // pointer to y-th scan line in depth image b=depthFOVy*(((double(y)-depthy0)/double(ys-1))-0.5); // latitude for (x=x3=0;x<xs;x++,x3+=3) { a=depthFOVx*(((double(x)-depthx0)/double(xs-1))-0.5); // longitude r=pd[x]&255; // RGB from drepth (just single channel r/=255.0; // linear scale if (int(depthcfg&_DepthImage_invert_z)) r=1.0-r; r=depthz0+(r*(depthz1-depthz0)); // range <z0,z1> // spherical -> cartesian p[0]=cos(a)*cos(b); // x p[1]=sin(a)*cos(b); // y p[2]= sin(b); // z if (int(depthcfg&_DepthImage_cos_correct_z)) r/=p[0]; pnt[y][x3+0]=r*p[1]; pnt[y][x3+1]=r*p[2]; pnt[y][x3+2]=r*p[0]; } }其中
pnt[][]是 3D 点的 2D 数组，存储为x,y,zand:pcl.depthz0=0.8; pcl.depthz1=1.00; pcl.depthcfg=_DepthImage_invert_z|_DepthImage_cos_correct_z;图像分辨率为
xs,ys。有关详细信息，请参阅：
- Does Kinect Infrared View Have an offset with the Kinect Depth View
从其邻居计算每个点（表面）的法线
简单的叉积就可以了，但是使用正确的操作数顺序很重要，这样生成的法线指向相同的方向而不是相反的方向......否则我们稍后需要使用
abs来处理它dot个结果。绘制法线直方图（与有限的一组方向对齐）
直方图是变量的某些值与其概率或发生之间的关系。例如，图像直方图告诉我们每种颜色有多少像素 (range/bins)。出于我们的目的，平坦区域（平面）应该具有相同的法线（在您的深度图像中有太多噪声，但我假设您将深度转换为颜色的渐变函数被奇怪地缩放并且非线性强调噪声，因为标准深度相机没有如此大的噪音）所以平坦区域的法线应该是非常常见的，因此它应该由直方图中的峰值表示。首先，我们应该将 3D 法线转换为单个整数值（为了简化代码），并将可能的方向限制为仅一些固定方向（以排除噪声并压缩输出数据大小）。
归一化法线是坐标在
<-1,+1>范围内的 3D 向量，因此我们可以将该范围转换为某个位宽的无符号整数，并将其中的 3 个叠加成单个整数。例如每个坐标5bit：normal = (nx,ny,nz) nx = 31.0*(nx+1.0)*0.5; // <-1,+1> -> <0,31> ny = 31.0*(nx+1.0)*0.5; // <-1,+1> -> <0,31> nz = 31.0*(nx+1.0)*0.5; // <-1,+1> -> <0,31> int ni = int(nx) + int(ny)<<5 + int(nz)<<10所以任何方向都将转换为整数
<0,2^15)所以<0,32767>这很容易管理。因此，只需创建一些数组int his[32768]并将其所有值重置为零即可。然后对于每个像素，将其法线转换为这样的整数ni并增加其计数器his[ni]++；当心如果你有高分辨率的图像，你需要记住计数器可能溢出。因此，对于 32 位 int，分辨率必须小于 2^31 像素...否则您可以使用更大的位深度或添加条件来避免递增已经达到最大值。删除正常出现率非常低的点
这将使我们能够忽略图像中的小表面或不够平坦的部分。我们可以将这些点设置为一些特定的法线，例如
(0,0,0)或者有一个带有标志的单独数组。找到具有相同法线的足够大的连续区域
你可以在数据中暴力搜索一个框（你的无人机的大小 + 一些边距）（所以 2 个嵌套 for 循环用于位置，另外 2 个用于测试）如果所有点都具有相同的法线，你发现可能着陆区。
正常
n1,n2s 是“相同的”，如果：dot(n1,n2)/(|n1|.|n2|) >= cos(margin)允许边距的地方 angular
deg或rad之间的正常差异取决于您的cos实施。
[Edit1] 您的数据对于正常方法而言噪音太大
这里的图像（在对数据进行平滑处理以减少噪声后）显示了各个法线方向（this gradient）的出现，因此蓝色出现率最低，红色最高，绿色介于两者之间...
如您所见，出现次数最多的是像素超出范围或无效值（很难说）以及正确测量的方框顶部。然而，你的视图的大部分表面都有奇怪的非常大的噪音（在两个振幅上d radius ...那些斑点），我以前从未在深度图像上看到过。我敢打赌你从原始深度到“颜色”渐变的转换是非线性的并且强调噪声（这很可能也是我很难将你的场景重建回 3D 的原因）+ JPEG 有损压缩伪影。
因此，除非您为像素颜色和深度之间的转换提供明确的方程式，否则这是行不通的。因此，您需要不同的较慢的方法。我会改为 window“暴力”搜索：
定义一个window
所以你的无人机有一些尺寸，所以我们需要一些定义尺寸的矩形（比你的无人机在地面上的投影面积略大，多少取决于着陆轨迹和飞行控制稳定性和精度......）
所以我们需要一些大小为
wx * wy的矩形（以米为单位）（或者您重建场景时使用的任何单位）。测试 window
的“所有”可能位置所以只需从左上角开始测试，如果我们的点云的 window 足够平坦，如果是，你找到了你的解决方案，如果没有，测试位置稍微向右移动......直到到达点云的末端.然后从左边重新开始，但稍微移动到底部。所以这将只是 2 个嵌套的循环，递增的值小于 window 大小（不需要只是像素）
每个window位置检测区域是否平坦
所以得到window每个角的平均位置。这将定义一个平面，因此通过计算它们与平面的垂直距离来简单地测试 window 中的所有点。如果大于某个阈值，则该区域不够平坦。如果所有点都正常，那么您就找到了着陆区。
这里是预览：
这种方法的结果。红色表示无效颜色（原始图像上为黑色），绿色是第一个找到的着陆区。我修改了正常方法的代码来执行此操作，因此有相当多的代码残余不再需要，例如颜色，法线等...您只需要点云点位置...这里是相关的 C++/OpenGL 代码:
第一个简单的点云深度图像class:
const int _DepthImage_invert_z =0x00000001; // brighter colros are closer
const int _DepthImage_cos_correct_z =0x00000002; // depth is perpendicular distance instead of euclid
const int _DepthImage_smooth =0x00000004; // average position to reduce noise
class DepthPoint
{
public:
double pos[3]; // position (x,y,z)
double nor[3]; // normal vector (x,y,z)
double col[3]; // color (r,g,b)
DepthPoint(){};
DepthPoint(DepthPoint& a) { *this=a; }
~DepthPoint(){};
DepthPoint* operator = (const DepthPoint *a) { *this=*a; return this; }
//DepthPoint* operator = (const DepthPoint &a) { ...copy... return this; }
};
class DepthImage
{
public:
// resoluton
int xs,ys; // resolution
// point cloud ys x xs
DepthPoint **pnt; // pnt[y][x] point for each pixel(x,y) of image
// depth camera properties
double depthFOVx,depthFOVy, // [rad] FOV angles
depthx0,depthy0, // [pixels] FOV offset
depthz0,depthz1; // [m] intensity <0,255> -> distance <z0,z1>
int depthcfg; // configuration flags
double error; // invalid depth
// color camera properties
double colorFOVx,colorFOVy, // [rad] FOV angles
colorx0,colory0; // [pixels] FOV offset
int colorcfg; // configuration flags
DepthImage()
{
pnt=NULL; _free();
depthFOVx=86.0*deg; colorFOVx=86.0*deg;
depthFOVy=57.0*deg; colorFOVy=57.0*deg;
depthx0=0.0; colorx0=0.0;
depthy0=0.0; colory0=0.0;
depthz0=0.0;
depthz1=6.0;
depthcfg=0; colorcfg=0;
}
DepthImage(DepthImage& a) { *this=a; }
~DepthImage() { _free(); }
DepthImage* operator = (const DepthImage *a) { *this=*a; return this; }
//DepthImage* operator = (const DepthImage &a) { ...copy... return this; }
void _free()
{
if (pnt)
{
if (pnt[0]) delete[] pnt[0];
delete[] pnt;
}
pnt=NULL; xs=ys=0;
}
void load(AnsiString _depth,AnsiString _color)
{
_free();
Graphics::TBitmap *depth,*color;
int i,x,y,*pd,*pc;
double p[3],q[3],a,b,r;
// load depth image
depth=new Graphics::TBitmap;
picture_load(depth,_depth,NULL);
depth->HandleType=bmDIB;
depth->PixelFormat=pf32bit;
xs=depth->Width;
ys=depth->Height;
// allocate PCL
pnt =new DepthPoint*[ ys];
pnt[0]=new DepthPoint [xs*ys];
for (y=0;y<ys;y++) pnt[y]=pnt[0]+(y*xs);
// depth -> PCL
error=depthz1*0.99;
for (y=0;y<ys;y++)
{
pd=(int*)depth->ScanLine[y]; // pointer to y-th scan line in depth image
b=depthFOVy*(((double(y)-depthy0)/double(ys-1))-0.5); // latitude
for (x=0;x<xs;x++)
{
a=depthFOVx*(((double(x)-depthx0)/double(xs-1))-0.5); // longitude
r=pd[x]&255; // RGB from drepth (just single channel
r/=255.0; // linear scale
if (int(depthcfg&_DepthImage_invert_z)) r=1.0-r;
r=depthz0+(r*(depthz1-depthz0)); // range <z0,z1>
// spherical -> cartesian
p[0]=cos(a)*cos(b); // x
p[1]=sin(a)*cos(b); // y
p[2]= sin(b); // z
if (int(depthcfg&_DepthImage_cos_correct_z)) r/=p[0];
// scale and reorder coordinates to match my view
pnt[y][x].pos[0]=r*p[1];
pnt[y][x].pos[1]=r*p[2];
pnt[y][x].pos[2]=r*p[0];
}
}
// smooth positions a bit to reduce noise (FIR averaging smooth/blur filter)
if (int(depthcfg&_DepthImage_smooth))
for (i=0;i<10;i++)
{
for (y=1;y<ys;y++)
for (x=1;x<xs;x++)
{
vector_mul(p,pnt[y][x].pos,3.0);
vector_add(p,p,pnt[y][x-1].pos);
vector_add(p,p,pnt[y-1][x].pos);
vector_mul(pnt[y][x].pos,p,0.2);
}
for (y=ys-1;y>0;y--)
for (x=xs-1;x>0;x--)
{
vector_mul(p,pnt[y-1][x-1].pos,3.0);
vector_add(p,p,pnt[y-1][x].pos);
vector_add(p,p,pnt[y][x-1].pos);
vector_mul(pnt[y][x].pos,p,0.2);
}
}
// compute normals
for (y=1;y<ys;y++)
for (x=1;x<xs;x++)
{
vector_sub(p,pnt[y][x].pos,pnt[y][x-1].pos);// p = pnt[y][x] - pnt[y][x-1]
vector_sub(q,pnt[y][x].pos,pnt[y-1][x].pos);// q = pnt[y][x] - pnt[y-1][x]
vector_mul(p,q,p); // p = cross(q,p)
vector_one(pnt[y][x].nor,p); // nor[y][x] = p/|p|
}
// copy missing edge normals
for (y=1;y<ys;y++) vector_copy(pnt[y][0].nor,pnt[y][1].nor);
for (x=0;x<xs;x++) vector_copy(pnt[0][x].nor,pnt[1][x].nor);
// set color
for (y=0;y<ys;y++)
for (x=0;x<xs;x++)
{
DepthPoint *p=&pnt[y][x];
vector_ld(p->col,0.5,0.5,0.5);
if (p->pos[2]>=error) vector_ld(p->col,1.0,0.0,0.0); // invalid depths as red
}
delete depth;
}
bool find_flat_rect(double* &p0,double* &p1,double* &p2,double* &p3,double wx,double wy,double dmax)
{
int i,x,y,x0,y0,x1,y1,dx=1,dy=1;
double d,p[3],n[3];
// whole image
for (y0=0;y0<ys;y0+=dx)
for (x0=0;x0<xs;x0+=dy)
{
dx=1; dy=1;
// first coorner
p0=pnt[y0][x0].pos; if (p0[2]>=error) continue;
// find next coorner wx distant to p0
for (x1=x0+1;x1<xs;x1++)
{
p1=pnt[y0][x1].pos; if (p1[2]>=error) continue;
vector_sub(p,p1,p0); // p = p1-p0
d=vector_len(p); // d = |p|
if (d>=wx) break;
} if (x1>=xs){ x0=xs; continue; }
// find next coorner wy distant to p0
for (y1=y0+1;y1<ys;y1++)
{
p3=pnt[y1][x0].pos; if (p3[2]>=error) continue;
vector_sub(p,p3,p0); // p = p3-p0
d=vector_len(p); // d = |p|
if (d>=wy) break;
} if (y1>=ys){ p0=p1=p2=p3=NULL; return false; }
// diagonal coorner
p2=pnt[y1][x1].pos;
// window position increments are 1/4 of window size
dx=1+((x1-x0)>>2);
dy=1+((y1-y0)>>2);
// plane normal
vector_sub(p,p1,p0); // p = p1-p0
vector_sub(n,p3,p0); // n = p3-p0
vector_mul(n,n,p); // n = cross(n,p)
vector_mul(n,n); // n = n/|n|
// test distances to plane
i=1;
for (y=y0;y<=y1;y++)
for (x=x0;x<=x1;x++)
{
if (pnt[y][x].pos[2]>=error) continue;
vector_sub(p,pnt[y][x].pos,p0); // p = pnt[y][x] - p0
d=fabs(vector_mul(p,n)); // d = |dot(p,n)| ... perpendicular distance
if (d>dmax){ i=0; x=x1+1; y=y1+1; break; }
}
if (i) return true;
}
p0=p1=p2=p3=NULL;
return false;
}
void gldraw()
{
int x,y;
DepthPoint *p;
for (y=1;y<ys;y++)
{
glBegin(GL_QUAD_STRIP);
for (x=0;x<xs;x++)
{
p=&pnt[y-1][x];
glColor3dv(p->col);
glNormal3dv(p->nor);
glVertex3dv(p->pos);
p=&pnt[y][x];
glColor3dv(p->col);
glNormal3dv(p->nor);
glVertex3dv(p->pos);
}
glEnd();
}
glColor3f(1.0,1.0,1.0);
}
};
//---------------------------------------------------------------------------
和用法：
DepthImage pcl; // pointcloud
double *q0,*q1,*q2,*q3; // pointers to landing zone rectangle points
pcl.depthz0=0.8; // some constants for reconstruction
pcl.depthz1=1.00;
pcl.depthcfg=
_DepthImage_invert_z
|_DepthImage_cos_correct_z
;
pcl.load("depth.jpg",""); // this creates the pointcloud from image
pcl.find_flat_rect(q0,q1,q2,q3,0.40,0.25,0.005); // this finds a rectangle of 0.4x0.25 m size and 0.005m deviation from flat surface (return true if found)
// the q0,q1,q2,q3 are pointers to 3D points of the found rectangle or NULL if no such has been found
图像加载器来自我的 OpenGL 引擎（部分基于 VCL 和 PNGDelphi），因此您需要使用环境中的任何内容。
唯一重要的东西（来自 3D 重建的应用程序）是函数 find_flat_rect，它完全按照此方法中的描述工作。
唯一要添加的是测试着陆区的坡度不是太大（通过平面法线 n 和向下矢量（来自加速度计或陀螺仪）之间的点积）。如果你的无人机总是在水平方向上，您可以使用 z 轴。
这里将3D点转换为2D深度图坐标的函数为：
void DepthImage::project(double &x,double &y,double* p) // p[3] -> (x,y) ... 3D to 2D perspective projection
{
x=atan2(p[0],p[2])/depthFOVx;
y=atan2(p[1],p[2])/depthFOVy;
if (int(depthcfg&_DepthImage_cos_correct_z))
y*=(double(ys)*depthFOVx)/(depthFOVy*double(xs));
x=(x+0.5)*xs;
y=(y+0.5)*ys;
}
然而，由于 3D 重建不完美（由于使用了未知的深度编码），这不是很准确，因此结果略有失真。当心结果可能略微超出图像范围，因此如果您想将其用于像素访问，则需要将其限制为 <0,xs) 和 <0,ys) ...