1. 技术文章
  2. Scan Lines for Particle Filter with openGL

Scan Lines for Particle Filter with openGL

由 江枫思渺然 提交于 2019-12-10 17:56:20

问题


I am implementing a Particle Filter for a 3D cube tracking in C++.

I faced a significant problem when assigning weights for the particles, because weights are based on the error in distance (in this case) between a sample point and a pixel (explained below).

Currently I am able to get a video stream from the camera on my laptop, display it on a screen and draw particles on top of that. Also I am able to extract edges of objects detected from the camera and find their contours' coordinates.

Screen shot

Here is how contour coordinates are detected:

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;
}

However, as I said particles are drawn on top of the video stream (as shown on the screen shot). Each plane is divided into a grid.

Here is how it's done:

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() ;
}

My problem now is the following:

  • Collect only boundary points of all six grids for each particle (sample points)
  • Draw a scan line from each such point with a size, say, 20 pixels in both directions from the point

Second bullet point is required in order to find the distance between sample points and detected edges as shown in the image below.

This is simplified version in 2D. Each green line represents a scan line and 'Dist' is the distance between a sample point and a pixel found in an edge

If somebody can suggest a solution how to find the distance or preferably could post some code snippets it will be highly appreciated.

来源：https://stackoverflow.com/questions/33152554/scan-lines-for-particle-filter-with-opengl

标签
opengl
3d
particle-filter
scanline
易学教程内所有资源均来自网络或用户发布的内容，如有违反法律规定的内容欢迎反馈！
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!