我有一架可以自主飞行的无人机,我希望它安全着陆。我在它下面有一个深度摄像头,现在我找不到一种方法来检测平坦区域来告诉无人机降落在上面。我正在使用 realsense D435i 相机..这是来自向下相机的深度图像样本。
你能帮忙吗?
我有一架可以自主飞行的无人机,我希望它安全着陆。我在它下面有一个深度摄像头,现在我找不到一种方法来检测平坦区域来告诉无人机降落在上面。我正在使用 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
但是,如果使用线性输出或透视进行后处理,我看不到有关深度帧的信息。如果物体随着距离变小,您可以从图像中推断出它仍然是透视我不知道但会假设它的情况。
你的图像是 848x480,所以我假设它只是线性缩放,1/1.5
所以 FOV 是相同的。
将深度线性化为规则网格采样点/网格
因此每个像素都必须转换为相对于相机的 3D 位置。因此,只需从相机焦点投射光线穿过像素并在其距离处停止。结果位置就是我们想要的 3D 位置。对每个点执行此操作将生成连接点的 2D 网格(表面网格)。但是我们不知道深度是欧几里得还是垂直距离(已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,z
,并且:
pcl.depthz0=0.8;
pcl.depthz1=1.00;
pcl.depthcfg=_DepthImage_invert_z|_DepthImage_cos_correct_z;
图像分辨率为xs,ys
。
有关更多信息,请参阅:
从其邻居计算每个点(表面)的法线
简单的叉积就可以了,但是使用正确的操作数顺序很重要,因此得到的法线指向相同的方向而不是相反的……否则我们稍后需要使用结果来处理abs
它dot
。
在法线上做一个直方图(与有限的方向对齐)
直方图是变量的某个值与其概率或发生率之间的关系。例如,图像直方图告诉我们每种颜色(范围/箱)有多少像素被着色。出于我们的目的,平坦区域(平面)应该具有相同的法线(在您的深度图像中噪声太多,但我假设您将深度转换为颜色的梯度函数被奇怪地缩放并且非线性强调噪声,因为标准深度相机没有如此大的噪音)所以平坦区域的法线应该是非常出现的,因此它应该由直方图中的峰值表示。首先,我们应该将 3D 法线转换为单个整数值(以简化代码),并将可能的方向限制为仅一些固定的方向(以排除噪声并压缩输出数据大小)。
归一化法线是坐标在范围内的 3D 向量,<-1,+1>
因此我们可以将该范围转换为某个位宽的无符号整数,并将其中的 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 个用于测试),如果所有点的法线相同,则您找到可能的着陆区。
normal n1,n2
s 是“相同的”,如果:
dot(n1,n2)/(|n1|.|n2|) >= cos(margin)
其中,margin 是允许的法线之间的角度差异,deg
或者rad
取决于您的cos
实现。
[Edit1] 您的数据对于正常方法来说太嘈杂了
此处的图像(在对数据进行平滑以减少噪声之后)显示了各个法线方向(具有此梯度)的出现,因此蓝色是最低出现率,红色最高,绿色介于两者之间......
如您所见,最常见的像素超出范围或无效值(很难说)以及正确测量的框顶部。但是,您视图的大部分表面都有奇怪的非常大的噪声(幅度和半径......那些斑点),这是我以前在深度图像上从未见过的。我敢打赌,您从原始深度到“颜色”渐变的转换是非线性的并且强调噪声(这很可能也是我很难将您的场景重建回 3D 的原因)+ JPEG 有损压缩伪影。
因此,除非您为像素颜色和深度之间的转换提供明确的方程式,否则这是不行的。因此,您需要不同的较慢方法。我会改为进行窗口“蛮力”搜索:
定义一个窗口
所以你的无人机有一定的尺寸,所以我们需要一些定义尺寸的矩形(比你的无人机投射在地面上的区域略大,多少取决于着陆轨迹和飞行控制的稳定性和精度......)
所以我们需要一些wx * wy
以米为单位的矩形(或者你重建场景的任何单位)。
测试窗口的“所有”可能位置
因此,只需从左上角开始测试,如果我们的点云的窗口足够平,如果是,您找到了解决方案,如果没有,则测试位置稍微向右移动......直到到达点云的末端。然后从左边重新开始,但稍微移动到底部。所以这将只是 2 个嵌套的 for 循环,增量小于窗口大小(不需要只是像素)
对于每个窗口位置检测该区域是否平坦
因此获得窗口每个角的平均位置。这将定义一个平面,因此只需通过计算它们与平面的垂直距离来测试窗口中的所有点。如果大于某个阈值,则该区域不够平坦。如果所有点都正常,那么你找到了你的着陆区。
这里有一个预览:
这种方法的结果。红色表示无效颜色(原始图像上的黑色),绿色是第一个找到的着陆区。我修改了正常方法的代码来执行此操作,因此有一些不再需要的代码残余,例如颜色、法线等...您只需要点云点位置...这里是相关的 C++/OpenGL 代码:
第一个简单的深度图像到点云类:
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)
如果您想将其用于像素访问......