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 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);
}
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::projectIntervals(IntImage &intervalImage,
const DepthImage &depthImage,
const float worldRadius) const {
assert(depthImage.rows > 0 && depthImage.cols > 0 && "PinholePointProjector: Depth image has zero dimensions");
intervalImage.create(depthImage.rows, depthImage.cols);
for(int r = 0; r < depthImage.rows; r++) {
const float *f = &depthImage(r, 0);
int *i = &intervalImage(r, 0);
for(int c = 0; c < depthImage.cols; c++, f++, i++) {
*i = _projectInterval(r, c, *f, worldRadius);
}
}
}
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);
}
