| Line 237... |
Line 237... |
| 237 |
// interpolate horizontal coordinate
|
237 |
// interpolate horizontal coordinate
|
| 238 |
float row = matches[i].row;
|
238 |
float row = matches[i].row;
|
| 239 |
float col = matches[i].col;
|
239 |
float col = matches[i].col;
|
| 240 |
float colRight = col+1;
|
240 |
float colRight = col+1;
|
| 241 |
|
241 |
|
| 242 |
float posHorz = frames[2*levelHorz+2].at<char>(row, col);
|
242 |
float posHorz = frames[2*levelHorz+2].at<unsigned short>(row, col);
|
| 243 |
float negHorz = frames[2*levelHorz+3].at<char>(row, col);
|
243 |
float negHorz = frames[2*levelHorz+3].at<unsigned short>(row, col);
|
| 244 |
float posHorzRight = frames[2*levelHorz+2].at<char>(row, colRight);
|
244 |
float posHorzRight = frames[2*levelHorz+2].at<unsigned short>(row, colRight);
|
| 245 |
float negHorzRight = frames[2*levelHorz+3].at<char>(row, colRight);
|
245 |
float negHorzRight = frames[2*levelHorz+3].at<unsigned short>(row, colRight);
|
| 246 |
|
246 |
|
| 247 |
float x = col + (posHorz - negHorz)/(negHorzRight - negHorz - posHorzRight + posHorz);
|
247 |
float x = col + (posHorz - negHorz)/(negHorzRight - negHorz - posHorzRight + posHorz);
|
| 248 |
|
248 |
|
| 249 |
// interpolate vertical coordinate
|
249 |
// interpolate vertical coordinate
|
| 250 |
float rowBelow = row+1;
|
250 |
float rowBelow = row+1;
|
| 251 |
|
251 |
|
| 252 |
float posVert = frames[2*NbitsHorz+2*levelVert+2].at<char>(row, col);
|
252 |
float posVert = frames[2*NbitsHorz+2*levelVert+2].at<unsigned short>(row, col);
|
| 253 |
float negVert = frames[2*NbitsHorz+2*levelVert+3].at<char>(row, col);
|
253 |
float negVert = frames[2*NbitsHorz+2*levelVert+3].at<unsigned short>(row, col);
|
| 254 |
float posVertBelow = frames[2*NbitsHorz+2*levelVert+2].at<char>(rowBelow, col);
|
254 |
float posVertBelow = frames[2*NbitsHorz+2*levelVert+2].at<unsigned short>(rowBelow, col);
|
| 255 |
float negVertBelow = frames[2*NbitsHorz+2*levelVert+3].at<char>(rowBelow, col);
|
255 |
float negVertBelow = frames[2*NbitsHorz+2*levelVert+3].at<unsigned short>(rowBelow, col);
|
| 256 |
|
256 |
|
| 257 |
float y = row + (posVert - negVert)/(negVertBelow - negVert - posVertBelow + posVert);
|
257 |
float y = row + (posVert - negVert)/(negVertBelow - negVert - posVertBelow + posVert);
|
| 258 |
|
258 |
|
| 259 |
// write into return vector
|
259 |
// write into return vector
|
| 260 |
q[i] = cv::Point2f(x, y);
|
260 |
q[i] = cv::Point2f(x, y);
|
| Line 303... |
Line 303... |
| 303 |
cv::cvtColor(frames1[i], frames1Gray[i], CV_BayerBG2GRAY);
|
303 |
cv::cvtColor(frames1[i], frames1Gray[i], CV_BayerBG2GRAY);
|
| 304 |
}
|
304 |
}
|
| 305 |
|
305 |
|
| 306 |
// colors
|
306 |
// colors
|
| 307 |
cv::Mat color0;
|
307 |
cv::Mat color0;
|
| - |
|
308 |
frames0[0].convertTo(color0, CV_8UC3, 1.0/256.0);
|
| 308 |
cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
|
309 |
cv::cvtColor(color0, color0, CV_BayerBG2RGB);
|
| 309 |
cv::Mat color1;
|
310 |
cv::Mat color1;
|
| - |
|
311 |
frames1[0].convertTo(color1, CV_8UC3, 1.0/256.0);
|
| 310 |
cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
|
312 |
cv::cvtColor(color1, color1, CV_BayerBG2RGB);
|
| 311 |
|
313 |
|
| 312 |
// occlusion masks
|
314 |
// occlusion masks
|
| 313 |
cv::Mat occlusion0, occlusion1;
|
315 |
cv::Mat occlusion0, occlusion1;
|
| 314 |
cv::subtract(frames0Gray[0], frames0Gray[1], occlusion0);
|
316 |
cv::subtract(frames0[0], frames0[1], occlusion0);
|
| 315 |
occlusion0 = occlusion0 > 25;
|
317 |
occlusion0 = (occlusion0 > 6400) & (occlusion0 < 60000);
|
| 316 |
cv::subtract(frames1Gray[0], frames1Gray[1], occlusion1);
|
318 |
cv::subtract(frames1[0], frames1[1], occlusion1);
|
| 317 |
occlusion1 = occlusion1 > 25;
|
319 |
occlusion1 = (occlusion1 > 6400) & (occlusion1 < 60000);
|
| 318 |
|
320 |
|
| 319 |
// erode occlusion masks
|
321 |
// erode occlusion masks
|
| 320 |
cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3));
|
322 |
cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,3));
|
| 321 |
cv::erode(occlusion0, occlusion0, strel);
|
323 |
cv::erode(occlusion0, occlusion0, strel);
|
| 322 |
cv::erode(occlusion1, occlusion1, strel);
|
324 |
cv::erode(occlusion1, occlusion1, strel);
|
| Line 331... |
Line 333... |
| 331 |
cv::Mat code1Vert(frameRows, frameCols, CV_32S, cv::Scalar(0));
|
333 |
cv::Mat code1Vert(frameRows, frameCols, CV_32S, cv::Scalar(0));
|
| 332 |
|
334 |
|
| 333 |
// horizontal codes into gray code
|
335 |
// horizontal codes into gray code
|
| 334 |
for(int i=0; i<NbitsHorz; i++){
|
336 |
for(int i=0; i<NbitsHorz; i++){
|
| 335 |
cv::Mat bit0;
|
337 |
cv::Mat bit0;
|
| 336 |
cv::subtract(frames0Gray[i*2+2], frames0Gray[i*2+3], bit0);
|
338 |
cv::compare(frames0Gray[i*2+2], frames0Gray[i*2+3], bit0, cv::CMP_GT);
|
| 337 |
bit0 = bit0 > 0;
|
- |
|
| 338 |
bit0.convertTo(bit0, CV_32S, 1.0/255.0);
|
339 |
bit0.convertTo(bit0, CV_32S, 1.0/255.0);
|
| 339 |
cv::add(code0Horz, bit0*twopowi(NbitsHorz-i-1), code0Horz, cv::Mat(), CV_32S);
|
340 |
cv::add(code0Horz, bit0*twopowi(NbitsHorz-i-1), code0Horz, cv::Mat(), CV_32S);
|
| 340 |
|
341 |
|
| 341 |
cv::Mat bit1;
|
342 |
cv::Mat bit1;
|
| 342 |
cv::subtract(frames1Gray[i*2+2], frames1Gray[i*2+3], bit1);
|
343 |
cv::compare(frames1Gray[i*2+2], frames1Gray[i*2+3], bit1, cv::CMP_GT);
|
| 343 |
bit1 = bit1 > 0;
|
- |
|
| 344 |
bit1.convertTo(bit1, CV_32S, 1.0/255.0);
|
344 |
bit1.convertTo(bit1, CV_32S, 1.0/255.0);
|
| 345 |
cv::add(code1Horz, bit1*twopowi(NbitsHorz-i-1), code1Horz, cv::Mat(), CV_32S);
|
345 |
cv::add(code1Horz, bit1*twopowi(NbitsHorz-i-1), code1Horz, cv::Mat(), CV_32S);
|
| 346 |
}
|
346 |
}
|
| 347 |
|
347 |
|
| 348 |
// vertical codes into gray code
|
348 |
// vertical codes into gray code
|
| 349 |
for(int i=0; i<NbitsVert; i++){
|
349 |
for(int i=0; i<NbitsVert; i++){
|
| 350 |
cv::Mat bit0;
|
350 |
cv::Mat bit0;
|
| 351 |
cv::subtract(frames0Gray[i*2+2*NbitsHorz+2], frames0Gray[i*2+2*NbitsHorz+3], bit0);
|
351 |
cv::compare(frames0Gray[i*2+2*NbitsHorz+2], frames0Gray[i*2+2*NbitsHorz+3], bit0, cv::CMP_GT);
|
| 352 |
bit0 = bit0 > 0;
|
- |
|
| 353 |
bit0.convertTo(bit0, CV_32S, 1.0/255.0);
|
352 |
bit0.convertTo(bit0, CV_32S, 1.0/255.0);
|
| 354 |
cv::add(code0Vert, bit0*twopowi(NbitsVert-i-1), code0Vert, cv::Mat(), CV_32S);
|
353 |
cv::add(code0Vert, bit0*twopowi(NbitsVert-i-1), code0Vert, cv::Mat(), CV_32S);
|
| 355 |
|
354 |
|
| 356 |
cv::Mat bit1;
|
355 |
cv::Mat bit1;
|
| 357 |
cv::subtract(frames1Gray[i*2+2*NbitsHorz+2], frames1Gray[i*2+2*NbitsHorz+3], bit1);
|
356 |
cv::compare(frames1Gray[i*2+2*NbitsHorz+2], frames1Gray[i*2+2*NbitsHorz+3], bit1, cv::CMP_GT);
|
| 358 |
bit1 = bit1 > 0;
|
- |
|
| 359 |
bit1.convertTo(bit1, CV_32S, 1.0/255.0);
|
357 |
bit1.convertTo(bit1, CV_32S, 1.0/255.0);
|
| 360 |
cv::add(code1Vert, bit1*twopowi(NbitsVert-i-1), code1Vert, cv::Mat(), CV_32S);
|
358 |
cv::add(code1Vert, bit1*twopowi(NbitsVert-i-1), code1Vert, cv::Mat(), CV_32S);
|
| 361 |
}
|
359 |
}
|
| 362 |
|
360 |
|
| 363 |
// set occluded pixels to -1
|
361 |
// set occluded pixels to -1
|
| Line 431... |
Line 429... |
| 431 |
|
429 |
|
| 432 |
color[i] = 0.5*c0 + 0.5*c1;
|
430 |
color[i] = 0.5*c0 + 0.5*c1;
|
| 433 |
}
|
431 |
}
|
| 434 |
|
432 |
|
| 435 |
//cv::correctMatches(calibration.F, q0, q1, q0, q1);
|
433 |
//cv::correctMatches(calibration.F, q0, q1, q0, q1);
|
| 436 |
// correct lens distortion
|
434 |
// correct for lens distortion
|
| 437 |
cv::undistortPoints(q0, q0, calibration.K0, calibration.k0, cv::noArray(), calibration.K0);
|
435 |
cv::undistortPoints(q0, q0, calibration.K0, calibration.k0, cv::noArray(), calibration.K0);
|
| 438 |
cv::undistortPoints(q1, q1, calibration.K1, calibration.k1, cv::noArray(), calibration.K1);
|
436 |
cv::undistortPoints(q1, q1, calibration.K1, calibration.k1, cv::noArray(), calibration.K1);
|
| 439 |
cv::correctMatches(calibration.F, q0, q1, q0, q1);
|
437 |
cv::correctMatches(calibration.F, q0, q1, q0, q1);
|
| 440 |
|
438 |
|
| 441 |
// triangulate points
|
439 |
// triangulate points
|