void PixelMapper::mergeProjections(Eigen::MatrixXf& depthImage, Eigen::MatrixXi& indexImage,
Eigen::MatrixXf* depths, Eigen::MatrixXi* indices, int numImages){
assert (numImages>0);
int rows=depths[0].rows();
int cols=depths[0].cols();
depthImage.resize(indexImage.rows(), indexImage.cols());
depthImage.fill(std::numeric_limits<float>::max());
indexImage.resize(rows, cols);
indexImage.fill(-1);
#pragma omp parallel for
for (int c=0; c<cols; c++){
int* destIndexPtr = &indexImage.coeffRef(0,c);
float* destDepthPtr = &depthImage.coeffRef(0,c);
int* srcIndexPtr[numImages];
float* srcDepthPtr[numImages];
for (int i=0; i<numImages; i++){
srcIndexPtr[i] = &indices[i].coeffRef(0,c);
srcDepthPtr[i] = &depths[i].coeffRef(0,c);
}
for (int r=0; r<rows; r++){
for (int i=0; i<numImages; i++){
if (*destDepthPtr>*srcDepthPtr[i]){
*destDepthPtr = *srcDepthPtr[i];
*destIndexPtr = *srcIndexPtr[i];
}
srcDepthPtr[i]++;
srcIndexPtr[i]++;
}
destDepthPtr++;
destIndexPtr++;
}
}
}
void PointProjector::project(Eigen::MatrixXi &indexImage,
Eigen::MatrixXf &depthImage,
const HomogeneousPoint3fVector &points) const {
depthImage.resize(indexImage.rows(), indexImage.cols());
depthImage.fill(std::numeric_limits<float>::max());
indexImage.fill(-1);
const HomogeneousPoint3f* point = &points[0];
for (size_t i=0; i<points.size(); i++, point++){
int x, y;
float d;
if (!project(x, y, d, *point) ||
x<0 || x>=indexImage.rows() ||
y<0 || y>=indexImage.cols() )
continue;
float& otherDistance=depthImage(x,y);
int& otherIndex=indexImage(x,y);
if (otherDistance>d) {
otherDistance = d;
otherIndex = i;
}
}
}