Виртуальный квадрокоптер на Unity + OpenCV (Часть 3)05.12.2015 13:18

/** @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 rvecs, tvecs;
        vector 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& 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 > imagePoints,
        Size imageSize, Size boardSize,
        float squareSize, float aspectRatio,
        int flags, Mat& cameraMatrix, Mat& distCoeffs,
        vector& rvecs, vector& tvecs,
        vector& reprojErrs,
        double& totalAvgErr
) {
        //инициализация матрицы внутренних параметров камеры
        cameraMatrix = Mat::eye(3, 3, CV_64F);
        if( flags & CALIB_FIX_ASPECT_RATIO )
        cameraMatrix.at(0,0) = aspectRatio;

        //инициализация коэффициентов дисторсии
        distCoeffs = Mat::zeros(8, 1, CV_64F);

        //рассчитываем координаты точек на калибровочном паттерне
        //в системе калибровочного паттерна
        //это необходимо для калибровки
        vector > 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>& camera1SamplesPoints,
        const std::vector>& camera2SamplesPoints,
        cv::Mat& camera1Matrix,
        cv::Mat& camera1DistCoeffs,
        cv::Mat& camera2Matrix,
        cv::Mat& camera2DistCoeffs
) {

        //снова нам нужно передать геометрию калибровочного паттерна
        std::vector> 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_(3, 3) <<
        355.3383989449604, 0, 258.0008490063121,
         0, 354.5068750418187, 255.7252273330564,
         0, 0, 1);

        Mat dist1 = (Mat_(5, 1) <<
        -0.02781875153957544,
         0.05084431574408409,
         0.0003262438299225566,
         0.0005420218184546293,
         -0.06711413339515834);

        Mat cam2 = (Mat_(3, 3) <<
        354.8366825622115, 0, 255.7668702403205,
         0, 353.9950515096826, 254.3218524455621,
         0, 0, 1);

        Mat dist2 = (Mat_(12, 1) <<
        -0.03429254591232522,
         0.04304840389703278,
         -0.0005799461588668822,
         0.0005396568753307817,
         -0.01867317550268149);

        Mat R = (Mat_(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_(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);
}