Kinect smooth depth image but not very fast yet. can anybody help make it faster?

it is a little messy

ofApp.h

#pragma once

#include "ofMain.h"
#include "ofxOpenCv.h"
#include "ofxKinect.h"

// Windows users:
// You MUST install the libfreenect kinect drivers in order to be able to use
// ofxKinect. Plug in the kinect and point your Windows Device Manager to the
// driver folder in:
//
//     ofxKinect/libs/libfreenect/platform/windows/inf
//
// This should install the Kinect camera, motor, & audio drivers.
//
// You CANNOT use this driver and the OpenNI driver with the same device. You
// will have to manually update the kinect device to use the libfreenect drivers
// and/or uninstall/reinstall it in Device Manager.
//
// No way around the Windows driver dance, sorry.

// uncomment this to read from two kinects simultaneously
//#define USE_TWO_KINECTS

class ofApp : public ofBaseApp {
public:
	
	void setup();
	void update();
	void draw();
	void exit();
	
	void drawPointCloud();
	
	void keyPressed(int key);
	void mouseDragged(int x, int y, int button);
	void mousePressed(int x, int y, int button);
	void mouseReleased(int x, int y, int button);
	void mouseEntered(int x, int y);
	void mouseExited(int x, int y);
	void windowResized(int w, int h);
	
	ofxKinect kinect;
	
#ifdef USE_TWO_KINECTS
	ofxKinect kinect2;
#endif
    ofShader mask;
    ofFbo masker;
	ofxCvContourFinder contourFinder;
	
	bool bThreshWithOpenCV;
	bool bDrawPointCloud;
	
	int Threshold;
    ofxCvGrayscaleImage gray1,gray2,gray3;
	
    ofVideoPlayer img0,img1,img2;

//    ofImage eindBeeld;

    ofImage img3, display1,display2,display3,eindBeeld;
//    ofTexture tex1,tex2,tex0;
//            ofImage img0,img1,img2,img3,display1,display2,display3,eindBeeld;
	int angle;
	
	// used for viewing the point cloud

};

main.cpp

#include "ofApp.h"

int main() {
    ofSetupOpenGL(1024, 768, OF_WINDOW);
	ofRunApp(new ofApp());
}

ofApp.cpp

#include "ofApp.h"

/*
    If you are struggling to get the device to connect ( especially Windows Users )
    please look at the ReadMe: in addons/ofxKinect/README.md
*/

//--------------------------------------------------------------
void ofApp::setup() {
	ofSetLogLevel(OF_LOG_VERBOSE);
	
	// enable depth->video image calibration
    
    //corectie van cam en depth sensor zodat ze op een lijn zitten
//	kinect.setRegistration(true);
    
    
	kinect.init();
	//kinect.init(true); // shows infrared instead of RGB video image
	//kinect.init(false, false); // disable video image (faster fps)

	kinect.open();		// opens first available kinect
	//kinect.open(1);	// open a kinect by id, starting with 0 (sorted by serial # lexicographically))
	//kinect.open("A00362A08602047A");	// open a kinect using it's unique serial #
	
	// print the intrinsic IR sensor values

	
#ifdef USE_TWO_KINECTS
	kinect2.init();
	kinect2.open();
#endif
	

	Threshold = 230;

	
	ofSetFrameRate(60);
	
	// zero the tilt on startup
	angle = 0;
	kinect.setCameraTiltAngle(angle);
	
	// start from the front
	bDrawPointCloud = false;
    mask.load("shadersGL2/shader");
//    img0.load("Comp1.mov");
    img1.load("Comp2.mov");
    img2.load("interview-joey.mov");
    img3.load("1.jpg");

    
//    img0.play();
    img1.play();
    img2.play();
    
    display1.allocate(640, 480, OF_IMAGE_GRAYSCALE);
    display2.allocate(640, 480, OF_IMAGE_GRAYSCALE);
    display3.allocate(640, 480, OF_IMAGE_GRAYSCALE);
    masker.allocate(1024, 768);
    gray1.allocate(640,480);
    gray2.allocate(640,480);
    gray3.allocate(640,480);
    eindBeeld.allocate(1024,768,OF_IMAGE_COLOR);
    
}

//--------------------------------------------------------------
void ofApp::update() {
	img0.update();
    img1.update();
    img2.update();
	
//    ofBackground(100, 100, 100);
	
	kinect.update();
	
	// there is a new frame and we are connected
	if(kinect.isFrameNew()) {
		
		// load grayscale depth image from the kinect source
		
		// we do two thresholds - one for the far plane and one for the near plane
		// we then do a cvAnd to get the pixels which are a union of the two thresholds
			
			// or we do it ourselves - show people how they can work with the pixels
//        ofPixels pixImg1 = img1.getPixels();
//        ofPixels pixImg2 = img2.getPixels();
//        ofPixels pixImg3 = img3.getPixels();
//0-255
//			ofPixels & pix = grayImage.getPixels();
        ofPixels & dispPix1 = display1.getPixels();
        ofPixels & dispPix2 = display2.getPixels();
        ofPixels & dispPix3 = display3.getPixels();

        ofShortPixels pix = kinect.getRawDepthPixels();
//        pix.mirror(false,true);
        
			int numPixels = pix.size();
			for(int i = 0; i < numPixels; i++) {
                //2000
				if(pix[i] < 1900+Threshold) {
//                    dispPix[3*i]=pixImg1[3*i];
//                    dispPix[3*i+1]=pixImg1[3*i+1];
//                    dispPix[3*i+2]=pixImg1[3*i+2];
                    dispPix1[i]=0;
                    dispPix2[i]=255;
                    dispPix3[i]=0;

                    //                    dispPix[3*i+1]=255;
//                    dispPix[3*i+2]=0;
                    
                    //					pix[i] = pixImg1[i];
                    //2100
				} else if (pix[i]<1950+Threshold){
//                    dispPix[3*i]=pixImg2[3*i];
//                    dispPix[3*i+1]=pixImg2[3*i+1];
//                    dispPix[3*i+2]=pixImg2[3*i+2];
                    dispPix1[i]=255;
                    dispPix2[i]=0;
                    dispPix3[i]=0;

//                    dispPix[3*i+1]=0;
//                    dispPix[3*i+2]=0;
//					pix[i] = pixImg2[i];
//				}
                // LAAG EEN
                //2500   //1500 is ong 180cm
//                else if (pix[i]<1800+Threshold){
//                    dispPix[3*i]=pixImg3[3*i];
//                    dispPix[3*i+1]=pixImg3[3*i+1];
//                    dispPix[3*i+2]=pixImg3[3*i+2];
//                    dispPix[3*i]=0;
//                    dispPix[3*i+1]=0;
//                    dispPix[3*i+2]=255;
                    //					pix[i] = pixImg2[i];
                }
                else {
//                    dispPix[3*i]=pixImg3[3*i];
//                    dispPix[3*i+1]=pixImg3[3*i+1];
//                    dispPix[3*i+2]=pixImg3[3*i+2];
                    
                    dispPix1[i]=0;
                    dispPix2[i]=0;
                    dispPix3[i]=255;
//                    dispPix[3*i+1]=255;
//                    dispPix[3*i+2]=0;
                }
			}
		
		// update the cv images
        display1.update();
        display2.update();
        display3.update();

        
        gray1.setFromPixels(display1.getPixels());
        gray2.setFromPixels(display2.getPixels());
        gray3.setFromPixels(display3.getPixels());

		// find contours which are between the size of 20 pixels and 1/3 the w*h pixels.
		// also, find holes is set to true so we will get interior contours as well....
	}
	
#ifdef USE_TWO_KINECTS
	kinect2.update();
#endif
}

//--------------------------------------------------------------
void ofApp::draw() {
    
    int a = 8;
    
    
    
	ofSetColor(255, 255, 255);
//    gray1.draw(0, 0, 640,480);
    ofFill();
    masker.begin();
    ofClear(0, 0, 0);
    contourFinder.findContours(gray3, 20, (640*480), 10, true);
    for (int i = 0; i <contourFinder.nBlobs; i++){
        
        // does blob have any points?
        if(contourFinder.blobs[i].pts.size()>5){
            
            ofPolyline tempPoly;
            
            tempPoly.addVertices(contourFinder.blobs[i].pts);
            tempPoly.setClosed(true);
            
            // smoothing is set to 200
            //            ofPolyline smoothTempPoly = tempPoly.getSmoothed(20, 0.5);
            ofPolyline smoothTempPoly = tempPoly.getSmoothed(a,0.5);
            
            
            if(!smoothTempPoly.isClosed()){
                smoothTempPoly.close();
            }
            ofSetColor(255, 0, 0);
            ofBeginShape();
            for( int i = 0; i < smoothTempPoly.getVertices().size(); i++) {
                ofVertex(smoothTempPoly.getVertices().at(i).x*1.6, smoothTempPoly.getVertices().at(i).y*1.6);
            }
            ofEndShape();
//            smoothTempPoly.draw();
            //            contourPoly.push_back(smoothTempPoly);
            
        }
        
    }
    contourFinder.findContours(gray2, 20, (640*480), 10, true);
    for (int i = 0; i <contourFinder.nBlobs; i++){
        
        // does blob have any points?
        if(contourFinder.blobs[i].pts.size()>5){
            
            ofPolyline tempPoly;
            
            tempPoly.addVertices(contourFinder.blobs[i].pts);
            tempPoly.setClosed(true);
            
            // smoothing is set to 200
            //            ofPolyline smoothTempPoly = tempPoly.getSmoothed(20, 0.5);
            ofPolyline smoothTempPoly = tempPoly.getSmoothed(a,0.5);
            
            
            if(!smoothTempPoly.isClosed()){
                smoothTempPoly.close();
            }
            ofSetColor(0, 255, 0);
            ofBeginShape();
            for( int i = 0; i < smoothTempPoly.getVertices().size(); i++) {
                ofVertex(smoothTempPoly.getVertices().at(i).x*1.6, smoothTempPoly.getVertices().at(i).y*1.6);
            }
            ofEndShape();
            //            contourPoly.push_back(smoothTempPoly);
            
        }
        
    }    contourFinder.findContours(gray1, 20, (640*480), 10, true);
    for (int i = 0; i <contourFinder.nBlobs; i++){
        
        // does blob have any points?
        if(contourFinder.blobs[i].pts.size()>5){
            
            ofPolyline tempPoly;
            
            tempPoly.addVertices(contourFinder.blobs[i].pts);
            tempPoly.setClosed(true);
            
            // smoothing is set to 200
            //            ofPolyline smoothTempPoly = tempPoly.getSmoothed(20, 0.5);
            ofPolyline smoothTempPoly = tempPoly.getSmoothed(a,0.5); //5,0.5
            
            
            if(!smoothTempPoly.isClosed()){
                smoothTempPoly.close();
            }
            ofSetColor(0, 0, 255);
            ofBeginShape();
            for( int i = 0; i < smoothTempPoly.getVertices().size(); i++) {
                ofVertex(smoothTempPoly.getVertices().at(i).x*1.6, smoothTempPoly.getVertices().at(i).y*1.6);
            }
            ofEndShape();
            //            contourPoly.push_back(smoothTempPoly);
            
        }
        
    }
    masker.end();
//    img0.draw(0,0,100,100);
    img1.draw(0, 0, 1024, 768);
    img2.draw(0, 0, 1024, 768);
    masker.draw(0,0);
    ofTexture tex0= img0.getTexture();
    
    mask.begin();
    mask.setUniformTexture("tex0", img1.getTexture(), 1);
    mask.setUniformTexture("tex1", img2.getTexture(), 2);
    mask.setUniformTexture("tex2", img3.getTexture(), 3);
    mask.setUniformTexture("imageMask", masker.getTexture(), 4);
    masker.draw(0, 0);
    mask.end();
//    gray3.draw(0,0,640,480);
//    kinect.drawDepth(0, 0);
#ifdef USE_TWO_KINECTS
//		kinect2.draw(420, 320, 400, 300);
#endif
//	}
//    gray1.draw(150,0,130,90);
//    gray2.draw(300,0,130,90);
//    gray3.draw(450,0,130,90);
//    masker.draw(600,0,130,90);
    

}

//--------------------------------------------------------------
void ofApp::exit() {
	kinect.setCameraTiltAngle(20); // zero the tilt on exit
	kinect.close();
	
#ifdef USE_TWO_KINECTS
	kinect2.close();
#endif
}

//--------------------------------------------------------------
void ofApp::keyPressed (int key) {
	switch (key) {
            case '>':
            Threshold+=10;
            break;
        case '<':
            Threshold-=10;
            break;
		case OF_KEY_UP:
			angle++;
			if(angle>30) angle=30;
			kinect.setCameraTiltAngle(angle);
			break;
			
		case OF_KEY_DOWN:
			angle--;
			if(angle<-30) angle=-30;
			kinect.setCameraTiltAngle(angle);
			break;
	}
}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button)
{
	
}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button)
{

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button)
{

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y){

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h)
{

}