| Line 81... |
Line 81... |
| 81 |
std::vector<cv::Vec4i>::iterator it;
|
81 |
std::vector<cv::Vec4i>::iterator it;
|
| 82 |
it = std::unique(edges.begin(), edges.end(), sortingEqual);
|
82 |
it = std::unique(edges.begin(), edges.end(), sortingEqual);
|
| 83 |
edges.resize(std::distance(edges.begin(),it));
|
83 |
edges.resize(std::distance(edges.begin(),it));
|
| 84 |
}
|
84 |
}
|
| 85 |
|
85 |
|
| 86 |
void AlgorithmGrayCode::get3DPoints(const SMCalibrationParameters &calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3b>& color){
|
86 |
void AlgorithmGrayCode::get3DPoints(const SMCalibrationParameters &calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3f>& color){
|
| 87 |
|
87 |
|
| 88 |
assert(frames0.size() == N);
|
88 |
assert(frames0.size() == N);
|
| 89 |
assert(frames1.size() == N);
|
89 |
assert(frames1.size() == N);
|
| 90 |
|
90 |
|
| 91 |
int frameRows = frames0[0].rows;
|
91 |
int frameRows = frames0[0].rows;
|
| Line 108... |
Line 108... |
| 108 |
// gray-scale and remap
|
108 |
// gray-scale and remap
|
| 109 |
std::vector<cv::Mat> frames0Rect(N);
|
109 |
std::vector<cv::Mat> frames0Rect(N);
|
| 110 |
std::vector<cv::Mat> frames1Rect(N);
|
110 |
std::vector<cv::Mat> frames1Rect(N);
|
| 111 |
for(unsigned int i=0; i<N; i++){
|
111 |
for(unsigned int i=0; i<N; i++){
|
| 112 |
cv::Mat temp;
|
112 |
cv::Mat temp;
|
| 113 |
cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
|
113 |
cv::cvtColor(frames0[i], temp, CV_RGB2GRAY);
|
| 114 |
cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
|
114 |
cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
|
| 115 |
cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
|
115 |
cv::cvtColor(frames1[i], temp, CV_RGB2GRAY);
|
| 116 |
cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
|
116 |
cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
|
| 117 |
}
|
117 |
}
|
| 118 |
|
118 |
|
| 119 |
#ifdef SM_DEBUG
|
119 |
#ifdef QT_DEBUG
|
| 120 |
cvtools::writeMat(frames0Rect[0], "frames0Rect_0.mat", "frames0Rect_0");
|
120 |
cvtools::writeMat(frames0Rect[0], "frames0Rect_0.mat", "frames0Rect_0");
|
| 121 |
cvtools::writeMat(frames0[0], "frames0_0.mat", "frames0_0");
|
121 |
cvtools::writeMat(frames0[0], "frames0_0.mat", "frames0_0");
|
| 122 |
|
122 |
|
| 123 |
cvtools::writeMat(frames0Rect[22], "frames0Rect_22.mat", "frames0Rect_22");
|
123 |
cvtools::writeMat(frames0Rect[22], "frames0Rect_22.mat", "frames0Rect_22");
|
| 124 |
cvtools::writeMat(frames0Rect[23], "frames0Rect_23.mat", "frames0Rect_23");
|
124 |
cvtools::writeMat(frames0Rect[23], "frames0Rect_23.mat", "frames0Rect_23");
|
| Line 128... |
Line 128... |
| 128 |
|
128 |
|
| 129 |
cv::imwrite("frames1[0].png", frames1[0]);
|
129 |
cv::imwrite("frames1[0].png", frames1[0]);
|
| 130 |
cv::imwrite("frames1Rect[0].png", frames1Rect[0]);
|
130 |
cv::imwrite("frames1Rect[0].png", frames1Rect[0]);
|
| 131 |
#endif
|
131 |
#endif
|
| 132 |
|
132 |
|
| 133 |
// color debayer and remap
|
133 |
// color remap
|
| 134 |
cv::Mat color0Rect, color1Rect;
|
134 |
cv::Mat color0Rect, color1Rect;
|
| 135 |
cv::cvtColor(frames0[0], color0Rect, CV_BayerBG2RGB);
|
- |
|
| 136 |
cv::remap(color0Rect, color0Rect, map0X, map0Y, CV_INTER_LINEAR);
|
135 |
cv::remap(frames0[0], color0Rect, map0X, map0Y, CV_INTER_LINEAR);
|
| 137 |
|
- |
|
| 138 |
cv::cvtColor(frames1[0], color1Rect, CV_BayerBG2RGB);
|
- |
|
| 139 |
cv::remap(color1Rect, color1Rect, map1X, map1Y, CV_INTER_LINEAR);
|
136 |
cv::remap(frames1[0], color1Rect, map1X, map1Y, CV_INTER_LINEAR);
|
| 140 |
|
137 |
|
| 141 |
int frameRectRows = frames0Rect[0].rows;
|
138 |
int frameRectRows = frames0Rect[0].rows;
|
| 142 |
int frameRectCols = frames0Rect[0].cols;
|
139 |
int frameRectCols = frames0Rect[0].cols;
|
| 143 |
|
140 |
|
| 144 |
// occlusion masks
|
141 |
// occlusion masks
|
| 145 |
cv::Mat occlusion0Rect, occlusion1Rect;
|
142 |
cv::Mat occlusion0Rect, occlusion1Rect;
|
| 146 |
cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0Rect);
|
143 |
cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0Rect);
|
| 147 |
occlusion0Rect = (occlusion0Rect > 10) & (occlusion0Rect < 250);
|
144 |
occlusion0Rect = (occlusion0Rect > 0.1) & (occlusion0Rect < 0.99);
|
| 148 |
cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1Rect);
|
145 |
cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1Rect);
|
| 149 |
occlusion1Rect = (occlusion1Rect > 10) & (occlusion1Rect < 250);
|
146 |
occlusion1Rect = (occlusion1Rect > 0.1) & (occlusion1Rect < 0.99);
|
| 150 |
|
147 |
|
| 151 |
// erode occlusion masks
|
148 |
// erode occlusion masks
|
| 152 |
cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2,2));
|
149 |
cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2,2));
|
| 153 |
cv::erode(occlusion0Rect, occlusion0Rect, strel);
|
150 |
cv::erode(occlusion0Rect, occlusion0Rect, strel);
|
| 154 |
cv::erode(occlusion1Rect, occlusion1Rect, strel);
|
151 |
cv::erode(occlusion1Rect, occlusion1Rect, strel);
|
| Line 197... |
Line 194... |
| 197 |
|
194 |
|
| 198 |
//cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
|
195 |
//cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
|
| 199 |
//cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
|
196 |
//cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
|
| 200 |
|
197 |
|
| 201 |
|
198 |
|
| 202 |
#ifdef SM_DEBUG
|
199 |
#ifdef QT_DEBUG
|
| 203 |
// convert to standard binary
|
200 |
// convert to standard binary
|
| 204 |
cv::Mat code0Binary(code0Rect.rows, code0Rect.cols, CV_32F);
|
201 |
cv::Mat code0Binary(code0Rect.rows, code0Rect.cols, CV_32F);
|
| 205 |
cv::Mat code1Binary(code1Rect.rows, code1Rect.cols, CV_32F);
|
202 |
cv::Mat code1Binary(code1Rect.rows, code1Rect.cols, CV_32F);
|
| 206 |
for(int r=0; r<frameRectRows; r++){
|
203 |
for(int r=0; r<frameRectRows; r++){
|
| 207 |
for(int c=0; c<frameRectCols; c++){
|
204 |
for(int c=0; c<frameRectCols; c++){
|
| Line 234... |
Line 231... |
| 234 |
if(occlusion1Rect.at<unsigned char>(r,c) == 0)
|
231 |
if(occlusion1Rect.at<unsigned char>(r,c) == 0)
|
| 235 |
code1Rect.at<int>(r,c) = -1;
|
232 |
code1Rect.at<int>(r,c) = -1;
|
| 236 |
}
|
233 |
}
|
| 237 |
}
|
234 |
}
|
| 238 |
|
235 |
|
| 239 |
#ifdef SM_DEBUG
|
236 |
#ifdef QT_DEBUG
|
| 240 |
cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
|
237 |
cvtools::writeMat(code0Rect, "code0Rect.mat", "code0Rect");
|
| 241 |
cvtools::writeMat*/(code1Rect, "code1Rect.mat", "code1Rect");
|
238 |
cvtools::writeMat(code1Rect, "code1Rect.mat", "code1Rect");
|
| 242 |
#endif
|
239 |
#endif
|
| 243 |
|
240 |
|
| 244 |
// matching
|
241 |
// matching
|
| 245 |
std::vector<cv::Vec2f> q0, q1;
|
242 |
std::vector<cv::Vec2f> q0, q1;
|
| 246 |
for(int row=0; row<frameRectRows; row++){
|
243 |
for(int row=0; row<frameRectRows; row++){
|
| Line 277... |
Line 274... |
| 277 |
|
274 |
|
| 278 |
// refine for camera 0
|
275 |
// refine for camera 0
|
| 279 |
float c0 = matchedEdges0[i][0];
|
276 |
float c0 = matchedEdges0[i][0];
|
| 280 |
float c1 = c0+1;
|
277 |
float c1 = c0+1;
|
| 281 |
|
278 |
|
| 282 |
float pos0 = frames0Rect[2*level+2].at<unsigned char>(row, c0);
|
279 |
float pos0 = frames0Rect[2*level+2].at<float>(row, c0);
|
| 283 |
float pos1 = frames0Rect[2*level+2].at<unsigned char>(row, c1);
|
280 |
float pos1 = frames0Rect[2*level+2].at<float>(row, c1);
|
| 284 |
float neg0 = frames0Rect[2*level+3].at<unsigned char>(row, c0);
|
281 |
float neg0 = frames0Rect[2*level+3].at<float>(row, c0);
|
| 285 |
float neg1 = frames0Rect[2*level+3].at<unsigned char>(row, c1);
|
282 |
float neg1 = frames0Rect[2*level+3].at<float>(row, c1);
|
| 286 |
|
283 |
|
| 287 |
float col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
|
284 |
float col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
|
| 288 |
q0.push_back(cv::Point2f(col, row));
|
285 |
q0.push_back(cv::Point2f(col, row));
|
| 289 |
|
286 |
|
| 290 |
// refine for camera 1
|
287 |
// refine for camera 1
|
| 291 |
c0 = matchedEdges1[i][0];
|
288 |
c0 = matchedEdges1[i][0];
|
| 292 |
c1 = c0+1;
|
289 |
c1 = c0+1;
|
| 293 |
|
290 |
|
| 294 |
pos0 = frames1Rect[2*level+2].at<unsigned char>(row, c0);
|
291 |
pos0 = frames1Rect[2*level+2].at<float>(row, c0);
|
| 295 |
pos1 = frames1Rect[2*level+2].at<unsigned char>(row, c1);
|
292 |
pos1 = frames1Rect[2*level+2].at<float>(row, c1);
|
| 296 |
neg0 = frames1Rect[2*level+3].at<unsigned char>(row, c0);
|
293 |
neg0 = frames1Rect[2*level+3].at<float>(row, c0);
|
| 297 |
neg1 = frames1Rect[2*level+3].at<unsigned char>(row, c1);
|
294 |
neg1 = frames1Rect[2*level+3].at<float>(row, c1);
|
| 298 |
|
295 |
|
| 299 |
col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
|
296 |
col = c0 + (pos0 - neg0)/(neg1 - neg0 - pos1 + pos0);
|
| 300 |
q1.push_back(cv::Point2f(col, row));
|
297 |
q1.push_back(cv::Point2f(col, row));
|
| 301 |
|
298 |
|
| 302 |
}
|
299 |
}
|
| Line 314... |
Line 311... |
| 314 |
|
311 |
|
| 315 |
// retrieve color information (at integer coordinates)
|
312 |
// retrieve color information (at integer coordinates)
|
| 316 |
color.resize(nMatches);
|
313 |
color.resize(nMatches);
|
| 317 |
for(int i=0; i<nMatches; i++){
|
314 |
for(int i=0; i<nMatches; i++){
|
| 318 |
|
315 |
|
| 319 |
cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0[i][1], q0[i][0]);
|
316 |
cv::Vec3f c0 = color0Rect.at<cv::Vec3f>(q0[i][1], q0[i][0]);
|
| 320 |
cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1[i][1], q1[i][0]);
|
317 |
cv::Vec3f c1 = color1Rect.at<cv::Vec3f>(q1[i][1], q1[i][0]);
|
| 321 |
|
318 |
|
| 322 |
color[i] = 0.5*c0 + 0.5*c1;
|
319 |
color[i] = 0.5*c0 + 0.5*c1;
|
| 323 |
}
|
320 |
}
|
| 324 |
|
321 |
|
| 325 |
// Triangulate by means of disparity projection
|
322 |
// Triangulate by means of disparity projection
|