void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
{
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float & z = depth.at<float>(v, u);
if(uIsNan(z) || z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
z = zf;
}
}
}
else
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(uIsNan(z) || z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);
z = zf*1000;
}
}
}
}
bool Transform::isNull() const
{
return (data_[0] == 0.0f &&
data_[1] == 0.0f &&
data_[2] == 0.0f &&
data_[3] == 0.0f &&
data_[4] == 0.0f &&
data_[5] == 0.0f &&
data_[6] == 0.0f &&
data_[7] == 0.0f &&
data_[8] == 0.0f &&
data_[9] == 0.0f &&
data_[10] == 0.0f &&
data_[11] == 0.0f) ||
uIsNan(data_[0]) ||
uIsNan(data_[1]) ||
uIsNan(data_[2]) ||
uIsNan(data_[3]) ||
uIsNan(data_[4]) ||
uIsNan(data_[5]) ||
uIsNan(data_[6]) ||
uIsNan(data_[7]) ||
uIsNan(data_[8]) ||
uIsNan(data_[9]) ||
uIsNan(data_[10]) ||
uIsNan(data_[11]);
}