void XtionDepthDriverImpl::onNewFrame(VideoStream &stream)
{
  VideoFrameRef ref;
  stream.readFrame(&ref);
  _lastCaptured = XtionDepthImage(ref.getData(), ref.getDataSize(),
    ref.getWidth(), ref.getHeight(), 0, this);
}

Status ClosestPoint::getNextData(IntPoint3D& closestPoint, VideoFrameRef& rawFrame)
{
	Status rc = m_pInternal->m_pDepthStream->readFrame(&rawFrame);
	if (rc != STATUS_OK)
	{
		printf("readFrame failed\n%s\n", OpenNI::getExtendedError());
	}

	DepthPixel* pDepth = (DepthPixel*)rawFrame.getData();
	bool found = false;
	closestPoint.Z = 0xffff;
	int width = rawFrame.getWidth();
	int height = rawFrame.getHeight();

	for (int y = 0; y < height; ++y)
		for (int x = 0; x < width; ++x, ++pDepth)
		{
			if (*pDepth < closestPoint.Z && *pDepth != 0)
			{
				closestPoint.X = x;
				closestPoint.Y = y;
				closestPoint.Z = *pDepth;
				found = true;
			}
		}

	if (!found)
	{
		return STATUS_ERROR;
	}

	return STATUS_OK;
}

int readOpenNiColorAndDepth(VideoStream &color , VideoStream &depth,VideoFrameRef &colorFrame,VideoFrameRef &depthFrame)
{
  #if USE_WAITFORANYSTREAM_TO_GRAB
   #warning "Please turn #define USE_WAITFORANYSTREAM_TO_GRAB 0"
   #warning "This is a bad idea taken from OpenNI2/Samples/SimpleViewer , we dont just want to update 'any' frame we really want to snap BOTH and do that sequentially"
   #warning "It is better to sequencially grab them instead of waiting for any stream a couple of times "
   openni::VideoStream** m_streams = new openni::VideoStream*[2];
   m_streams[0] = &depth;
   m_streams[1] = &color;


  unsigned char haveDepth=0,haveColor=0;
  int changedIndex;
  while ( (!haveDepth) || (!haveColor) )
  {
	openni::Status rc = openni::OpenNI::waitForAnyStream(m_streams, 2, &changedIndex);
	if (rc != openni::STATUS_OK)
	{
		fprintf(stderr,"Wait failed\n");
		return 0 ;
	}

  unsigned int i=0;

	switch (changedIndex)
	{
	case 0:
		depth.readFrame(&depthFrame);
		haveDepth=1;
    break;
	case 1:
		color.readFrame(&colorFrame);
		haveColor=1;
    break;
	default:
		printf("Error in wait\n");
		return 0;
	}
  }

	delete m_streams;
	return 1;
  #else
    //Using serial frame grabbing
    readFrameBlocking(depth,depthFrame,MAX_TRIES_FOR_EACH_FRAME); // depth.readFrame(&depthFrame);
    readFrameBlocking(color,colorFrame,MAX_TRIES_FOR_EACH_FRAME); // color.readFrame(&colorFrame);

    if(depthFrame.isValid() && colorFrame.isValid()) { return 1; }

    fprintf(stderr,"Depth And Color frames are wrong!\n");
  #endif
    return 0;

}

/**
 Reads a frame.

 @return If no frame can be read or the frame read is not valid returns false, otherwise returns true.
*/
bool Kinect::readFrame(cv::Mat &frame, CameraMode camMode)
{
	VideoFrameRef irf;
	int hIr, wIr;

	VideoFrameRef colorf; 
	int hColor, wColor;


	switch (camMode)
	{
		case (NI_SENSOR_DEPTH):

			rc = depth.readFrame(&irf);
			if (irf.isValid())
			{
				const uint16_t* imgBufIr = (const uint16_t*)irf.getData();
				hIr = irf.getHeight();
        		wIr = irf.getWidth();
        		frame.create(hIr, wIr, CV_16U);
        		memcpy(frame.data, imgBufIr, hIr * wIr * sizeof(uint16_t));
        		frame.convertTo(frame, CV_8U);
				return true;
			}
			else
			{
				cout << "ERROR: Frame not valid." << endl;
				return false;
			}

		case (NI_SENSOR_COLOR):

			rc = color.readFrame(&colorf);
			if(colorf.isValid())
			{
				const openni::RGB888Pixel* imgBufColor = (const openni::RGB888Pixel*)colorf.getData();
				hColor = colorf.getHeight();
        		wColor = colorf.getWidth();
        		frame.create(hColor, wColor, CV_8UC3);
        		memcpy(frame.data, imgBufColor,  3 * hColor * wColor * sizeof(uint8_t));
        		cvtColor(frame, frame, CV_BGR2RGB);
        		return true;
			}
			else
			{
				cout << "ERROR: Frame not valid." << endl;
				return false;
			}

		default:
			cout << "ERROR: No frame to be read. Object not initialize." << endl;
			return false;
	}
}

void KinectHelper::consume_color(VideoFrameRef frame){
    /// Fetch new color frame from the frame listener class

    /// Get color data from the frame just fetched
    const openni::RGB888Pixel* imageBuffer = (const openni::RGB888Pixel*) frame.getData();

    (*color_back_buffer) = QImage((uchar*) imageBuffer, frame.getWidth(), frame.getHeight(), QImage::Format_RGB888);

    _mutex.lock();
    qSwap(color_front_buffer, color_back_buffer);
    _mutex.unlock();
}

int readFrameBlocking(VideoStream &stream,VideoFrameRef &frame , unsigned int max_tries)
{
  unsigned int tries_for_frame = 0 ;
  while (  tries_for_frame < max_tries  )
          {
            stream.readFrame(&frame);
	        if (frame.isValid()) { return 1; }
	        ++tries_for_frame;
          }

  if (!frame.isValid()) { fprintf(stderr,"Could not get a valid frame even after %u tries \n",max_tries); return 0; }
  return (tries_for_frame<max_tries);
}

pcl::PointCloud<pcl::PointXYZ> cloud_xyz(VideoFrameRef frame,VideoStream &stream)
{
	pcl::PointCloud<pcl::PointXYZ> cloud;
    if(frame.getData() == NULL) std::cout<<"Empty data\n..";
    float px,py,pz;
    DepthPixel *depthp,dp;
    Status status;
    cloud.width = frame.getWidth();
    cloud.height = frame.getHeight();
    cloud.resize(cloud.height * cloud.width);
	int count=0;
	depthp = (DepthPixel*)((char*)frame.getData());
    for(int y=0;y<frame.getHeight();y++)
    {
        for(int x=0;x<frame.getWidth();x++)
        {
            dp=*depthp;
            status = CoordinateConverter::convertDepthToWorld(stream,x,y,dp,&px,&py,&pz);
            if(!handlestatus(status)) exit(0);
            cloud.points[x + (frame.getStrideInBytes() * y)/2].x = (px);
            cloud.points[x + (frame.getStrideInBytes() * y)/2].y = (py);
            cloud.points[x + (frame.getStrideInBytes() * y)/2].z = (pz);
            depthp++;
        }
    }
	return cloud;
}

std::stringstream cloud_xyz_comp(VideoFrameRef frame,VideoStream &stream,bool showstats)
{
	std::stringstream comp_data;
	pcl::PointCloud<pcl::PointXYZ> cloud;
    if(frame.getData() == NULL) std::cout<<"Empty data\n..";
    float px,py,pz;
    DepthPixel *depthp,dp;
    Status status;
    cloud.width = frame.getWidth();
    cloud.height = frame.getHeight();
    cloud.resize(cloud.height * cloud.width);
	int count=0;
	depthp = (DepthPixel*)((char*)frame.getData());
    for(int y=0;y<frame.getHeight();y++)
    {
        for(int x=0;x<frame.getWidth();x++)
        {
            dp=*depthp;
            status = CoordinateConverter::convertDepthToWorld(stream,x,y,dp,&px,&py,&pz);
            if(!handlestatus(status)) exit(0);
            cloud.points[x + (frame.getStrideInBytes() * y)/2].x = (px);
            cloud.points[x + (frame.getStrideInBytes() * y)/2].y = (py);
            cloud.points[x + (frame.getStrideInBytes() * y)/2].z = (pz);
            depthp++;
        }
    }
	pcl::PointCloud<pcl::PointXYZ>::ConstPtr const_cloud(new pcl::PointCloud<pcl::PointXYZ>(cloud));
	pcl::octree::PointCloudCompression<pcl::PointXYZ> *encoder;
	pcl::octree::compression_Profiles_e comp_profile = pcl::octree::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
	encoder = new pcl::octree::PointCloudCompression<pcl::PointXYZ>(comp_profile,showstats);
	encoder->encodePointCloud(const_cloud,comp_data);
	return comp_data;
}

KinectHelper::KinectHelper(QObject *parent) : QObject(parent){
    /// Initialize and open device, create depth stream
    /// NOTE: by doing this in the constructor it remains in the main thread,
    /// so the plugin will lock until Kinect is initialized
    bool success = initialize_device();
    if(!success) 
        throw StarlabException("Failed to initialize Kinect");

    /// GUI Elements
    widget = NULL;
    colorLabel = NULL;
    
    /// Instantiate listeners
    depthListener = new ModeKinectListener(this);
    colorListener = new ModeKinectListener(this);
    depth.addNewFrameListener(depthListener);
    color.addNewFrameListener(colorListener);
       
    /// Buffer pointers always valid
    points_front_buffer = &points_buffer_1;
    points_back_buffer  = &points_buffer_2;
    color_front_buffer = &color_buffer_1;
    color_back_buffer = &color_buffer_2;
    
    /// Avoid displaying stuff when data is not ready
    has_consumed_first_frame = false;
    
    ///Initialize the data sizes
    {
        VideoFrameRef frame;
        Status status = depth.readFrame(&frame);
        if(!status == STATUS_OK) 
            throw StarlabException("Failed to initialize Kinect");
        
        /// Allocate memory
        points_buffer_1.resize(frame.getHeight(), frame.getWidth());
        points_buffer_2.resize(frame.getHeight(), frame.getWidth());
        color_buffer_1 = QImage(frame.getWidth(),frame.getHeight(),QImage::Format_RGB888);
        color_buffer_2 = QImage(frame.getWidth(),frame.getHeight(),QImage::Format_RGB888);
                
        /// Communicate the viewport to the GUI
        compute_frame_bbox(frame); 
        emit scene_bbox_updated(_bbox);        
    }    
           
    /// Allow passing VideoFrameRefs
    qRegisterMetaType<VideoFrameRef>("VideoFrameRef");///< Allow pass reference frame as signals
}

//	save depth images
void saveDepthImage(const std::string name, const VideoFrameRef &depthFrame)
{
	int height = depthFrame.getHeight();
	int width = depthFrame.getWidth();
	cv::Mat image = cv::Mat::zeros(height, width, CV_16UC1);
	DepthPixel *pDepth = (DepthPixel *)depthFrame.getData();
	for (int i = 0; i < height; i++)
	{
		for (int j = 0; j < width; j++)
		{
			image.at<uint16_t>(i, j) = (uint16_t)(*pDepth);
			pDepth++;
		}
	}
	cv::imwrite(name, image);
}

BBox3 KinectHelper::compute_frame_bbox(VideoFrameRef frame){
    // qDebug() << "KinectThread::setBoundingBox(VideoFrameRef frame)";

    /// Get depth data from the frame just fetched
    const openni::DepthPixel* pDepth = (const openni::DepthPixel*)frame.getData();
    float wx,wy,wz;
    BBox3 box; box.setNull();
    for (int y = 0; y<frame.getHeight(); ++y){
        for(int x = 0; x<frame.getWidth(); ++x){
            DepthPixel d = *pDepth;
            pDepth++;
            CoordinateConverter::convertDepthToWorld(depth, x, y, d, &wx, &wy, &wz);
            box.extend( Vector3(wx,wy,wz) );
        }
    }
    this->_bbox = box;
}

int readOpenNiColor(VideoStream &color , VideoFrameRef &colorFrame)
{
    readFrameBlocking(color,colorFrame,MAX_TRIES_FOR_EACH_FRAME); // color.readFrame(&colorFrame);

    if (colorFrame.isValid()) { return 1; }
    fprintf(stderr,"Color frame  is wrong!\n");
    return 0;
}

void analyzeFrame(const VideoFrameRef& frame)
{
	DepthPixel* pDepth;
	RGB888Pixel* pColor;

	int middleIndex = (frame.getHeight()+1)*frame.getWidth()/2;

	switch (frame.getVideoMode().getPixelFormat())
	{
	case PIXEL_FORMAT_DEPTH_1_MM:
	case PIXEL_FORMAT_DEPTH_100_UM:
		pDepth = (DepthPixel*)frame.getData();
		printf("[%08llu] %8d\n", (long long)frame.getTimestamp(),
			pDepth[middleIndex]);
		break;
	case PIXEL_FORMAT_RGB888:
		pColor = (RGB888Pixel*)frame.getData();
		printf("[%08llu] 0x%02x%02x%02x\n", (long long)frame.getTimestamp(),
			pColor[middleIndex].r&0xff,
			pColor[middleIndex].g&0xff,
			pColor[middleIndex].b&0xff);
		break;
	default:
		printf("Unknown format\n");
	}
}

void copyFrameToImage(VideoFrameRef frame, DepthImage& image)
{
	if (image.height() != frame.getHeight() || image.width() != frame.getWidth())
	{
		image = DepthImage(
			DepthImage::Data(
			static_cast<const uint16_t*>(frame.getData()),
			static_cast<const uint16_t*>(frame.getData()) + frame.getWidth() * frame.getHeight()),
			frame.getWidth(),
			frame.getHeight());
	}
	else
	{
		image.data(DepthImage::Data(
			static_cast<const uint16_t*>(frame.getData()),
			static_cast<const uint16_t*>(frame.getData()) + frame.getWidth() * frame.getHeight()));
	}
}

void DepthCallback::analyzeFrame(const VideoFrameRef& frame)
{

    Mat image;
    image.create(frame.getHeight(),frame.getWidth(),CV_16UC1);
    const openni::DepthPixel* pImageRow = (const openni::DepthPixel*)frame.getData();
    memcpy(image.data,pImageRow,frame.getStrideInBytes() * frame.getHeight());
//    image *= 16;
    if (createCVWindow)
    {
        imshow( "DepthWindow", image );
        waitKey(10);
    }

//    cout<<"New depth frame w: "<<frame.getWidth()<<"  h: "<<frame.getHeight()<<endl;

    if (saveOneFrame || saveFrameSequence)
    {
        char buffer[50];
        sprintf(buffer,"depth%lld.png",frame.getTimestamp());
        imwrite(buffer,image);

        saveOneFrame = false;
        std::cout<<"DepthCallback :: saved file "<<buffer<<std::endl;
    }

    if (publishRosMessage)
    {
        cv_bridge::CvImage aBridgeImage;
        aBridgeImage.image = image;
        aBridgeImage.encoding = "mono16";
        cv::flip(aBridgeImage.image,aBridgeImage.image,1);
        sensor_msgs::ImagePtr rosImage = aBridgeImage.toImageMsg();
//        rosImage.get()->header.frame_id="/camera_depth_optical_frame";
        rosImage.get()->header.frame_id=string("/") + string (m_CameraNamespace)+string("_depth_optical_frame");
        rosImage.get()->encoding="16UC1";
        rosImage.get()->header.stamp = ros::Time::now();
        m_RosPublisher.publish(rosImage);

        sensor_msgs::CameraInfo camInfo;
        camInfo.width = frame.getWidth();
        camInfo.height = frame.getHeight();
        camInfo.distortion_model = "plumb_bob";
        camInfo.K = {{570.3422241210938, 0.0, 314.5, 0.0, 570.3422241210938, 235.5, 0.0, 0.0, 1.0}};
        camInfo.R = {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}};
        camInfo.P = {{570.3422241210938, 0.0, 314.5, -21.387834254417157, 0.0, 570.3422241210938, 235.5, 0.0, 0.0, 0.0, 1.0, 0.0}};
        double D[5] = {0.0,0.0,0.0,0.0,0.0};
        camInfo.D.assign(&D[0], &D[0]+5);
        camInfo.header.frame_id = string("/") + string (m_CameraNamespace)+string("_depth_optical_frame");
        camInfo.header.stamp = rosImage.get()->header.stamp;
        m_RosCameraInfoPublisher.publish(camInfo);
    }

}

Status NUIPoints::getNextData(std::list<cv::Point3f>& nuiPoints, VideoFrameRef& rawFrame)
{
	Status rc = m_pInternal->m_pDepthStream->readFrame(&rawFrame);
	if (rc != STATUS_OK)
	{
		printf("readFrame failed\n%s\n", OpenNI::getExtendedError());
	}

	DepthPixel* pDepth = (DepthPixel*)rawFrame.getData();
	bool found = false;
	cv::Point3f nuiPoint;
	int width = rawFrame.getWidth();
	int height = rawFrame.getHeight();

	for (int x = 1; x < width / 6.0; ++x) {
		for (int y = 1; y < height - (height / 4.5); ++y)
		{
			if (pDepth[y * width + x] < FAR_LIMIT
				&& pDepth[y * width + x] != 0
				&& (ZERO_TO_INFINITY(pDepth[(y + 1) * width + (x + 1)]) - pDepth[y * width + x] > DEPTH_TRESHOLD
				|| ZERO_TO_INFINITY(pDepth[(y - 1) * width + (x + 1)]) - pDepth[y * width + x] > DEPTH_TRESHOLD
				|| ZERO_TO_INFINITY(pDepth[(y + 1) * width + (x - 1)]) - pDepth[y * width + x] > DEPTH_TRESHOLD
				|| ZERO_TO_INFINITY(pDepth[(y - 1) * width + (x - 1)]) - pDepth[y * width + x] > DEPTH_TRESHOLD))
			{
				nuiPoint.x = x;
				nuiPoint.y = y;
				nuiPoint.z = pDepth[y * width + x];
				nuiPoints.push_back(nuiPoint);
			}
		}
	}

	if (nuiPoints.empty())
	{
		return STATUS_ERROR;
	}

	return STATUS_OK;
}

void KinectHelper::consume_depth(VideoFrameRef frame){
    /// Get depth data from the frame just fetched
    const openni::DepthPixel* pDepth = (const openni::DepthPixel*)frame.getData();

    float wx,wy,wz;
    
    /// NaN values must be handled well and not displayed by OpenGL
    for (int y = 0; y<frame.getHeight(); ++y){
        for(int x = 0; x<frame.getWidth(); ++x){
            DepthPixel d = *pDepth;
            pDepth++;
            /// Convert depth coordinates to world coordinates to remove camera intrinsics
            CoordinateConverter::convertDepthToWorld(depth, x, y, d, &wx, &wy, &wz);
            /// Change the coordinates of the vertices in the model
            (*points_back_buffer)(y,x) = Vector3(wx,wy,wz);
        }
    }
    
    /// Now swap front and back buffers
    _mutex.lock();
        qSwap(points_front_buffer, points_back_buffer);
    _mutex.unlock();    
}

bool OpenNI2CameraImpl::copyInfrared( OpenNI2Camera::FrameView& frame )
{
    frame.infraredUpdated = false;

    VideoFrameRef src;
    Status rc = m_infraredStream.readFrame( &src );
    if( rc == STATUS_OK )
    {
        Array2DReadView< uint16_t > srcData( src.getData(),
            { src.getWidth(), src.getHeight() },
            { sizeof( uint16_t ), src.getStrideInBytes() } );
        frame.infraredTimestampNS =
            usToNS( static_cast< int64_t >( src.getTimestamp() ) );
        frame.infraredFrameNumber = src.getFrameIndex();
        frame.infraredUpdated = copy( srcData, frame.infrared );
        return frame.infraredUpdated;
    }
    return false;
}

int main()
{
	// 2. initialize OpenNI
	Status rc = OpenNI::initialize();
	if (rc != STATUS_OK)
	{
		printf("Initialize failed\n%s\n", OpenNI::getExtendedError());
		return 1;
	}

	// 3. open a device
	Device device;
	rc = device.open(ANY_DEVICE);
	if (rc != STATUS_OK)
	{
		printf("Couldn't open device\n%s\n", OpenNI::getExtendedError());
		return 2;
	}

	// 4. create depth stream
	VideoStream depth;
	if (device.getSensorInfo(SENSOR_DEPTH) != NULL){
		rc = depth.create(device, SENSOR_DEPTH);
		if (rc != STATUS_OK){
			printf("Couldn't create depth stream\n%s\n", OpenNI::getExtendedError());
			return 3;
		}
	}
	VideoStream color;
	if (device.getSensorInfo(SENSOR_COLOR) != NULL){
		rc = color.create(device, SENSOR_COLOR);
		if (rc != STATUS_OK){
			printf("Couldn't create color stream\n%s\n", OpenNI::getExtendedError());
			return 4;
		}
	}

	// 5. create OpenCV Window
	cv::namedWindow("Depth Image", CV_WINDOW_AUTOSIZE);
	cv::namedWindow("Color Image", CV_WINDOW_AUTOSIZE);

	// 6. start
	rc = depth.start();
	if (rc != STATUS_OK)
	{
		printf("Couldn't start the depth stream\n%s\n", OpenNI::getExtendedError());
		return 5;
	}
	rc = color.start();
	if (rc != STATUS_OK){
		printf("Couldn't start the depth stream\n%s\n", OpenNI::getExtendedError());
		return 6;
	}
	VideoFrameRef colorframe;
	VideoFrameRef depthframe;
	int iMaxDepth = depth.getMaxPixelValue();

	cv::Mat colorimageRGB;
	cv::Mat colorimageBGR;
	cv::Mat depthimage;
	cv::Mat depthimageScaled;

	// 7. main loop, continue read
	while (!wasKeyboardHit())
	{
		// 8. check is color stream is available
		if (color.isValid()){
			if (color.readFrame(&colorframe) == STATUS_OK){
				colorimageRGB = { colorframe.getHeight(), colorframe.getWidth(), CV_8UC3, (void*)colorframe.getData() };
				cv::cvtColor(colorimageRGB, colorimageBGR, CV_RGB2BGR);
			}
		}

		// 9. check is depth stream is available
		if (depth.isValid()){
			if (depth.readFrame(&depthframe) == STATUS_OK){
				depthimage = { depthframe.getHeight(), depthframe.getWidth(), CV_16UC1, (void*)depthframe.getData() };
				depthimage.convertTo(depthimageScaled, CV_8U, 255.0 / iMaxDepth);
			}
		}

		cv::imshow("Color Image", colorimageBGR);
		cv::imshow("Depth Image", depthimageScaled);
		cv::waitKey(10);
	}

	color.stop();
	depth.stop();

	color.destroy();
	depth.destroy();

	device.close();
	OpenNI::shutdown();

	return 0;
}

void KinectHelper::consume_depth(VideoFrameRef frame){
    /// Get depth data from the frame just fetched
    const openni::DepthPixel* pDepth = (const openni::DepthPixel*)frame.getData(); // for point cloud
    const openni::DepthPixel* iDepth = (const openni::DepthPixel*)frame.getData(); // for depth image

    /// Depth Image
    (*depth_back_buffer) = QImage(frame.getWidth(), frame.getHeight(), QImage::Format_RGB32);

    for (unsigned nY=0; nY<frame.getHeight(); nY++)
    {
        uchar *imageptr = (*depth_back_buffer).scanLine(nY);
        for (unsigned nX=0; nX < frame.getWidth(); nX++)
        {
            unsigned depth = *iDepth;
            unsigned maxdist=10000;
            if(depth>maxdist) depth=maxdist;
            if(depth)
            {
                depth = (depth*255)/maxdist;
                //depth = (maxdist-depth)*255/maxdist+1;
            }
            // depth: 0: invalid
            // depth: 255: closest
            // depth: 1: furtherst (maxdist distance)
            // Here we could do depth*color, to show the colored depth
            imageptr[0] = depth;
            imageptr[1] = depth;
            imageptr[2] = depth;
            imageptr[3] = 0xff;
            iDepth++;
            imageptr+=4;
        }
    }


    _mutex.lock();
    qSwap(depth_front_buffer, depth_back_buffer);
    _mutex.unlock();


    /// Point Cloud

    float wx,wy,wz;

    /// NaN values must be handled well and not displayed by OpenGL
    for (int y = 0; y<frame.getHeight(); ++y){
        for(int x = 0; x<frame.getWidth(); ++x){
            DepthPixel d = *pDepth;
            pDepth++;
            /// Convert depth coordinates to world coordinates to remove camera intrinsics
            CoordinateConverter::convertDepthToWorld(depth, x, y, d, &wx, &wy, &wz);
            /// Change the coordinates of the vertices in the model
            (*points_back_buffer)(y,x) = Vector3(wx,wy,wz);
        }
    }

    /// Now swap front and back buffers
    _mutex.lock();
        qSwap(points_front_buffer, points_back_buffer);
    _mutex.unlock();
}