Subversion Repositories seema-scanner

Rev

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

Rev 207 Rev 231
Line 349... Line 349...
349 
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
 349 
        cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
350 
 
 350 
 
351 
        color[i] = 0.5*c0 + 0.5*c1;
 351 
        color[i] = 0.5*c0 + 0.5*c1;
352 
    }
 352 
    }
353 
 
 353 
 
354 
    // triangulate points
 354 
//    // Triangulate points
355 
    cv::Mat QMatHomogenous, QMat;
 355 
//    cv::Mat QMatHomogenous, QMat;
356 
//    cv::Mat C0 = P0.clone();
 356 
////    cv::Mat C0 = P0.clone();
357 
//    cv::Mat C1 = P1.clone();
 357 
////    cv::Mat C1 = P1.clone();
358 
//    C0.colRange(0, 3) = C0.colRange(0, 3)*R0;
 358 
////    C0.colRange(0, 3) = C0.colRange(0, 3)*R0;
359 
//    C1.colRange(0, 3) = C1.colRange(0, 3)*R1.t();
 359 
////    C1.colRange(0, 3) = C1.colRange(0, 3)*R1.t();
360 
    cv::triangulatePoints(P0, P1, q0Rect, q1Rect, QMatHomogenous);
 360 
//    cv::triangulatePoints(P0, P1, q0Rect, q1Rect, QMatHomogenous);
361 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
 361 
//    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
362 
 
 - 
 
363 
    // undo rectifying rotation
 - 
 
364 
    cv::Mat R0Inv;
 - 
 
365 
    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
 - 
 
366 
    QMat = R0Inv*QMat;
 - 
 
367 
 
 362 
 
- 
 
 363 
//    // undo rectifying rotation
- 
 
 364 
//    cv::Mat R0Inv;
- 
 
 365 
//    cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
- 
 
 366 
//    QMat = R0Inv*QMat;
- 
 
 367 
 
368 
    cvtools::matToPoints3f(QMat, Q);
 368 
//    cvtools::matToPoints3f(QMat, Q);
- 
 
 369 
 
- 
 
 370 
 
- 
 
 371 
    // Triangulate by means of disparity projection
- 
 
 372 
    Q.resize(q0Rect.size());
- 
 
 373 
    cv::Matx44f QRectx = cv::Matx44f(QRect);
- 
 
 374 
    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));
- 
 
 375 
 
- 
 
 376 
    #pragma omp parallel for
- 
 
 377 
    for(unsigned int i=0; i<q0Rect.size(); i++){
- 
 
 378 
        float disparity = q0Rect[i][0]-q1Rect[i][0];
- 
 
 379 
        cv::Vec4f Qih = QRectx*cv::Vec4f(q0Rect[i][0], q0Rect[i][1], disparity, 1.0);
- 
 
 380 
        float winv = float(1.0)/Qih[3];
- 
 
 381 
        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
- 
 
 382 
    }
369 
 
 383 
 
370 
}
 384 
}