void CameraCalibration::StereoCalibration() { vector<vector<Point2f> > ImagePoints[2]; vector<vector<Point3f> > ObjectPoints(1); for(int i=0; i<BoardSize.height; i++) for(int j=0; j<BoardSize.width; j++) ObjectPoints.at(0).push_back(Point3f(float( j*SquareSize ), float( i*SquareSize ), 0)); ObjectPoints.resize(NumFrames, ObjectPoints[0]); vector<Mat> RVecs[2], TVecs[2]; double rms; for(int c_idx=0; c_idx<2; c_idx++) { for(int i=0; i < NumFrames; i++) { Mat img = imread(data_path+"/"+calib_params[c_idx].ImageList.at(i), CV_LOAD_IMAGE_COLOR); vector<Point2f> pointBuf; bool found = false; found = findChessboardCorners(img, BoardSize, pointBuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE); if(found) { Mat viewGray; cvtColor(img, viewGray, CV_BGR2GRAY); cornerSubPix(viewGray, pointBuf, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.01)); //drawChessboardCorners(img, BoardSize, Mat(pointBuf), found); //namedWindow("Image View", CV_WINDOW_AUTOSIZE); //imshow("Image View", img); //waitKey(); } else { cerr << i << "th image cannot be found a pattern." << endl; exit(EXIT_FAILURE); } ImagePoints[c_idx].push_back(pointBuf); } calib_params[c_idx].DistCoeffs = Mat::zeros(8, 1, CV_64F); calib_params[c_idx].CameraMatrix = initCameraMatrix2D(ObjectPoints, ImagePoints[c_idx], ImageSize, 0); rms = calibrateCamera(ObjectPoints, ImagePoints[c_idx], ImageSize, calib_params[c_idx].CameraMatrix, calib_params[c_idx].DistCoeffs, RVecs[c_idx], TVecs[c_idx], CV_CALIB_USE_INTRINSIC_GUESS | CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6); cout << c_idx << " camera re-projection error reported by calibrateCamera: "<< rms << endl; } rms = stereoCalibrate(ObjectPoints, ImagePoints[0], ImagePoints[1], calib_params[0].CameraMatrix, calib_params[0].DistCoeffs, calib_params[1].CameraMatrix, calib_params[1].DistCoeffs, ImageSize, stereo_params->R, stereo_params->T, stereo_params->E, stereo_params->F, TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5)); cout << "Stereo re-projection error reported by stereoCalibrate: " << rms << endl; cout << "Fundamental Matrix reprojection error: " << FundamentalMatrixQuality(ImagePoints[0], ImagePoints[1], calib_params[0].CameraMatrix, calib_params[1].CameraMatrix, calib_params[0].DistCoeffs, calib_params[1].DistCoeffs, stereo_params->F) << endl; // Transfer matrix from OpenCV Mat to Pangolin matrix CvtCameraExtrins(RVecs, TVecs); Timer PangolinTimer; // Stereo rectification stereoRectify(calib_params[0].CameraMatrix, calib_params[0].DistCoeffs, calib_params[1].CameraMatrix, calib_params[1].DistCoeffs, ImageSize, stereo_params->R, stereo_params->T, rect_params->LeftRot, rect_params->RightRot, rect_params->LeftProj, rect_params->RightProj, rect_params->Disp2DepthReProjMat, CALIB_ZERO_DISPARITY, // test later 1, // test later ImageSize, &rect_params->LeftRoi, &rect_params->RightRoi); cout << "\nStereo rectification using calibration spent: " << PangolinTimer.getElapsedTimeInMilliSec() << "ms." << endl; rect_params->isVerticalStereo = fabs(rect_params->RightProj.at<double>(1, 3)) > fabs(rect_params->RightProj.at<double>(0, 3)); // Get the rectification re-map index initUndistortRectifyMap(calib_params[0].CameraMatrix, calib_params[0].DistCoeffs, rect_params->LeftRot, rect_params->LeftProj, ImageSize, CV_16SC2, rect_params->LeftRMAP[0], rect_params->LeftRMAP[1]); initUndistortRectifyMap(calib_params[1].CameraMatrix, calib_params[1].DistCoeffs, rect_params->RightRot, rect_params->RightProj, ImageSize, CV_16SC2, rect_params->RightRMAP[0], rect_params->RightRMAP[1]); }
int Depth_and_Disparity::stereo_match_and_disparity_init(int argc, char** argv, Size img_size) { if(argc < 3) { return 0; } //alg = STEREO_SGBM; alg = STEREO_BM; desired_param_set = 3; // called at the end of this function. minDisparityToCut = 35; // for the threshold cut //default here. runover later in ParamFunction. SADWindowSize = 0; numberOfDisparities = 0; no_display = false; scale = 1.f; target_image_size = img_size; last_disparity_depth= 0; for( int i = 1+2; i < argc; i++ ) { //if( argv[i][0] != '-' ) //{ // /* if( !img1_filename ) // img1_filename = argv[i]; // else // img2_filename = argv[i];*/ //} //else if( strncmp(argv[i], algorithm_opt, strlen(algorithm_opt)) == 0 ) { char* _alg = argv[i] + strlen(algorithm_opt); alg = strcmp(_alg, "bm") == 0 ? STEREO_BM : strcmp(_alg, "sgbm") == 0 ? STEREO_SGBM : strcmp(_alg, "hh") == 0 ? STEREO_HH : strcmp(_alg, "var") == 0 ? STEREO_VAR : -1; if( alg < 0 ) { printf("Command-line parameter error: Unknown stereo algorithm\n\n"); //print_help(); return -1; } } else if( strncmp(argv[i], maxdisp_opt, strlen(maxdisp_opt)) == 0 ) { if( sscanf( argv[i] + strlen(maxdisp_opt), "%d", &numberOfDisparities ) != 1 || numberOfDisparities < 1 || numberOfDisparities % 16 != 0 ) { printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n"); //print_help(); return -1; } } else if( strncmp(argv[i], blocksize_opt, strlen(blocksize_opt)) == 0 ) { if( sscanf( argv[i] + strlen(blocksize_opt), "%d", &SADWindowSize ) != 1 || SADWindowSize < 1 || SADWindowSize % 2 != 1 ) { printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n"); return -1; } } else if( strncmp(argv[i], scale_opt, strlen(scale_opt)) == 0 ) { if( sscanf( argv[i] + strlen(scale_opt), "%f", &scale ) != 1 || scale < 0 ) { printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n"); return -1; } } else if( strcmp(argv[i], nodisplay_opt) == 0 ) no_display = true; else if( strcmp(argv[i], "-i" ) == 0 ) intrinsic_filename = argv[++i]; else if( strcmp(argv[i], "-e" ) == 0 ) extrinsic_filename = argv[++i]; else if( strcmp(argv[i], "-o" ) == 0 ) disparity_filename = argv[++i]; else if( strcmp(argv[i], "-p" ) == 0 ) point_cloud_filename = argv[++i]; else { printf("Command-line parameter error: unknown option %d %s\n", i, argv[i]); //return -1; } } if( (intrinsic_filename != 0) ^ (extrinsic_filename != 0) ) { printf("Command-line parameter error: either both intrinsic and extrinsic parameters must be specified, or none of them (when the stereo pair is already rectified)\n"); return -1; } if( extrinsic_filename == 0 && point_cloud_filename ) { printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n"); return -1; } if( intrinsic_filename ) { // reading intrinsic parameters FileStorage fs(intrinsic_filename, FileStorage::READ); if(!fs.isOpened()) { printf("Failed to open file %s\n", intrinsic_filename); return -1; } fs["M1"] >> M1; fs["D1"] >> D1; fs["M2"] >> M2; fs["D2"] >> D2; M1 *= scale; M2 *= scale; fs.release(); FileStorage fs2(extrinsic_filename, FileStorage::READ); ///fs2.open(extrinsic_filename, FileStorage::READ); if(!fs2.isOpened()) { printf("Failed to open file %s\n", extrinsic_filename); return -1; } fs2["R"] >> R; fs2["T"] >> T; fs2.release(); /* initialize rectification mapping */ /* when calibrated and saving matrices - i calibrated Left camera as img1 , Right camera as img2 */ Size img_ORG_size = Size(320,240); // the images size when calibrated the cameras stereoRectify( M1, D1, M2, D2, img_ORG_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 ); initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12); initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22); } //// set algorithm and parameters : if (alg == STEREO_SGBM) { if (desired_param_set==1) set_SGBM_params_options_1(); else set_SGBM_params_options_2(); } else if (alg == STEREO_BM) { switch (desired_param_set) { case 1: set_BM_params_options_1(); break; case 2: set_BM_params_options_2(); break; case 3: set_BM_params_options_3(); break; default: break; } } else ; //ERROR about alg type return 0; }
void StereoCalib(const vector<string>& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true) { if( imagelist.size() % 2 != 0 ) { cout << "Error: the image list contains odd (non-even) number of elements\n"; return; } printf("board size: %d x %d", boardSize.width, boardSize.height); bool displayCorners = true; const int maxScale = 2; const float squareSize = 1.f; // Set this to your actual square size // ARRAY AND VECTOR STORAGE: vector<vector<Point2f> > imagePoints[2]; vector<vector<Point3f> > objectPoints; Size imageSize; int i, j, k, nimages = (int)imagelist.size()/2; imagePoints[0].resize(nimages); imagePoints[1].resize(nimages); vector<string> goodImageList; for( i = j = 0; i < nimages; i++ ) { for( k = 0; k < 2; k++ ) { const string& filename = imagelist[i*2+k]; Mat img = imread(filename, 0); if(img.empty()) break; if( imageSize == Size() ) imageSize = img.size(); else if( img.size() != imageSize ) { cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n"; break; } bool found = false; vector<Point2f>& corners = imagePoints[k][j]; for( int scale = 1; scale <= maxScale; scale++ ) { Mat timg; if( scale == 1 ) timg = img; else resize(img, timg, Size(), scale, scale); found = findChessboardCorners(timg, boardSize, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE); if( found ) { if( scale > 1 ) { Mat cornersMat(corners); cornersMat *= 1./scale; } break; } } if( displayCorners ) { cout << filename << endl; Mat cimg, cimg1; cvtColor(img, cimg, CV_GRAY2BGR); drawChessboardCorners(cimg, boardSize, corners, found); double sf = 640./MAX(img.rows, img.cols); resize(cimg, cimg1, Size(), sf, sf); imshow("corners", cimg1); char c = (char)waitKey(500); if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit exit(-1); } else putchar('.'); if( !found ) break; cornerSubPix(img, corners, Size(11,11), Size(-1,-1), TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 30, 0.01)); } if( k == 2 ) { goodImageList.push_back(imagelist[i*2]); goodImageList.push_back(imagelist[i*2+1]); j++; } } cout << j << " pairs have been successfully detected.\n"; nimages = j; if( nimages < 2 ) { cout << "Error: too little pairs to run the calibration\n"; return; } imagePoints[0].resize(nimages); imagePoints[1].resize(nimages); objectPoints.resize(nimages); for( i = 0; i < nimages; i++ ) { for( j = 0; j < boardSize.height; j++ ) for( k = 0; k < boardSize.width; k++ ) objectPoints[i].push_back(Point3f(j*squareSize, k*squareSize, 0)); } cout << "Running stereo calibration ...\n"; Mat cameraMatrix[2], distCoeffs[2]; cameraMatrix[0] = Mat::eye(3, 3, CV_64F); cameraMatrix[1] = Mat::eye(3, 3, CV_64F); Mat R, T, E, F; double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1], cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1], imageSize, R, T, E, F, TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + //CV_CALIB_SAME_FOCAL_LENGTH + CV_CALIB_RATIONAL_MODEL + CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5); cout << "done with RMS error=" << rms << endl; // CALIBRATION QUALITY CHECK // because the output fundamental matrix implicitly // includes all the output information, // we can check the quality of calibration using the // epipolar geometry constraint: m2^t*F*m1=0 double err = 0; int npoints = 0; vector<Vec3f> lines[2]; for( i = 0; i < nimages; i++ ) { int npt = (int)imagePoints[0][i].size(); Mat imgpt[2]; for( k = 0; k < 2; k++ ) { imgpt[k] = Mat(imagePoints[k][i]); undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]); computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]); } for( j = 0; j < npt; j++ ) { double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] + imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + fabs(imagePoints[1][i][j].x*lines[0][j][0] + imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]); err += errij; } npoints += npt; } cout << "average reprojection err = " << err/npoints << endl; // save intrinsic parameters FileStorage fs("calib/intrinsics.yml", CV_STORAGE_WRITE); if( fs.isOpened() ) { fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] << "M2" << cameraMatrix[1] << "D2" << distCoeffs[1]; fs.release(); } else cout << "Error: can not save the intrinsic parameters\n"; Mat R1, R2, P1, P2, Q; Rect validRoi[2]; stereoRectify(cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1], imageSize, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]); fs.open("calib/extrinsics.yml", CV_STORAGE_WRITE); if( fs.isOpened() ) { fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q; fs.release(); } else cout << "Error: can not save the intrinsic parameters\n"; // OpenCV can handle left-right // or up-down camera arrangements bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3)); // COMPUTE AND DISPLAY RECTIFICATION if( !showRectified ) return; Mat rmap[2][2]; // IF BY CALIBRATED (BOUGUET'S METHOD) if( useCalibrated ) { // we already computed everything } // OR ELSE HARTLEY'S METHOD else // use intrinsic parameters of each camera, but // compute the rectification transformation directly // from the fundamental matrix { vector<Point2f> allimgpt[2]; for( k = 0; k < 2; k++ ) { for( i = 0; i < nimages; i++ ) std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k])); } F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0); Mat H1, H2; stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3); R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0]; R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1]; P1 = cameraMatrix[0]; P2 = cameraMatrix[1]; } //Precompute maps for cv::remap() initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]); initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]); Mat canvas; double sf; int w, h; if( !isVerticalStereo ) { sf = 600./MAX(imageSize.width, imageSize.height); w = cvRound(imageSize.width*sf); h = cvRound(imageSize.height*sf); canvas.create(h, w*2, CV_8UC3); } else { sf = 300./MAX(imageSize.width, imageSize.height); w = cvRound(imageSize.width*sf); h = cvRound(imageSize.height*sf); canvas.create(h*2, w, CV_8UC3); } for( i = 0; i < nimages; i++ ) { for( k = 0; k < 2; k++ ) { Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg; remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR); cvtColor(rimg, cimg, CV_GRAY2BGR); Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h)); resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA); if( useCalibrated ) { Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf), cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf)); rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8); } } if( !isVerticalStereo ) for( j = 0; j < canvas.rows; j += 16 ) line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8); else for( j = 0; j < canvas.cols; j += 16 ) line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8); imshow("rectified", canvas); char c = (char)waitKey(); if( c == 27 || c == 'q' || c == 'Q' ) break; } }
int main(int argc, char *argv[]) { gflags::ParseCommandLineFlags(&argc, &argv, true); cv::vector<cv::Mat> lefts, rights; const cv::Size patternSize(9, 6); cv::vector<cv::vector<cv::Point3f>> worldPoints; cv::vector<cv::vector<cv::vector<cv::Point2f>>> imagePoints(2); for (size_t i = 0; i < 2; i++) imagePoints[i].resize(FLAGS_size); loadImages(lefts, rights, FLAGS_size); FLAGS_size = findChessboards(lefts, rights, imagePoints, patternSize, FLAGS_size); std::cout << "number of correct files = " << FLAGS_size << std::endl; setWorldPoints(worldPoints, patternSize, 0.024, FLAGS_size); std::cout << "calibrate stereo cameras" << std::endl; cv::vector<cv::Mat> cameraMatrix(2); cv::vector<cv::Mat> distCoeffs(2); cameraMatrix[0] = cv::Mat::eye(3, 3, CV_64FC1); cameraMatrix[1] = cv::Mat::eye(3, 3, CV_64FC1); distCoeffs[0] = cv::Mat(8, 1, CV_64FC1); distCoeffs[1] = cv::Mat(8, 1, CV_64FC1); cv::Mat R, T, E, F; double rms = stereoCalibrate( worldPoints, imagePoints[0], imagePoints[1], cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1], lefts[0].size(), R, T, E, F, cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH + CV_CALIB_RATIONAL_MODEL + CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5); std::cout << "done with RMS error = " << rms << std::endl; double err = 0; int npoints = 0; for (int i = 0; i < FLAGS_size; i++) { int size = (int) imagePoints[0][i].size(); cv::vector<cv::Vec3f> lines[2]; cv::Mat imgpt[2]; for (int k = 0; k < 2; k++) { imgpt[k] = cv::Mat(imagePoints[k][i]); cv::undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], cv::Mat(), cameraMatrix[k]); cv::computeCorrespondEpilines(imgpt[k], k + 1, F, lines[k]); } for (int j = 0; j < size; j++) { double errij = std::fabs(imagePoints[0][i][j].x * lines[1][j][0] + imagePoints[0][i][j].y * lines[1][j][1] + lines[1][j][2]) + std::fabs(imagePoints[1][i][j].x * lines[0][j][0] + imagePoints[1][i][j].y * lines[0][j][1] + lines[0][j][2]); err += errij; } npoints += size; } std::cout << "average reprojection error = " << err / npoints << std::endl; cv::Mat R1, R2, P1, P2, Q; cv::Rect validROI[2]; stereoRectify(cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1], lefts[0].size(), R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 1, lefts[0].size(), &validROI[0], &validROI[1]); { cv::FileStorage fs(FLAGS_intrinsics.c_str(), cv::FileStorage::WRITE); if (fs.isOpened()) { fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] << "M2" << cameraMatrix[1] << "D2" << distCoeffs[1]; fs.release(); } } cv::Mat rmap[2][2]; cv::initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, lefts[0].size(), CV_16SC2, rmap[0][0], rmap[0][1]); cv::initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, lefts[0].size(), CV_16SC2, rmap[1][0], rmap[1][1]); { cv::FileStorage fs(FLAGS_extrinsics.c_str(), cv::FileStorage::WRITE); if (fs.isOpened()) { fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q << "V1" << validROI[0] << "V2" << validROI[1]; fs.release(); } } cv::Mat canvas; double sf; int w, h; sf = 600. / MAX(lefts[0].size().width, lefts[0].size().height); w = cvRound(lefts[0].size().width * sf); h = cvRound(lefts[0].size().height * sf); canvas.create(h, w * 2, CV_8UC3); cv::namedWindow("Rectified", CV_WINDOW_AUTOSIZE | CV_WINDOW_FREERATIO); for (int i = 0; i < FLAGS_size; i++) { for (int k = 0; k < 2; k++) { if (k == 0) { cv::Mat img = lefts[i].clone(), rimg, cimg; cv::remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR); cv::cvtColor(rimg, cimg, CV_GRAY2BGR); cv::Mat canvasPart = canvas(cv::Rect(w * k, 0, w, h)); cv::resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA); cv::Rect vroi(cvRound(validROI[k].x * sf), cvRound(validROI[k].y * sf), cvRound(validROI[k].width * sf), cvRound(validROI[k].height * sf)); cv::rectangle(canvasPart, vroi, cv::Scalar(0, 0, 255), 3, 8); } else { cv::Mat img = rights[i].clone(), rimg, cimg; cv::remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR); cvtColor(rimg, cimg, CV_GRAY2BGR); cv::Mat canvasPart = canvas(cv::Rect(w * k, 0, w, h)); cv::resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA); cv::Rect vroi(cvRound(validROI[k].x * sf), cvRound(validROI[k].y * sf), cvRound(validROI[k].width * sf), cvRound(validROI[k].height * sf)); cv::rectangle(canvasPart, vroi, cv::Scalar(0, 0, 255), 3, 8); } } for (int j = 0; j < canvas.rows; j += 16) cv::line(canvas, cv::Point(0, j), cv::Point(canvas.cols, j), cv::Scalar(0, 255, 0), 1, 8); cv::imshow("Rectified", canvas); if (cv::waitKey(0) == 'q') break; } cv::destroyAllWindows(); return 0; }
// Receives cartesian images and calibration matrices StereoData PeripheralStereo::computeStereo(const cv::Mat & left_image, const cv::Mat & right_image, const cv::Mat & R1, const cv::Mat & R2, const cv::Mat & P1_, const cv::Mat & P2_, const cv::Mat & left_to_center) { cv::Mat Q=makeProjectionMatrix(P2_,1.0,1.0); std::cout << "Q:" << Q << std::endl; /* EMULATE HUMAN EYE (START) */ // Convert cartesian to cortical /*sensor.stereo_data.left_cortical_image=sensor.to_cortical(left_image); sensor.stereo_data.right_cortical_image=sensor.to_cortical(right_image); // Convert cortical to cartesian sensor.stereo_data.left_retinal_image=sensor.to_cartesian(sensor.stereo_data.left_cortical_image); sensor.stereo_data.right_retinal_image=sensor.to_cartesian(sensor.stereo_data.right_cortical_image);//*/ /* EMULATE HUMAN EYE (FINISH) */ cv::Mat rectified_left_image; cv::Mat rectified_right_image; stereoRectify(sensor.stereo_data.left_retinal_image, sensor.stereo_data.right_retinal_image, R1, R2, P1_, P2_, rectified_left_image, rectified_right_image); //////////////////////////////////////////////////////////////////////////////// // Compute Stereo (MARTELADO NO CARTESIANO, IDEALMENTE DEVIA SER NO CORTICAL) // //////////////////////////////////////////////////////////////////////////////// getCartesianDisparityMap(rectified_left_image, rectified_right_image, sensor.stereo_data.disparity_image, sensor.stereo_data.disparity_values); cv::Mat R1_ = cv::Mat::eye(4,4,CV_64F); R1.copyTo(R1_(cv::Range(0,3), cv::Range(0,3))); //cv::Mat transf_=left_to_center*R1_.t(); cv::Mat transf_=R1_.t(); get3DpointCloud(disparity32F,sensor.stereo_data.point_cloud_cartesian,transf_,Q); sensor.stereo_data.point_cloud_rgb=rectified_left_image; //sensor.stereo_data.point_cloud_rgb=sensor.to_cortical(sensor.stereo_data.point_cloud_rgb); /////////////////////////// // Propagate uncertainty // /////////////////////////// std::cout << "propagate uncertainty" << std::endl; sensor.computeUncertainty(sensor.stereo_data.disparity_values, H1, H2, stereo_rectification_map1_left, stereo_rectification_map2_left, stereo_rectification_map1_right, stereo_rectification_map2_right, trans2.at<double>(0,0)); return sensor.stereo_data; }
void RectifyStereo::calRemapMatrix(){ Mat K_00, D_00, R_rect_00, P_rect_00; Mat K_01, D_01, R_rect_01, P_rect_01; Size S_00; Mat R_01, T_01; // Matlab computes these K_00 = Mat(SQRT_SIZE_K, SQRT_SIZE_K, CV_64FC1, calibData.K_00); D_00 = Mat(1, SIZE_D, CV_64FC1, calibData.D_00); K_01 = Mat(SQRT_SIZE_K, SQRT_SIZE_K, CV_64FC1, calibData.K_01); D_01 = Mat(1, SIZE_D, CV_64FC1, calibData.D_01); S_00 = Size(calibData.S_00[0], calibData.S_00[1]); R_01 = Mat(3, 3, CV_64FC1, calibData.R_01); T_01 = Mat(3, 1, CV_64FC1, calibData.T_01); // Check if R_rect_0X, P_rect_0X are valide. bool checkRP = true; for (int i = 0; i < SIZE_R && checkRP; i++) { if (abs(calibData.R_rect_00[i]) > 5000 || abs(calibData.R_rect_01[i]) > 5000) checkRP = false; } for (int i = 0; i < SIZE_P_ROWS * SIZE_P_COLS && checkRP; i++) { if (abs(calibData.P_rect_00[i]) > 5000 || abs(calibData.P_rect_01[i]) > 5000) checkRP = false; } Size S_rect_00; // If valide, read them directly if (checkRP == true) { R_rect_00 = Mat(SQRT_SIZE_R, SQRT_SIZE_R, CV_64FC1, calibData.R_rect_00); P_rect_00 = Mat(SIZE_P_ROWS, SIZE_P_COLS, CV_64FC1, calibData.P_rect_00); R_rect_01 = Mat(SQRT_SIZE_R, SQRT_SIZE_R, CV_64FC1, calibData.R_rect_01); P_rect_01 = Mat(SIZE_P_ROWS, SIZE_P_COLS, CV_64FC1, calibData.P_rect_01); S_rect_00 = Size(calibData.S_rect_00[0], calibData.S_rect_00[1]); //int _y = calibData.ROI_00[1]; //if (_y < calibData.ROI_01[1]) // _y = calibData.ROI_01[1]; //int _y2 = calibData.ROI_00[1] + calibData.ROI_00[3]; //if (_y2 > calibData.ROI_01[1] + calibData.ROI_01[3]) // _y2 = calibData.ROI_01[1] + calibData.ROI_01[3]; //int _width = calibData.ROI_00[2]; //if (_width > calibData.ROI_01[2]) // _width = calibData.ROI_01[2]; //roi[0] = Rect(calibData.ROI_00[0], _y, _width, _y2 - _y); //roi[1] = Rect(calibData.ROI_01[0], _y, _width, _y2 - _y); } // If not, compute them else { S_rect_00 = S_00; Mat Q; // Rectify and compute the rest camera parameters stereoRectify(K_00, D_00, K_01, D_01, S_00, R_01, T_01, //input R_rect_00, R_rect_01, P_rect_00, P_rect_01, // output Q, CV_CALIB_ZERO_DISPARITY, 0, S_rect_00, &roi[0], &roi[1]); //output memcpy(calibData.R_rect_00, (double*)R_rect_00.data, sizeof(calibData.R_rect_00)); memcpy(calibData.P_rect_00, (double*)P_rect_00.data, sizeof(calibData.P_rect_00)); memcpy(calibData.R_rect_01, (double*)R_rect_01.data, sizeof(calibData.R_rect_01)); memcpy(calibData.P_rect_01, (double*)P_rect_01.data, sizeof(calibData.P_rect_01)); calibData.ROI_00[0] = roi[0].x, calibData.ROI_00[1] = roi[0].y; calibData.ROI_00[2] = roi[0].width, calibData.ROI_00[3] = roi[0].height; calibData.ROI_01[0] = roi[1].x, calibData.ROI_01[1] = roi[1].y; calibData.ROI_01[2] = roi[1].width, calibData.ROI_01[3] = roi[1].height; calibData.S_rect_00[0] = S_rect_00.width, calibData.S_rect_00[1] = S_rect_00.height; calibData.S_rect_01[0] = S_rect_00.width, calibData.S_rect_01[1] = S_rect_00.height; } if (!rectified) { //compute remapM[2][2] initUndistortRectifyMap( K_00, D_00, R_rect_00, P_rect_00, S_rect_00, CV_32FC1, remapM[0][0], remapM[0][1]); initUndistortRectifyMap( K_01, D_01, R_rect_01, P_rect_01, S_rect_00, CV_32FC1, remapM[1][0], remapM[1][1]); } }