60 const int xL = (int)floor(x);
61 const int xH = (int)ceil(x);
62 const int yL = (int)floor(y);
63 const int yH = (int)ceil(y);
65 if(xL < 0 || yL < 0 || xH >= in.cols || yH >= in.rows)
70 const uint16_t pLT = in.at<uint16_t>(yL, xL);
71 const uint16_t pRT = in.at<uint16_t>(yL, xH);
72 const uint16_t pLB = in.at<uint16_t>(yH, xL);
73 const uint16_t pRB = in.at<uint16_t>(yH, xH);
78 int count = vLT + vRT + vLB + vRB;
85 const uint16_t avg = (pLT + pRT + pLB + pRB) / count;
86 const uint16_t thres = 0.01 * avg;
87 vLT = abs(pLT - avg) < thres;
88 vRT = abs(pRT - avg) < thres;
89 vLB = abs(pLB - avg) < thres;
90 vRB = abs(pRB - avg) < thres;
91 count = vLT + vRT + vLB + vRB;
98 double distXL = x - xL;
99 double distXH = 1.0 - distXL;
100 double distYL = y - yL;
101 double distYH = 1.0 - distYL;
106 const double tmp = sqrt(2.0);
107 const double fLT = vLT ? tmp - sqrt(distXL + distYL) : 0;
108 const double fRT = vRT ? tmp - sqrt(distXH + distYL) : 0;
109 const double fLB = vLB ? tmp - sqrt(distXL + distYH) : 0;
110 const double fRB = vRB ? tmp - sqrt(distXH + distYH) : 0;
111 const double sum = fLT + fRT + fLB + fRB;
113 return ((pLT * fLT + pRT * fRT + pLB * fLB + pRB * fRB) / sum) + 0.5;
119 #pragma omp parallel for 122 uint16_t *itO = scaled.ptr<uint16_t>(r);
123 const float *itX =
mapX.ptr<
float>(r);
124 const float *itY =
mapY.ptr<
float>(r);
125 for(
size_t c = 0; c < (size_t)
sizeRegistered.width; ++c, ++itO, ++itX, ++itY)
136 #pragma omp parallel for 139 const uint16_t *itD = scaled.ptr<uint16_t>(r);
140 const double y =
lookupY.at<
double>(0, r);
141 const double *itX =
lookupX.ptr<
double>();
143 for(
size_t c = 0; c < (size_t)
sizeRegistered.width; ++c, ++itD, ++itX)
145 const double depthValue = *itD / 1000.0;
147 if(depthValue < zNear || depthValue >
zFar)
152 Eigen::Vector4d pointD(*itX * depthValue, y * depthValue, depthValue, 1);
153 Eigen::Vector4d pointP =
proj * pointD;
155 const double z = pointP[2];
157 const double invZ = 1 / z;
158 const int xP = (
fx * pointP[0]) * invZ +
cx;
159 const int yP = (
fy * pointP[1]) * invZ +
cy;
161 if(xP >= 0 && xP < sizeRegistered.width && yP >= 0 && yP <
sizeRegistered.height)
163 const uint16_t z16 = z * 1000;
164 uint16_t &zReg = registered.at<uint16_t>(yP, xP);
165 if(zReg == 0 || z16 < zReg)
bool init(const int deviceId)
void projectDepth(const cv::Mat &scaled, cv::Mat ®istered) const
cv::Mat cameraMatrixRegistered
bool registerDepth(const cv::Mat &depth, cv::Mat ®istered)
void remapDepth(const cv::Mat &depth, cv::Mat &scaled) const
uint16_t interpolate(const cv::Mat &in, const float &x, const float &y) const