void RealisticCamera::ComputeThickLensApproximation(Float pz[2],
        Float fz[2]) const {
    // Find height $x$ from optical axis for parallel rays
    Float x = .001 * film->diagonal;

    // Compute cardinal points for film side of lens system
    Ray rScene(Point3f(x, 0, LensFrontZ() + 1), Vector3f(0, 0, -1));
    Ray rFilm;
    bool ok = TraceLensesFromScene(rScene, &rFilm);
    if (!ok)
        Severe(
            "Unable to trace ray from scene to film for thick lens "
            "approximation. Is aperture stop extremely small?");
    ComputeCardinalPoints(rScene, rFilm, &pz[0], &fz[0]);

    // Compute cardinal points for scene side of lens system
    rFilm = Ray(Point3f(x, 0, LensRearZ() - 1), Vector3f(0, 0, 1));
    ok = TraceLensesFromFilm(rFilm, &rScene);
    if (!ok)
        Severe(
            "Unable to trace ray from film to scene for thick lens "
            "approximation. Is aperture stop extremely small?");
    ComputeCardinalPoints(rFilm, rScene, &pz[1], &fz[1]);
}

void Game::spawnAllBlocks() {
	float z = -currentLevel->length + 16.0f + 6 * currentStep;

	for (int i = 0; i < currentLevel->height; ++i) {
		for (int j = 0; j < currentLevel->width; ++j) {
			float x = 2.0f * j - currentLevel->width;
			float y = i*2.0f + 1.0f;

			if (1 == rand() % 2) {
				mainBlocks.push_back(std::make_shared<GameObject>(Point3f(x, y, z), 2.0f, SDL_COLOR_BLUE));
			}
			else {
				freeBlockSlots.push_back(Point3f(x, y, z));
			}
		}
	}

	int blocksNeeded = currentLevel->width * currentLevel->height - mainBlocks.size();

	for (int i = 0; i < blocksNeeded; ++i) {
		Point3f p((float)getRandom(-currentLevel->width, currentLevel->width), 4 + (blocksNeeded-i)*2.0f, player->getCenter().z + getRandom(-5, 5));
		extraBlocks.push_back(std::make_shared<GameObject>(p, 2.0f, getRandomColor(50, 200)));
	}
}

void CameraCalibration::calcBoardCornerPositions(Size boardSize, float squareSize, 
										vector<Point3f>& corners, Settings::Pattern patternType)
{
	
	corners.clear();

	switch (patternType)
	{
	case Settings::CHESSBOARD:
	case Settings::CIRCLES_GRID:
		for (int i = 0; i < boardSize.height; ++i)
			for (int j = 0; j < boardSize.width; ++j)
				corners.push_back(Point3f(float(j*squareSize), float(i*squareSize), 0));
		break;

	case Settings::ASYMMETRIC_CIRCLES_GRID:
		for (int i = 0; i < boardSize.height; i++)
			for (int j = 0; j < boardSize.width; j++)
				corners.push_back(Point3f(float((2 * j + i % 2)*squareSize), float(i*squareSize), 0));
		break;
	default:
		break;
	}
}

Point3f Reprojector::reproject_to_3d(float pt0_x, float pt0_y, float pt1_x, float pt1_y)
{
	float disparity_val = abs(pt1_x - pt0_x);
	float depth = compute_depth(disparity_val);

	Point2f plane_size = compute_plane_size(depth);

	float half_plane_width = plane_size.x / 2;
	float halfPlaneHeight = plane_size.y / 2;

	float real_x = map_val(pt0_x, 0, WIDTH_LARGE, -half_plane_width, half_plane_width);
	float real_y = map_val(pt0_y, 0, HEIGHT_LARGE, -halfPlaneHeight, halfPlaneHeight);

	return Point3f(real_x * 10, real_y * 10, depth * 10);
}

  Level_1::Level_1()
    :  /*m_floor(Point3f(0.0f, 0.0f, 0.0f),
              Vector3f(200.0f, 200.0f, -1.0f)),
		m_crate(Point3f(50.0f, 50.0f, 0.0f),
              Vector3f(30.0f, 30.0f, 30.0f)),
		m_crate1(Point3f(150.0f, 0.0f, 0.0f),
			Vector3f(20.0f, 20.0f, 20.0f)),
		m_crate2(Point3f(0.0f, 150.0f, 100.0f),
			Vector3f(40.0f, 40.0f, 40.0f)),*/
    m_player(Camera(Point3f(-200.0f, 0.0f, 50.0f),
             Quaternion(),
             1.0f, 10000.0f),
             Vector3f(0.0f, 0.0f, -39.0f),
             12.0f)/*,
			 m_fire(false)*/
  {
	  m_floor = Crate(Point3f(-200.0f, -200.0f, 0.0f), Vector3f(400.0f, 400.0f, -1.0f));
		m_crate = Crate(Point3f(0.0f, 60.0f, 0.0f), Vector3f(30.0f, 30.0f, 30.0f));
		//m_crate1 = Crate(Point3f(150.0f, 0.0f, 0.0f), Vector3f(20.0f, 20.0f, 20.0f));
		m_crate2 = Crate(Point3f(100.0f, 0.0f, 20.0f), Vector3f(10.0f, 10.0f, 10.0f));
    //m_player = Player(Camera(Point3f(0.0f, 0.0f, 50.0f), Quaternion(), 1.0f, 10000.0f), Vector3f(0.0f, 0.0f, -39.0f), 12.0f);
			 m_fire = false;
    set_pausable(true);
  }

    Point3f Unproject( float xp, float yp, float depth ) const
    {
      CV_Assert( depth > 0 );

      Matx31f x, ray, ret;
      x(0) = xp; x(1) = yp; x(2) = 1;

      ray = m_inv_K * x;
      ray *= depth;
      // -z into the screen
      ray(2) = -ray(2);

      ret = m_inv_R*( ray - m_t );
      return Point3f( ret(0), ret(1), ret(2) );
    }

void Game::initLevel() {
	// clear everything associated with level
	mainBlocks.clear();
	extraBlocks.clear();
	freeBlockSlots.clear();
	platforms.clear();
	openPlatforms.clear();

	// tell the "platform builder" where to start
	Point2 firstPlatform(currentLevel->width / 2, 0);
	openPlatforms.push_back(firstPlatform);
	currentLevel->data[firstPlatform.x][firstPlatform.y] = true;

	// return objects to initial state
	player->setCenter(Point3f(-1.0f, 1.0f, -currentLevel->length + 6.0f));	// set player at the beginning + 6 blocks for some margin 
	prize->setCenter(Point3f(-1.0f, 1.0f, currentLevel->length - 2.0f));	// set prize right at the end
	prize->setRotate(0, 0, 0);
	prize->setScale(1, 1, 1);
	currentStep = 0;

	worldTimer.reset();

	currentCutScene = CutScene::LEVEL_BEGINNING;
}

//导入标定图片，提取角点
bool CSingleCalib::addChessBoardPoints(String *strImageName)
{
	cout << "CSingleCalib::addChessBoardPoints !" << endl;
	bool bRet = false;
	do
	{
		vector<Point2f> imageCorners;
		vector<Point3f> objectCorners;
		

		for (int i = 0; i < ChessBoardSize_h; ++i)
		{
			for (int j = 0; j < ChessBoardSize_w; ++j)
			{
				objectCorners.push_back(Point3f(i*SquareWidth, j*SquareWidth, 0));
			}
		}
		cv::Mat image;
		
		for (int i = 1; i <= NImage; ++i)
		{
			image = imread(*strImageName);
			
			cvtColor(image, grayImage, CV_BGR2GRAY);
			
			bool found = findChessboardCorners(image, ChessBoardSize, imageCorners);
			cornerSubPix(grayImage, imageCorners, Size(5, 5), cv::Size(-1, -1),
						cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1));
			cout << i << endl;
			if (imageCorners.size() != ChessBoardSize.area())
			{
				cout << "检测的角点数和棋盘格本身的数目不等" << endl;
				break; 
			}
			
			addPoints(imageCorners, objectCorners);
			
			cv::drawChessboardCorners(image, ChessBoardSize, imageCorners, found);
			cv::imshow("BoardCorner", image);
			cv::waitKey(10);
			++strImageName;
		}
//		cvDestroyWindow("image");
		cvDestroyWindow("BoardCorner");
		bRet = true;
	} while (false);
	return bRet;
}

		void draw()
		{
			engine->controller.updateMouse();
			pos_x = engine->controller.getMousePos().x;
			pos_y = engine->controller.getMousePos().y;
			renderer->drawQuad(square,Point3f(pos_x+100,pos_y,0),pos_y*1.0,pos_y*1.0,rot);
			renderer->drawQuad(square2,Point3f(pos_x,pos_y,-.01),pos_y*1.0,pos_y*1.0,rot*.9);	
			renderer->drawQuad(square3,Point3f(pos_x-100,pos_y,-.02),pos_y*1.0,pos_y*1.0,rot*.8);				
			renderer->drawText("default font","Scale 1",Point3f(100,100,0),ColorRGBA(1,1,1,1),8,1.0);
			renderer->drawText("default font","Scale 2",Point3f(100,200,0),ColorRGBA(1,1,1,1),8*2,2.0);
			renderer->drawText("default font","Scale 3",Point3f(100,300,0),ColorRGBA(1,1,1,1),8*3,3.0);
			renderer->drawText("default font",std::to_string(engine->t).c_str(),Point3f(100,400,0),ColorRGBA(1,1,1,1),8*10,10.0);
			renderer->drawBuffer();
		};                              //executes as often as possible

Float PerspectiveCamera::GenerateRayDifferential(const CameraSample &sample,
                                                 RayDifferential *ray) const {
    ProfilePhase prof(Prof::GenerateCameraRay);
    // Compute raster and camera sample positions
    Point3f pFilm = Point3f(sample.pFilm.x, sample.pFilm.y, 0);
    Point3f pCamera = RasterToCamera(pFilm);
    Vector3f dir = Normalize(Vector3f(pCamera.x, pCamera.y, pCamera.z));
    *ray = RayDifferential(Point3f(0, 0, 0), dir);
    // Modify ray for depth of field
    if (lensRadius > 0) {
        // Sample point on lens
        Point2f pLens = lensRadius * ConcentricSampleDisk(sample.pLens);

        // Compute point on plane of focus
        Float ft = focalDistance / ray->d.z;
        Point3f pFocus = (*ray)(ft);

        // Update ray for effect of lens
        ray->o = Point3f(pLens.x, pLens.y, 0);
        ray->d = Normalize(pFocus - ray->o);
    }

    // Compute offset rays for _PerspectiveCamera_ ray differentials
    if (lensRadius > 0) {
        // Compute _PerspectiveCamera_ ray differentials accounting for lens

        // Sample point on lens
        Point2f pLens = lensRadius * ConcentricSampleDisk(sample.pLens);
        Vector3f dx = Normalize(Vector3f(pCamera + dxCamera));
        Float ft = focalDistance / dx.z;
        Point3f pFocus = Point3f(0, 0, 0) + (ft * dx);
        ray->rxOrigin = Point3f(pLens.x, pLens.y, 0);
        ray->rxDirection = Normalize(pFocus - ray->rxOrigin);

        Vector3f dy = Normalize(Vector3f(pCamera + dyCamera));
        ft = focalDistance / dy.z;
        pFocus = Point3f(0, 0, 0) + (ft * dy);
        ray->ryOrigin = Point3f(pLens.x, pLens.y, 0);
        ray->ryDirection = Normalize(pFocus - ray->ryOrigin);
    } else {
        ray->rxOrigin = ray->ryOrigin = ray->o;
        ray->rxDirection = Normalize(Vector3f(pCamera) + dxCamera);
        ray->ryDirection = Normalize(Vector3f(pCamera) + dyCamera);
    }
    ray->time = Lerp(sample.time, shutterOpen, shutterClose);
    ray->medium = medium;
    *ray = CameraToWorld(*ray);
    ray->hasDifferentials = true;
    return 1;
}

void SPHSystem::computeForce()
{
	int i, j, s, numNei;
	float dij;
	float term1, term2;
	Point3f vij;
	Point3f pf, vf, tf, gf, cf;

	//#pragma omp parallel for private( j, nId, numNei, d, term1, term2, dv, pf, vf, tf, gf )
	for(i=0; i<p.size(); ++i) {
		pf.Set(0,0,0);
		vf.Set(0,0,0);
		tf.Set(0,0,0);
		numNei = p[i]->pq.getSize();

		
		for(s=1; s<=numNei; ++s) {
			j = p[i]->pq.queue[s];
			if(i==j) continue;
			term1 = p[i]->p/(p[i]->d*p[i]->d);
			term2 = p[j]->p/(p[j]->d*p[j]->d);
			vij = *p[i]-*p[j];
			dij = vij.Length();
			// pressure force
			pf += (term1+term2)*p[j]->m*k->pw1(dij)*vij;
			// viscosity force
			vf += p[j]->m*k->vw2(dij)*(p[j]->v-p[i]->v);
			// surface tension			
		}

		
		pf = -p[i]->d*pf;
		vf = (X/p[i]->d)*vf;
		gf = p[i]->d*Point3f(0.0f, -GR, 0.0f);
		
		if(p[i]->n.Length() < ( SIM_DIM==2 ? 0.6f : 3.0f) )		// for 2d < 0.6 ,   3d < 3.0
			tf.Zero();
		else
			tf = -p[i]->lapc*p[i]->n.GetNormalized();
		
		p[i]->tf = tf;
		p[i]->pf = pf;

		p[i]->vf = vf;
		p[i]->a = (pf+vf+gf+tf)/p[i]->d;
	}
}

    SpotLight(const PropertyList &props) :
        Emitter()
    {
        //set the arguments
        m_position =  props.getPoint("position");
        m_intensity = props.getColor("Intensity");
        bool useLookAt = props.getBoolean("useLookAt", true);
        if(useLookAt){
            Vector3f lookPT = props.getPoint("lookAt", Point3f(0.0f, 0.0f, 0.0f));
            m_direction = (lookPT - m_position).normalized();
        } else {
            m_direction = props.getVector("direction");
        }
        m_theta = props.getFloat("theta");
        m_cosFalloffStart = props.getFloat("falloff");

    }

void Game::playCutSceneGameWin() {
	player->lookAt(Point3f(0, 0, 0));
	if (cutSceneTimer.getElapsed() >= 0.075 * __SECOND) {
		killPlatform();
		player->addScore(SCORE_PER_BLOCK);
	}

	if (eventSystem->isPressed(Key::SPACE) || platforms.empty()) {
		while (!platforms.empty()) {
			killPlatform();
			player->addScore(SCORE_PER_BLOCK);
		}

		resetCutScene();
		running = false;
	}
}

int Get3d_2dPoints(PointCloud<PointXYZ>::ConstPtr  cloud_p, 
	            PointCloud<PointXYZ>::ConstPtr  cloud_p2, 
		    MatrixXf &x3d, 
		    MatrixXf &x2d,   
		   vector <Point3f>& points3d,
		    vector <Point2f>& imagePoints){

 x3d= MatrixXf::Ones(cloud_p->points.size (),4);
  x2d= MatrixXf::Ones(cloud_p2->points.size (),3);
  std::cout<<"points selected \n"<<cloud_p->points.size ()<<std::endl;
  
  
  if (cloud_p->points.size ()!=cloud_p2->points.size ()){
   
    
    return (-1);
  }
  
  int i,j;  
  
   
  for (i=0;i<cloud_p->points.size ();i++){
    
    x3d(i,0)=cloud_p->points[i].x;
    x3d(i,1)=cloud_p->points[i].y;
    x3d(i,2)=cloud_p->points[i].z;
    x3d(i,3)=1;   
    
    points3d.push_back(Point3f(x3d(i,0), x3d(i,1), x3d(i,2)));
    
    
  }
  
  for (i=0;i<cloud_p2->points.size ();i++){
    
    x2d(i,0)=cloud_p2->points[i].x;
    x2d(i,1)=cloud_p2->points[i].y;
    x2d(i,2)=cloud_p2->points[i].z;
    
     imagePoints.push_back(Point2f(x2d(i,0), x2d(i,1)));
  }
  
  return (0);
  
  
}

void RealisticCamera::TestExitPupilBounds() const {
    Float filmDiagonal = film->diagonal;

    static RNG rng;

    Float u = rng.UniformFloat();
    Point3f pFilm(u * filmDiagonal / 2, 0, 0);

    Float r = pFilm.x / (filmDiagonal / 2);
    int pupilIndex =
        std::min((int)exitPupilBounds.size() - 1,
                 (int)std::floor(r * (exitPupilBounds.size() - 1)));
    Bounds2f pupilBounds = exitPupilBounds[pupilIndex];
    if (pupilIndex + 1 < (int)exitPupilBounds.size())
        pupilBounds = Union(pupilBounds, exitPupilBounds[pupilIndex + 1]);

    // Now, randomly pick points on the aperture and see if any are outside
    // of pupil bounds...
    for (int i = 0; i < 1000; ++i) {
        Point2f pd = ConcentricSampleDisk(
                         Point2f(rng.UniformFloat(), rng.UniformFloat()));
        pd *= RearElementRadius();

        Ray testRay(pFilm, Point3f(pd.x, pd.y, 0.f) - pFilm);
        Ray testOut;
        if (!TraceLensesFromFilm(testRay, &testOut)) continue;

        if (!Inside(pd, pupilBounds)) {
            fprintf(stderr,
                    "Aha! (%f,%f) went through, but outside bounds (%f,%f) - "
                    "(%f,%f)\n",
                    pd.x, pd.y, pupilBounds.pMin[0], pupilBounds.pMin[1],
                    pupilBounds.pMax[0], pupilBounds.pMax[1]);
            RenderExitPupil(
                (Float)pupilIndex / exitPupilBounds.size() * filmDiagonal / 2.f,
                0.f, "low.exr");
            RenderExitPupil((Float)(pupilIndex + 1) / exitPupilBounds.size() *
                            filmDiagonal / 2.f,
                            0.f, "high.exr");
            RenderExitPupil(pFilm.x, 0.f, "mid.exr");
            exit(0);
        }
    }
    fprintf(stderr, ".");
}

System3d::System3d()
{
    scale = 3;

    // initiate Mat images
    //pmdGrayImage = Mat::zeros(scale*IMG_HEIGHT, scale*IMG_WIDTH, CV_32F);
    pmdGrayImage = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_32F);
    pmdDepthImage = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_32F);
    webcamImage = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_8UC3);

    pmdChessImage = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_8U);
    webcamChessImage = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_8U);
    webcamImageTmp = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_8UC3);

    pmdTmp = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_8U);
    webTmp = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_8UC3);

    undistortPMD = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_32F);
    undistortWebcam = Mat::zeros(scale*IMG_WIDTH, scale*IMG_HEIGHT, CV_8UC3);

    // calibration matrices
    offset = Mat::zeros(4,4,CV_64F);

    // cloud parameters
    cloud.width = scale*IMG_WIDTH;
    cloud.height = scale*IMG_HEIGHT;
    cloud.points.resize (cloud.width*cloud.height);

    cloud_width = cloud.width;
    cloud_height = cloud.height;

    // chessboard pattern points coordinate
    for (int i=0; i<4; i++)
        for (int j=0; j<3; j++)
            chessboardPointsWebcam.push_back(Point3f(i*0.0988, j*0.0994, 0)); // poziomo - 9.94 cm, pionowo - 9.88 cm

    /*
    for (int i=0; i<4; i++)
        for (int j=0; j<3; j++)
            chessboardPointsPMDcam.push_back(Point3f(i*0.0988*scale, j*0.0994*scale, 0));
    */

    calibrated = false;
    patternCount = 0;
}

static void longLatLabel(const string& labelText,
                         double longitude,
                         double latitude,
                         const Vec3d& viewRayOrigin,
                         const Vec3d& viewNormal,
                         const Vec3d& bodyCenter,
                         const Quatd& bodyOrientation,
                         const Vec3f& semiAxes,
                         float labelOffset,
                         Renderer* renderer)
{
    double theta = degToRad(longitude);
    double phi = degToRad(latitude);
    Vec3d pos(cos(phi) * cos(theta) * semiAxes.x,
              sin(phi) * semiAxes.y,
              -cos(phi) * sin(theta) * semiAxes.z);
    
    float nearDist = renderer->getNearPlaneDistance();
    
    pos = pos * (1.0 + labelOffset);
    
    double boundingRadius = max(semiAxes.x, max(semiAxes.y, semiAxes.z));

    // Draw the label only if it isn't obscured by the body ellipsoid
    double t = 0.0;
    if (testIntersection(Ray3d(Point3d(0.0, 0.0, 0.0) + viewRayOrigin, pos - viewRayOrigin),
                         Ellipsoidd(Vec3d(semiAxes.x, semiAxes.y, semiAxes.z)), t) && t >= 1.0)
    {
        // Compute the position of the label
        Vec3d labelPos = bodyCenter +
                         (1.0 + labelOffset) * pos * bodyOrientation.toMatrix3();
        
        // Calculate the intersection of the eye-to-label ray with the plane perpendicular to
        // the view normal that touches the front of the objects bounding sphere
        double planetZ = viewNormal * bodyCenter - boundingRadius;
        if (planetZ < -nearDist * 1.001)
            planetZ = -nearDist * 1.001;
        double z = viewNormal * labelPos;
        labelPos *= planetZ / z;
        
        renderer->addObjectAnnotation(NULL, labelText,
                                      Renderer::PlanetographicGridLabelColor,
                                      Point3f((float) labelPos.x, (float) labelPos.y, (float) labelPos.z));                             
    }
}

bool PointerMapper::project_to_plane(Point3f& pt, Point3f& result, float& dist_to_plane)
{
    Ray ray = Ray(pt, direction_plane);

	float t;
    if (ray.intersects(plane, t))
    {
    	const float x = pt.x + t * direction_plane.x;
    	const float y = pt.y + t * direction_plane.y;
    	const float z = pt.z + t * direction_plane.z;
        result = Point3f(x, y, z);
        dist_to_plane = get_distance(result, pt);
        
        return true;
    }

    return false;
}

void Game::buildPlatforms() {
	std::list<Point2> neighbors;

	for (auto point : openPlatforms) {
		std::list<Point2> tmp = getNeighborPlatforms(point);
		for (auto p : tmp)
			neighbors.push_back(p);

		float x = 2.0f * point.x - currentLevel->width;
		float z = 2.0f * point.y - currentLevel->length;	// place it the way that level center is always at 0.0.0 origin

		// y = -0.1f so that the top of the platforms represent the 0th line in Y - the ground
		std::shared_ptr<GameObject> plat = std::make_shared<GameObject>(Point3f(x, -0.1f, z), 2.0f, 0.2f, 2.0f, ResourceManager::getTextureID(_RES_TEX_BRICK));
		platforms.push_back(plat);
	}

	openPlatforms = neighbors;
}

void testApp::updatePointCloud() {
	pointCloud.clear();
	
	const unsigned int Xres = 640;
	const unsigned int Yres = 480;
	
	Point2d fov = kinectCalibration.getUndistortedIntrinsics().getFov();
	float fx = tanf(ofDegToRad(fov.x) / 2) * 2;
	float fy = tanf(ofDegToRad(fov.y) / 2) * 2;
	
	Point2d principalPoint = kinectCalibration.getUndistortedIntrinsics().getPrincipalPoint();
	cv::Size imageSize = kinectCalibration.getUndistortedIntrinsics().getImageSize();
	
	int w = 640;
	int h = 480;
//	int w = curKinect.getWidth();
//	int h = curKinect.getHeight();
//	float* pixels = curKinect.getPixels();
	Mat pixels = curKinect;
	int i = 0;
	
	/*
	 principalPoint.x += ofMap(mouseX, 0, ofGetWidth(), -4, 4);
	 principalPoint.y += ofMap(mouseY, 0, ofGetHeight(), -4, 4);
	 cout << "fudge: " << ofMap(mouseX, 0, ofGetWidth(), -4, 4) << "x" << ofMap(mouseY, 0, ofGetHeight(), -4, 4) << endl;
	 */
	
	for(int y = 0; y < h; y++) {
		for(int j = 0; j < w; j++) {
			float pixel = curKinect.at<float>(y, j);
			if(pixel < 1000) { // the rest is basically noise
				int x = Xres - j - 1; // x axis is flipped from depth image
				float z = rawToCentimeters(pixel);
				
				float xReal = (((float) x - principalPoint.x) / imageSize.width) * z * fx;
				float yReal = (((float) y - principalPoint.y) / imageSize.height) * z * fy;
				
				// add each point into pointCloud
				pointCloud.push_back(Point3f(xReal, yReal, z));
			}									
			i++;
		}
	}
}