Блог

Мне удалось извлечь облако точек с помощью Kinect, но я не могу пойти дальше, чтобы сохранить его или добавить к нему следующий захваченный кадр. Вот что я нашел до сих пор, и я хотел бы улучшить его, чтобы я мог хранить много облаков точек в одном файле, чтобы иметь большую 3D-карту.

     #include "main.h"
    #include "glut.h"

    #include <cmath>
    #include <cstdio>

    #include <Windows.h>
    #include <Ole2.h>

    #include <Kinect.h>





    // We'll be using buffer objects to store the kinect point cloud
    GLuint vboId;
    GLuint cboId;

    // Intermediate Buffers
    unsigned char rgbimage[colorwidth*colorheight*4];    // Stores RGB color image
    ColorSpacePoint depth2rgb[width*height];             // Maps depth pixels to rgb pixels
    CameraSpacePoint depth2xyz[width*height];            // Maps depth pixels to 3d coordinates

    // Kinect Variables
    IKinectSensor* sensor;             // Kinect sensor
    IMultiSourceFrameReader* reader;   // Kinect data source
    ICoordinateMapper* mapper;         // Converts between depth, color, and 3d coordinates

    bool initKinect() {
        if (FAILED(GetDefaultKinectSensor(amp;sensor))) {
            return false;
        }
        if (sensor) {
            sensor->get_CoordinateMapper(amp;mapper);

            sensor->Open();
            sensor->OpenMultiSourceFrameReader(
                FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
                amp;reader);
            return reader;
        } else {
            return false;
        }
    }

    void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
        IDepthFrame* depthframe;
        IDepthFrameReference* frameref = NULL;
        frame->get_DepthFrameReference(amp;frameref);
        frameref->AcquireFrame(amp;depthframe);
        if (frameref) frameref->Release();

        if (!depthframe) return;

        // Get data from frame
        unsigned int sz;
        unsigned short* buf;
        depthframe->AccessUnderlyingBuffer(amp;sz, amp;buf);

        // Write vertex coordinates
        mapper->MapDepthFrameToCameraSpace(width*height, buf, width*height, depth2xyz);
        float* fdest = (float*)dest;
        for (int i = 0; i < sz; i  ) {
            *fdest   = depth2xyz[i].X;
            *fdest   = depth2xyz[i].Y;
            *fdest   = depth2xyz[i].Z;
        }

        // Fill in depth2rgb map
        mapper->MapDepthFrameToColorSpace(width*height, buf, width*height, depth2rgb);
        if (depthframe) depthframe->Release();
    }

    void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) {
        IColorFrame* colorframe;
        IColorFrameReference* frameref = NULL;
        frame->get_ColorFrameReference(amp;frameref);
        frameref->AcquireFrame(amp;colorframe);
        if (frameref) frameref->Release();

        if (!colorframe) return;

        // Get data from frame
        colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba);

        // Write color array for vertices
        float* fdest = (float*)dest;
        for (int i = 0; i < width*height; i  ) {
            ColorSpacePoint p = depth2rgb[i];
            // Check if color pixel coordinates are in bounds
            if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) {
                *fdest   = 0;
                *fdest   = 0;
                *fdest   = 0;
            }
            else {
                int idx = (int)p.X   colorwidth*(int)p.Y;
                *fdest   = rgbimage[4*idx   0]/255.;
                *fdest   = rgbimage[4*idx   1]/255.;
                *fdest   = rgbimage[4*idx   2]/255.;
            }
            // Don't copy alpha channel
        }

        if (colorframe) colorframe->Release();
    }

    void getKinectData() {
        IMultiSourceFrame* frame = NULL;
        if (SUCCEEDED(reader->AcquireLatestFrame(amp;frame))) {
            GLubyte* ptr;
            glBindBuffer(GL_ARRAY_BUFFER, vboId);
            ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
            if (ptr) {
                getDepthData(frame, ptr);
            }
            glUnmapBuffer(GL_ARRAY_BUFFER);
            glBindBuffer(GL_ARRAY_BUFFER, cboId);
            ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
            if (ptr) {
                getRgbData(frame, ptr);
            }
            glUnmapBuffer(GL_ARRAY_BUFFER);
        }
        if (frame) frame->Release();
    }

    void rotateCamera() {
        static double angle = 0.;
        static double radius = 3.;
        double x = radius*sin(angle);
        double z = radius*(1-cos(angle)) - radius/2;
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(x,0,z,0,0,radius/2,0,1,0);
        angle  = 0.002;
    }

    void drawKinectData() {
        getKinectData();
        rotateCamera();

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_COLOR_ARRAY);

        glBindBuffer(GL_ARRAY_BUFFER, vboId);
        glVertexPointer(3, GL_FLOAT, 0, NULL);

        glBindBuffer(GL_ARRAY_BUFFER, cboId);
        glColorPointer(3, GL_FLOAT, 0, NULL);

        glPointSize(1.f);
        glDrawArrays(GL_POINTS, 0, width*height);

        glDisableClientState(GL_VERTEX_ARRAY);
        glDisableClientState(GL_COLOR_ARRAY);
    }

    int main(int argc, char* argv[]) {
        if (!init(argc, argv)) return 1;
        if (!initKinect()) return 1;

        // OpenGL setup
        glClearColor(0,0,0,0);
        glClearDepth(1.0f);

        // Set up array buffers
        const int dataSize = width*height * 3 * 4;
        glGenBuffers(1, amp;vboId);
        glBindBuffer(GL_ARRAY_BUFFER, vboId);
        glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);
        glGenBuffers(1, amp;cboId);
        glBindBuffer(GL_ARRAY_BUFFER, cboId);
        glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);

        // Camera setup
        glViewport(0, 0, width, height);
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(45, width /(GLdouble) height, 0.1, 1000);
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();

        gluLookAt(0,0,0,0,0,1,0,1,0);

        // Main loop
        execute();
        return 0;
    }
  

Если вы рассматриваете только объединение облаков точек, этого можно легко достичь с помощью PCL, используя = оператор между облаками точек. На веб-сайте PCL есть небольшое руководство по этому вопросу, которое вы можете найти здесь .

С другой стороны, если вы ищете способ построения большой карты путем объединения и сшивания различных облаков точек, вам нужно будет найти набор пересекающихся объектов между облаками точек и преобразовать их так, чтобы они перекрывались в нужной области. Вы можете сделать это, создав алгоритм, основанный на итеративной ближайшей точке. Может быть интересно посмотреть на KinFu, который делает это в режиме реального времени и создает сетку из отсканированных облаков. Исходный код доступен на github проекта PCL.