Subversion Repositories seema-scanner

Rev

Rev 207 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 207 Rev 236
Line 261... Line 261...
261 
//cvtools::writeMat(code1Gray, "code1Gray.mat", "code1Gray");
 261 
//cvtools::writeMat(code1Gray, "code1Gray.mat", "code1Gray");
262 
//cvtools::writeMat(code0Binary, "code0Binary.mat", "code0Binary");
 262 
//cvtools::writeMat(code0Binary, "code0Binary.mat", "code0Binary");
263 
//cvtools::writeMat(code1Binary, "code1Binary.mat", "code1Binary");
 263 
//cvtools::writeMat(code1Binary, "code1Binary.mat", "code1Binary");
264 
 
 264 
 
265 
    // matching
 265 
    // matching
266 
    std::vector<cv::Vec2f> q0Rect, q1Rect;
 266 
    std::vector<cv::Vec2f> q0, q1;
267 
    for(unsigned int s=0; s<nLineShifts; s++){
 267 
    for(unsigned int s=0; s<nLineShifts; s++){
268 
 
 268 
 
269 
        cv::Mat lines0 = frames0LineShift[s];
 269 
        cv::Mat lines0 = frames0LineShift[s];
270 
        cv::Mat lines1 = frames1LineShift[s];
 270 
        cv::Mat lines1 = frames1LineShift[s];
271 
 
 271 
 
Line 291... Line 291...
291 
            // match and store
 291 
            // match and store
292 
            unsigned int i=0, j=0;
 292 
            unsigned int i=0, j=0;
293 
            while(i<lineCenters0.size() && j<lineCenters1.size()){
 293 
            while(i<lineCenters0.size() && j<lineCenters1.size()){
294 
 
 294 
 
295 
                if(lineCenters0[i][1] == lineCenters1[j][1]){
 295 
                if(lineCenters0[i][1] == lineCenters1[j][1]){
296 
                    q0Rect.push_back(cv::Point2f(lineCenters0[i][0], row));
 296 
                    q0.push_back(cv::Point2f(lineCenters0[i][0], row));
297 
                    q1Rect.push_back(cv::Point2f(lineCenters1[j][0], row));
 297 
                    q1.push_back(cv::Point2f(lineCenters1[j][0], row));
298 
                    i += 1;
 298 
                    i += 1;
299 
                    j += 1;
 299 
                    j += 1;
300 
                } else if(lineCenters0[i][1] < lineCenters1[j][1]){
 300 
                } else if(lineCenters0[i][1] < lineCenters1[j][1]){
301 
                    i += 1;
 301 
                    i += 1;
302 
                } else if(lineCenters0[i][1] > lineCenters1[j][1]){
 302 
                } else if(lineCenters0[i][1] > lineCenters1[j][1]){
Line 305... Line 305...
305 
            }
 305 
            }
306 
 
 306 
 
307 
        }
 307 
        }
308 
    }
 308 
    }
309 
 
 309 
 
310 
    int nMatches = q0Rect.size();
 310 
    int nMatches = q0.size();
311 
 
 311 
 
312 
    if(nMatches < 1){
 312 
    if(nMatches < 1){
313 
        Q.resize(0);
 313 
        Q.resize(0);
314 
        color.resize(0);
 314 
        color.resize(0);
315 
 
 315 
 
Line 318... Line 318...
318 
 
 318 
 
319 
    // retrieve color information (at integer coordinates)
 319 
    // retrieve color information (at integer coordinates)
320 
    color.resize(nMatches);
 320 
    color.resize(nMatches);
321 
    for(int i=0; i<nMatches; i++){
 321 
    for(int i=0; i<nMatches; i++){
322 
 
 322 
 
323 
        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
 323 
        cv::Vec3b c0 = color0Rect.at<cv::Vec3b>(q0[i][1], q0[i][0]);
324 
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
 324 
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1[i][1], q1[i][0]);
325 
//        cv::Vec3b c0 = getColorSubpix(color0Rect, q0Rect[i]);
 325 
//        cv::Vec3b c0 = getColorSubpix(color0Rect, q0[i]);
326 
//        cv::Vec3b c1 = getColorSubpix(color1Rect, q0Rect[i]);
 326 
//        cv::Vec3b c1 = getColorSubpix(color1Rect, q0[i]);
327 
 
 327 
 
328 
        color[i] = 0.5*c0 + 0.5*c1;
 328 
        color[i] = 0.5*c0 + 0.5*c1;
329 
    }
 329 
    }
330 
 
 330 
 
331 
    // triangulate points
 331 
    // Triangulate by means of disparity projection
332 
    cv::Mat QMatHomogenous, QMat;
 332 
    Q.resize(q0.size());
333 
 
 - 
 
334 
    cv::triangulatePoints(P0, P1, q0Rect, q1Rect, QMatHomogenous);
 333 
    cv::Matx44f QRectx = cv::Matx44f(QRect);
335 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
 334 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
336 
 
 335 
 
337 
    // undo rectifying rotation
 336 
    #pragma omp parallel for
338 
    cv::Mat R0Inv;
 337 
    for(unsigned int i=0; i<q0.size(); i++){
339 
    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
 338 
        float disparity = q0[i][0]-q1[i][0];
- 
 
 339 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
340 
    QMat = R0Inv*QMat;
 340 
        float winv = float(1.0)/Qih[3];
341 
 
 - 
 
342 
    cvtools::matToPoints3f(QMat, Q);
 341 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
- 
 
 342 
    }
343 
 
 343 
 
344 
}
 344 
}