Exemplo n.º 1
0
    void CylindricalPointProjector::unProject(PointVector &points, 
					      Gaussian3fVector &gaussians,
					      IntImage &indexImage,
					      const DepthImage &depthImage) const {
      assert(depthImage.rows > 0 && depthImage.cols > 0 && "CylindricalPointProjector: Depth image has zero dimensions");
      points.resize(depthImage.rows * depthImage.cols);
      gaussians.resize(depthImage.rows * depthImage.cols);
      indexImage.create(depthImage.rows, depthImage.cols);
      int count = 0;
      Point *point = &points[0];
      Gaussian3f *gaussian = &gaussians[0];
      for(int r = 0; r < depthImage.rows; r++) {
	const float *f = &depthImage(r, 0);
	int *i = &indexImage(r, 0);
	for(int c = 0; c < depthImage.cols; c++, f++, i++) {      
	  if(!_unProject(*point, c, r, *f)) {
	    *i = -1;
	    continue;
	  }
	  Eigen::Matrix3f cov = Eigen::Matrix3f::Identity();
	  *gaussian = Gaussian3f(point->head<3>(), cov);
	  gaussian++;
	  point++;
	  *i = count;
	  count++;
	}
      }
      points.resize(count);
      gaussians.resize(count);
    }
Exemplo n.º 2
0
  void SphericalPointProjector::unProject(PointVector &points, 
					  IntImage &indexImage,
					  const DepthImage &depthImage) const {
    if(_imageRows == 0 ||  _imageCols == 0) { 
      throw "SphericalPointProjector: Depth image has zero dimensions";
    }
    points.resize(depthImage.rows * depthImage.cols);
    int count = 0;
    indexImage.create(depthImage.rows, depthImage.cols);
    Point *point = &points[0];
    for(int r = 0; r < depthImage.rows; r++) {
      const float *f = &depthImage(r, 0);
      int *i = &indexImage(r, 0);
      for(int c = 0; c < depthImage.cols; c++, f++, i++) {
	if(!_unProject(*point, c, r, *f)) {
	  *i = -1;
	  continue;
	}
	point++;
	*i = count;
	count++;
      }
    }
    points.resize(count);
  }
  void PinholePointProjector::unProject(PointVector &points, 
					Gaussian3fVector &gaussians,
					IntImage &indexImage,
					const DepthImage &depthImage) const {
    assert(depthImage.rows > 0 && depthImage.cols > 0 && "PinholePointProjector: Depth image has zero dimensions");
    points.resize(depthImage.rows * depthImage.cols);
    gaussians.resize(depthImage.rows * depthImage.cols);
    indexImage.create(depthImage.rows, depthImage.cols);
    int count = 0;
    Point *point = &points[0];
    Gaussian3f *gaussian = &gaussians[0];
    float fB = _baseline * _cameraMatrix(0, 0);
    Eigen::Matrix3f J;
    for(int r = 0; r < depthImage.rows; r++) {
      const float *f = &depthImage(r, 0);
      int *i = &indexImage(r, 0);
      for(int c = 0; c < depthImage.cols; c++, f++, i++) {      
	if(!_unProject(*point, c, r, *f)) {
	  *i = -1;
	  continue;
	}
	float z = *f;
	float zVariation = (_alpha * z * z) / (fB + z * _alpha);
	J <<       
	  z, 0, (float)r,
	  0, z, (float)c,
	  0, 0, 1;
	J = _iK * J;
	Diagonal3f imageCovariance(3.0f, 3.0f, zVariation);
	Eigen::Matrix3f cov = J * imageCovariance * J.transpose();
	*gaussian = Gaussian3f(point->head<3>(), cov);
	gaussian++;
	point++;
	*i = count;
	count++;
      }
    }

    points.resize(count);
    gaussians.resize(count);
  }
  void PinholePointProjector::unProject(PointVector &points, 
					IntImage &indexImage,
					const DepthImage &depthImage) const {
    assert(depthImage.rows > 0 && depthImage.cols > 0 && "PinholePointProjector: Depth image has zero dimensions");
    points.resize(depthImage.rows * depthImage.cols);
    int count = 0;
    indexImage.create(depthImage.rows, depthImage.cols);
    Point *point = &points[0];
    for(int r = 0; r < depthImage.rows; r++) {
      const float *f = &depthImage(r, 0);
      int *i = &indexImage(r, 0);
      for(int c = 0; c < depthImage.cols; c++, f++, i++) {
	if(!_unProject(*point, c, r, *f)) {
	  *i = -1;
	  continue;
	}
	point++;
	*i = count;
	count++;
      }
    }

    points.resize(count);
  }