Unity + OpenCVの仮想Quadrocopter（パート3）

/** @brief             . Size boardSize (9, 6) -    ,      ,  sampleFound        */ void CameraCalibrator::findSample (const cv::Mat& img) { currentSamplePoints.clear(); int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE; sampleFound = findChessboardCorners( img, boardSize, currentSamplePoints, chessBoardFlags); currentImage = &img; } bool CameraCalibrator::isSampleFound () { return sampleFound; } /** @brief     */ void CameraCalibrator::acceptSample () { //      Mat viewGray; cvtColor(*currentImage, viewGray, COLOR_BGR2GRAY); cornerSubPix( viewGray, currentSamplePoints, Size(11,11), Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 )); //      (     ) drawChessboardCorners(*currentImage, boardSize, Mat(currentSamplePoints), sampleFound); //  samplesPoints.push_back(currentSamplePoints); } /** @brief     */ void CameraCalibrator::makeCalibration () { vector<Mat> rvecs, tvecs; vector<float> reprojErrs; double totalAvgErr = 0; //  ,       //     , //   -   ,   //     ,  //   float aspectRatio = 1.0; int flags = CV_CALIB_FIX_ASPECT_RATIO; bool ok = runCalibration(samplesPoints, imageSize, boardSize, squareSize, aspectRatio, flags, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, totalAvgErr); //      stringstream sstr; sstr << "--- calib result: " << (ok ? "Calibration succeeded" : "Calibration failed") << ". avg reprojection error = " << totalAvgErr; DebugLog(sstr.str()); saveCameraParams(imageSize, boardSize, squareSize, aspectRatio, flags, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, samplesPoints, totalAvgErr); } /** @brief      */ void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners) { corners.resize(0); for( int i = 0; i < boardSize.height; ++i ) for( int j = 0; j < boardSize.width; ++j ) corners.push_back(Point3f(j*squareSize, i*squareSize, 0)); } /** @brief    */ bool runCalibration( vector<vector<Point2f> > imagePoints, Size imageSize, Size boardSize, float squareSize, float aspectRatio, int flags, Mat& cameraMatrix, Mat& distCoeffs, vector<Mat>& rvecs, vector<Mat>& tvecs, vector<float>& reprojErrs, double& totalAvgErr ) { //     cameraMatrix = Mat::eye(3, 3, CV_64F); if( flags & CALIB_FIX_ASPECT_RATIO ) cameraMatrix.at<double>(0,0) = aspectRatio; //   distCoeffs = Mat::zeros(8, 1, CV_64F); //      //    //    vector<vector<Point3f> > objectPoints(1); calcChessboardCorners(boardSize, squareSize, objectPoints[0]); objectPoints.resize(imagePoints.size(),objectPoints[0]); // OpenCV   //objectPoints -      //imagePoints -   //imageSize -    double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flags|CALIB_FIX_K4|CALIB_FIX_K5); ///*|CALIB_FIX_K3*/|CALIB_FIX_K4|CALIB_FIX_K5); //rms -   ,   , //     ,   1 printf("RMS error reported by calibrateCamera: %g\n", rms); bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs); totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs); return ok; } 

 /** @brief ,    */ void StereoCalibrator::makeCalibration ( const std::vector<std::vector<cv::Point2f>>& camera1SamplesPoints, const std::vector<std::vector<cv::Point2f>>& camera2SamplesPoints, cv::Mat& camera1Matrix, cv::Mat& camera1DistCoeffs, cv::Mat& camera2Matrix, cv::Mat& camera2DistCoeffs ) { //       std::vector<vector<Point3f>> objectPoints; for( int i = 0; i < camera1SamplesPoints.size(); i++ ) { objectPoints.push_back(chessboardCorners); } double rms = stereoCalibrate( objectPoints, camera1SamplesPoints, camera2SamplesPoints, //        camera1Matrix, camera1DistCoeffs, camera2Matrix, camera2DistCoeffs, //   imageSize, //         rotationMatrix, //        translationVector, //    essentialMatrix, //    fundamentalMatrix, //,        //       CV_CALIB_USE_INTRINSIC_GUESS, TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) ); //      - // , ,  ,      1 stringstream outs; outs << "--- stereo calib: done with RMS error=" << rms; DebugLog(outs.str()); } 

  //512, 70 deg Mat cam1 = (Mat_<double>(3, 3) << 355.3383989449604, 0, 258.0008490063121, 0, 354.5068750418187, 255.7252273330564, 0, 0, 1); Mat dist1 = (Mat_<double>(5, 1) << -0.02781875153957544, 0.05084431574408409, 0.0003262438299225566, 0.0005420218184546293, -0.06711413339515834); Mat cam2 = (Mat_<double>(3, 3) << 354.8366825622115, 0, 255.7668702403205, 0, 353.9950515096826, 254.3218524455621, 0, 0, 1); Mat dist2 = (Mat_<double>(12, 1) << -0.03429254591232522, 0.04304840389703278, -0.0005799461588668822, 0.0005396568753307817, -0.01867317550268149); Mat R = (Mat_<double>(3, 3) << 0.9999698145104303, 3.974878365893637e-06, 0.007769816740176146, -3.390471048492443e-05, 0.9999925806915616, 0.003851936175643478, -0.00776974378253147, -0.003852083336451321, 0.9999623955607145); Mat T = (Mat_<double>(3, 1) << 498.2890078004688, 0.3317087752736566, -6.137837861924672); 

 //     DisparityMapCalculator::DisparityMapCalculator () { bm = StereoBM::create(16,9); } /** @brief         */ void DisparityMapCalculator::set ( cv::Mat camera1Matrix, cv::Mat camera2Matrix, cv::Mat camera1distCoeff, cv::Mat camera2distCoeff, cv::Mat rotationMatrix, cv::Mat translationVector, cv::Size imageSize ) { this->camera1Matrix = camera1Matrix; this->camera2Matrix = camera2Matrix; this->camera1distCoeff = camera1distCoeff; this->camera2distCoeff = camera2distCoeff; this->rotationMatrix = rotationMatrix; this->translationVector = translationVector; //   ()  stereoRectify( camera1Matrix, camera1distCoeff, camera2Matrix, camera2distCoeff, imageSize, rotationMatrix, translationVector, R1, R2, P1, P2, Q, /*CALIB_ZERO_DISPARITY*/0, -1, imageSize, &roi1, &roi2 ); //    //     initUndistortRectifyMap(camera1Matrix, camera1distCoeff, R1, P1, imageSize, CV_16SC2, map11, map12); initUndistortRectifyMap(camera2Matrix, camera2distCoeff, R2, P2, imageSize, CV_16SC2, map21, map22); bm->setROI1(roi1); bm->setROI2(roi2); } /** @brief        */ void DisparityMapCalculator::setBMParameters ( int preFilterSize, int preFilterCap, int blockSize, int minDisparity, int numDisparities, int textureThreshold, int uniquenessRatio, int speckleWindowSize, int speckleRange, int disp12maxDiff ) { bm->setPreFilterSize(preFilterSize); bm->setPreFilterCap(preFilterCap); bm->setBlockSize(blockSize); bm->setMinDisparity(minDisparity); bm->setNumDisparities(numDisparities); bm->setTextureThreshold(textureThreshold); bm->setUniquenessRatio(uniquenessRatio); bm->setSpeckleWindowSize(speckleWindowSize); bm->setSpeckleRange(speckleRange); bm->setDisp12MaxDiff(disp12maxDiff); } /** @brief    */ void DisparityMapCalculator::compute ( const cv::Mat& image1, const cv::Mat& image2, cv::Mat& image1recified, cv::Mat& image2recified, cv::Mat& disparityMap ) { //   remap(image1, image1recified, map11, map12, INTER_LINEAR); remap(image2, image2recified, map21, map22, INTER_LINEAR); //          . //     OpenGL  ,   //   , //      OpenGL  OpenCV //      ,     flip(image1recified, L, 1); flip(image2recified, R, 1); // stereo bm -     sgbm, //      // StereoBM        cv::cvtColor(L, image1gray, CV_RGBA2GRAY, 1); cv::cvtColor(R, image2gray, CV_RGBA2GRAY, 1); int numberOfDisparities = bm->getNumDisparities(); //   bm->compute(image1gray, image2gray, disp); //    ,    disp.convertTo(disp8bit, CV_8U, 255/(numberOfDisparities*16.)); // ,       Unity flip (disp8bit, disp, 1); //     4   //      // 4  cv::cvtColor(disp, disparityMap, CV_GRAY2RGBA, 4); }