title: How to merge multiple point clouds to construct a 3D map?
body: <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>