How to merge multiple point clouds to construct a 3D map?
<p>I have succeeded in extracting a point cloud using a Kinect, but I cannot go further to save it or to add to it the next captured frame. Here is what I have found so far, and I would love to enhance it so that I can store many point clouds in one file to have a big 3D map.</p>

<pre><code>    #include "main.h"
    #include "glut.h"

    #include &lt;cmath&gt;
    #include &lt;cstdio&gt;

    #include &lt;Windows.h&gt;
    #include &lt;Ole2.h&gt;

    #include &lt;Kinect.h&gt;





    // 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-&gt;get_CoordinateMapper(&amp;mapper);

            sensor-&gt;Open();
            sensor-&gt;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-&gt;get_DepthFrameReference(&amp;frameref);
        frameref-&gt;AcquireFrame(&amp;depthframe);
        if (frameref) frameref-&gt;Release();

        if (!depthframe) return;

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

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

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

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

        if (!colorframe) return;

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

        // Write color array for vertices
        float* fdest = (float*)dest;
        for (int i = 0; i &lt; width*height; i++) {
            ColorSpacePoint p = depth2rgb[i];
            // Check if color pixel coordinates are in bounds
            if (p.X &lt; 0 || p.Y &lt; 0 || p.X &gt; colorwidth || p.Y &gt; 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-&gt;Release();
    }

    void getKinectData() {
        IMultiSourceFrame* frame = NULL;
        if (SUCCEEDED(reader-&gt;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-&gt;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;
    }
</code></pre>