Fatal编程技术网
  • opengl/
  • 基于openGL的粒子滤波扫描线 我在C++中实现了一个用于三维立方体跟踪的粒子滤波。

基于openGL的粒子滤波扫描线 我在C++中实现了一个用于三维立方体跟踪的粒子滤波。

opengl 3d

基于openGL的粒子滤波扫描线 我在C++中实现了一个用于三维立方体跟踪的粒子滤波。,opengl,3d,particle-filter,scanline,Opengl,3d,Particle Filter,Scanline,我在为粒子指定权重时遇到了一个重大问题，因为权重是基于采样点和像素之间的距离误差（在本例中）（解释如下） 目前，我能够从笔记本电脑上的摄像头中获取视频流，将其显示在屏幕上，并在上面绘制粒子。此外，我还能够提取从相机检测到的物体的边缘，并找到它们的轮廓坐标 以下是如何检测轮廓坐标： vector<vector<Point>> detectContours(Mat image){ vector<vector<Point>> contours

我在为粒子指定权重时遇到了一个重大问题，因为权重是基于采样点和像素之间的距离误差（在本例中）（解释如下）

目前，我能够从笔记本电脑上的摄像头中获取视频流，将其显示在屏幕上，并在上面绘制粒子。此外，我还能够提取从相机检测到的物体的边缘，并找到它们的轮廓坐标

以下是如何检测轮廓坐标：

vector<vector<Point>> detectContours(Mat image){

    vector<vector<Point>> contoursEdges;
    vector<Vec4i> hierarchy;

    Mat clone;
    clone = image.clone();

    Mat contours;
    GaussianBlur(clone, contours, Size(5,5), 1);

    cvtColor(contours, contours, CV_BGR2GRAY);

    Mat cannyEdges;
    Canny(contours, cannyEdges, 50, 100, 3);

    try{
        findContours(cannyEdges, contoursEdges, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE, Point(0,0));
    }catch(exception e){
        printf("Exception: Contours cannot be found!!!\n");
    }

    return contoursEdges;
}

矢量检测孔（Mat图像）{
矢量等高莎草；
向量层次；
无性系；
clone=image.clone（）；
垫状轮廓；
GaussianBlur（克隆，轮廓，大小（5,5），1）；
CVT颜色（轮廓、轮廓、CV_bgr2灰色）；
马口铁；
坎尼（等高线，坎尼边缘，50，100，3）；
试一试{
findContours（cannyEdges、contoursEdges、CV_RETR__外部、CV_CHAIN_近似无、点（0,0））；
}捕获（例外e）{
printf（“异常：找不到等高线！！！\n”）；
}
回归莎草；
}
但是，正如我所说，粒子被绘制在视频流的顶部（如屏幕截图所示）。每个平面被划分为一个网格

下面是如何做到的：

void DrawBox(GLfloat fWidth,GLfloat fHeight,GLfloat fDepth,GLint wslices,GLint dslices,GLint stacks, GLfloat scanSize)
{

glPushAttrib(GL_POLYGON_BIT | GL_ENABLE_BIT | GL_COLOR_BUFFER_BIT) ;

glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;

glEnable(GL_CULL_FACE); // allow only front surfaces to be visible

int iTopButtonQuads = wslices * dslices * 2;
int iLeftRightQuads = dslices * stacks * 2;
int iFrontBackQuads = wslices * stacks * 2;

float* pfVertices = new float[(iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 3 * 4];
float* pfColors = new float[(iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 3 * 4];
float* pfNormals = new float[(iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 3 * 4];

int iVertexIndex = 0;

GLfloat Xstep = fWidth / wslices;
GLfloat Ystep = fHeight / stacks;
GLfloat Zstep = fDepth / dslices;

GLfloat firstX = fWidth / 2.0f;
GLfloat firstY = fHeight / 2.0f;
GLfloat firstZ = fDepth / 2.0f;

GLfloat currX = 0.0f;
GLfloat currY = 0.0f;
GLfloat currZ = 0.0f;

GLfloat x_status = 0.0f;
GLfloat y_status = 0.0f;
GLfloat z_status = 0.0f;

// the bottom and the top of the box
for (currZ = -firstZ, z_status = 0.0f; currZ < firstZ - Zstep / 2.0f; currZ += Zstep, z_status += Zstep)
{
    for (currX = -firstX, x_status = 0.0f; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep)
    {
        int iCurrentIndex = iVertexIndex * 3 * 4;

        float pfNormal[3] = { 0.0f, -1.0f, 0.0f };

        memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);

        float pfColor[3] = { 1.0f, 0.0f, 0.0f };

        memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);

        float pfVertex0[3] = {currX,-firstY,currZ};
        float pfVertex1[3] = {currX + Xstep,-firstY,currZ};
        float pfVertex2[3] = {currX + Xstep,-firstY,currZ + Zstep};
        float pfVertex3[3] = {currX,-firstY,currZ + Zstep};

        memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);

        iVertexIndex++;
    }

    for (currX = -firstX, x_status = 0.0f; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep)
    {
        int iCurrentIndex = iVertexIndex * 3 * 4;

        float pfNormal[3] = { 0.0f, 1.0f, 0.0f };

        memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);

        float pfColor[3] = { 0.0f, 1.0f, 0.0f };

        memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);

        float pfVertex0[3] = {currX + Xstep,firstY,currZ + Zstep};
        float pfVertex1[3] = {currX + Xstep,firstY,currZ};
        float pfVertex2[3] = {currX,firstY,currZ};
        float pfVertex3[3] = {currX,firstY,currZ + Zstep};

        memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);

        iVertexIndex++;
    }
}

// the front and the back of the box
for (currY = -firstY, y_status = 0.0f; currY < firstY - Ystep / 2.0f ; currY += Ystep, y_status += Ystep)
{
    for (currX = -firstX, x_status = 0.0f; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep)
    {
        int iCurrentIndex = iVertexIndex * 3 * 4;

        float pfNormal[3] = { 0.0f, 0.0f, 1.0f };

        memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);

        float pfColor[3] = { 0.0f, 0.0f, 1.0f };

        memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);

        float pfVertex0[3] = {currX,currY,firstZ};
        float pfVertex1[3] = {currX + Xstep,currY,firstZ};
        float pfVertex2[3] = {currX + Xstep,currY + Ystep,firstZ};
        float pfVertex3[3] = {currX,currY + Ystep,firstZ};

        memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);

        iVertexIndex++;
    }

    for (currX = -firstX, x_status = 0.0f; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep)
    {
        int iCurrentIndex = iVertexIndex * 3 * 4;

        float pfNormal[3] = { 0.0f, 0.0f, -1.0f };

        memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);

        float pfColor[3] = { 0.0f, 1.0f, 1.0f };

        memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);

        float pfVertex0[3] = {currX + Xstep,currY + Ystep,-firstZ};
        float pfVertex1[3] = {currX + Xstep,currY,-firstZ};
        float pfVertex2[3] = {currX,currY,-firstZ};
        float pfVertex3[3] = {currX,currY + Ystep,-firstZ};

        memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);

        iVertexIndex++;
    }
}

// Right side and the left side of the box
for (currY = -firstY, y_status = 0.0f; currY < firstY - Ystep / 2.0f; currY += Ystep, y_status += Ystep)
{
    for (currZ = -firstZ, z_status = 0.0f; currZ < firstZ - Zstep / 2.0f; currZ += Zstep, z_status += Zstep)
    {
        int iCurrentIndex = iVertexIndex * 3 * 4;

        float pfNormal[3] = { 1.0f, 0.0f, 0.0f };

        memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);

        float pfColor[3] = { 1.0f, 0.0f, 1.0f };

        memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);

        float pfVertex0[3] = {firstX,currY,currZ};
        float pfVertex1[3] = {firstX,currY + Ystep,currZ};
        float pfVertex2[3] = {firstX,currY + Ystep,currZ + Zstep};
        float pfVertex3[3] = {firstX,currY,currZ + Zstep};

        memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);

        iVertexIndex++;
    }

    for (currZ = -firstZ, z_status = 0.0f; currZ < firstZ - Zstep / 2.0f; currZ += Zstep, z_status += Zstep)
    {
        int iCurrentIndex = iVertexIndex * 3 * 4;

        float pfNormal[3] = { -1.0f, 0.0f, 0.0f };

        memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
        memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);

        float pfColor[3] = { 1.0f, 1.0f, 0.0f };

        memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
        memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);

        float pfVertex0[3] = {-firstX,currY,currZ};
        float pfVertex1[3] = {-firstX,currY,currZ + Zstep};
        float pfVertex2[3] = {-firstX,currY + Ystep,currZ + Zstep};
        float pfVertex3[3] = {-firstX,currY + Ystep,currZ};

        memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
        memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);

        iVertexIndex++;
    }
}

glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);

glColorPointer(3, GL_FLOAT, 0, (void*)pfColors);
glNormalPointer(GL_FLOAT, 0, (void*)pfNormals);
glVertexPointer(3, GL_FLOAT, 0, (void*)pfVertices);

glDrawArrays(GL_QUADS, 0, (iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 4);

glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
glDisableClientState(GL_NORMAL_ARRAY);

// Get coord of first vertex
float fMinX = pfVertices[0];
float fMinY = pfVertices[1];
float fMinZ = pfVertices[2];

float fMaxX = pfVertices[0];
float fMaxY = pfVertices[1];
float fMaxZ = pfVertices[2];

for (int iVertexIndex = 0; iVertexIndex < (iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 4; iVertexIndex++)
{
    int iCurrentIndex = iVertexIndex * 3; // (x y z) per vertex

    if (pfVertices[iCurrentIndex] < fMinX)
        fMinX = pfVertices[iCurrentIndex];

    if (pfVertices[iCurrentIndex + 1] < fMinY)
        fMinY = pfVertices[iCurrentIndex + 1];

    if (pfVertices[iCurrentIndex + 2] < fMinZ)
        fMinZ = pfVertices[iCurrentIndex + 2];

    if (pfVertices[iCurrentIndex] > fMaxX)
        fMaxX = pfVertices[iCurrentIndex];

    if (pfVertices[iCurrentIndex + 1] > fMaxY)
        fMaxY = pfVertices[iCurrentIndex + 1];

    if (pfVertices[iCurrentIndex + 2] > fMaxZ)
        fMaxZ = pfVertices[iCurrentIndex + 2];
}

// Create an axes aligned bounding box 
// by simply drawing inflated min-maxes, that we collect
// example of using indexed primitives

glDisable(GL_CULL_FACE);

GLfloat vertices[] = {
    fMinX - scanSize, fMaxY + scanSize, fMaxZ + scanSize,
    fMaxX + scanSize, fMaxY + scanSize, fMaxZ + scanSize,
    fMaxX + scanSize, fMinY - scanSize, fMaxZ + scanSize,
    fMinX - scanSize, fMinY - scanSize, fMaxZ + scanSize,
    fMinX - scanSize, fMaxY + scanSize, fMinZ - scanSize,
    fMaxX + scanSize, fMaxY + scanSize, fMinZ - scanSize,
    fMaxX + scanSize, fMinY - scanSize, fMinZ - scanSize,
    fMinX - scanSize, fMinY - scanSize, fMinZ - scanSize
};

GLint indices[] = {
    0, 1, 2, 3,
    4, 5, 1, 0,
    3, 2, 6, 7,
    5, 4, 7, 6,
    1, 5, 6, 2,
    4, 0, 3, 7 
};

glColor3f(1.0f, 1.0f, 1.0f);

glEnableClientState(GL_VERTEX_ARRAY);

glVertexPointer(3, GL_FLOAT, 0, (void*)vertices);

glDrawElements(GL_QUADS, 24, GL_UNSIGNED_INT, (void*)indices);

glDisableClientState(GL_VERTEX_ARRAY);

glEnable(GL_CULL_FACE);

delete [] pfVertices;
delete [] pfNormals;
delete [] pfColors;

glPopAttrib() ;
} 

void DrawBox（GLfloat fWidth、GLfloat FHIGHT、GLfloat fDepth、GLfloat wslices、GLfloat dslices、GLfloat堆栈、GLfloat SCANSSIZE）
{
glPushAttrib（GL_多边形_位| GL_启用_位| GL_颜色_缓冲_位）；
glPolygonMode（GLU前、后、GLU线）；
glEnable（GL_CULL_FACE）；//仅允许前表面可见
int iTopButtonQuads=wslices*dslices*2；
int iLeftRightQuads=dslices*stacks*2；
int iFrontBackQuads=wslices*stacks*2；
float*pfVertices=newfloat[（iTopButtonQuads+iLeftRightQuads+iFrontBackQuads）*3*4]；
float*pfColors=新的float[（iTopButtonQuads+iLeftRightQuads+iFrontBackQuads）*3*4]；
float*pfNormals=新的float[（iTopButtonQuads+iLeftRightQuads+iFrontBackQuads）*3*4]；
int-index=0；
GLfloat Xstep=fWidth/wslices；
GLfloat Ystep=fh八个/堆叠；
GLfloat Zstep=fDepth/dslices；
GLfloat firstX=fWidth/2.0f；
GLfloat firstY=fHight/2.0f；
GLfloat firstZ=fDepth/2.0f；
GLX=0.0f；
GLfloat咖喱=0.0f；
GLfloat currZ=0.0f；
GLX_状态=0.0f；
GLfloat y_状态=0.0f；
GLfloat z_状态=0.0f；
//盒子的底部和顶部
对于（currZ=-firstZ，z_状态=0.0f；currZ