void PinholePointProjector::project(IntImage &indexImage,
				      DepthImage &depthImage, 
				      const PointVector &points) const {
    assert(_imageRows && _imageCols && "PinholePointProjector: _imageRows and _imageCols are zero");
    
    indexImage.create(_imageRows, _imageCols);
    depthImage.create(_imageRows, _imageCols);
 
    depthImage.setTo(cv::Scalar(std::numeric_limits<float>::max()));
    indexImage.setTo(cv::Scalar(-1));   

    float *drowPtrs[_imageRows];
    int *irowPtrs[_imageRows];
    for(int i = 0; i < _imageRows; i++) {
      drowPtrs[i] = &depthImage(i, 0);
      irowPtrs[i] = &indexImage(i, 0);
    }
    const Point *point = &points[0];
    for(size_t i = 0; i < points.size(); i++, point++) {
      int x, y;
      float d;
      if(!_project(x, y, d, *point) ||
	 d < _minDistance || d > _maxDistance ||
	 x < 0 || x >= indexImage.cols ||
	 y < 0 || y >= indexImage.rows)
	continue;
      float &otherDistance = drowPtrs[y][x];
      int &otherIndex = irowPtrs[y][x];
      if(!otherDistance || otherDistance > d) {
	otherDistance = d;
	otherIndex = i;
      }
    }
  }
Exemplo n.º 2
0
  void DepthImage_scale(DepthImage &dest, const DepthImage &src, int step) {
    int rows = src.rows / step;
    int cols = src.cols / step;
    dest.create(rows, cols);
    dest.setTo(cv::Scalar(0));
    for(int r = 0; r < dest.rows; r++) {
      for(int c = 0; c < dest.cols; c++) {
	float acc = 0;
	int np = 0;
	int sr = r * step;
	int sc = c * step;
	for(int i = 0; i < step; i++) {
	  for(int j = 0; j < step; j++) {
	    if(sr + i < src.rows && sc + j < src.cols) {
	      acc += src(sr + i, sc + j);
	      np += src(sr + i, sc + j) > 0;
	    }
	  }
	}
	if(np)
	  dest(r, c) = acc / np;
      }
    }
  }