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;
}
}
}
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;
}
}
}