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

Всем привет!

Сегодня я хотел бы продолжить серию о том, как подружить Unity, C++ и OpenCV. А также, как получить виртуальную среду для тестирования алгоритмов компьютерного зрения и навигации дронов на основе Unity. В предыдущих статьях я рассказывал о том, как сделать виртуальный квадрокоптер в Unity и как подключить C++ плагин, передать туда изображение с виртуальной камеры и обработать его посредством OpenCV. В этой статье я расскажу как сделать из двух виртуальных камер на квадрокоптере стереопару и как получить карту смещений (disparity map), которую можно использовать для оценки глубины пикселей изображения.

Идея

О том, как сделать 3д реконструкцию написано не мало. Например есть замечательная статья на хабре. Очень советую ее прочитать, если вы совсем не в теме. Более математически строго можно почитать тут. Здесь же я ооочень упрощенно изложу основную идею. Техника получения карты смещений, которая будет использоваться, называется плотная 3д реконструкция (dense 3d reconstruction) по двум изображениям. Используется то, что две расположенные рядом и имеющие одну ориентацию камеры видят одну сцену с немного отличающихся точек зрения. Называется это стереопара. Мы будем использовать обычную горизонтальную стереопару, то есть камеры, смещенные перпендикулярно направлению «зрения» камеры. Если найти на первом и втором изображении одну и ту же точку сцены, то есть найти две проекции точки сцены, то можно заметить, что, в общем случае, координаты этих двух проекций не совпадают. То есть проекции смещены друг относительно друга в случае наложения снимков. Это и дает возможность вычислить глубину точки сцены по величине смещения (упрощенно: точки смещенные больше находятся ближе точек смещенных меньше).

изображение из http://ift.tt/1R0M8BI

Чтобы это сделать необходимо откалибровать камеры. Далее необходимо откалибровать стереопару, убрать дисторсию и выпрямить изображения, так чтобы те самые проекции точки лежали на одной горизонтальной прямой. Это требование алгоритма плотной 3д реконструкции из OpenCV, ускоряющее поиск соответствующих точек. Мы будем использовать самый простой и самый быстрый алгоритм из OpenCV — StereoBM. Описание АПИ находится здесь. Приступим.

Калибровка камер

Как добавить еще одну камеру на квадрокоптер и получить с нее изображение описывается в предидущей статье, поэтому сразу начнем с калибровки камер. Я использую две камеры с углом обзора (field of view) 70 градусов, текстуры изображений с камер разрешением 512 x 512 пикселей. Откалибровать камеру — это значит получить матрицу 3х3 внутренних параметров и вектор параметров ее искажений. Калибровка происходит путем получения набора калибровочных сэмплов. Один калибровочный сэмпл — это координаты точек калибровочного паттерна с заранее известной геометрией в двухмерной системе отсчета снимка камеры. От качества калибровки очень сильно зависит работа всех алгоритмов, которые ее используют, поэтому очень важно хорошо откалибровать камеры. Я калибрую по 40-50 калибровочным сэмплам, причем, важна большая вариабельность калибровочных сэмплов, то есть по возможности необходимо получить как можно больший разброс по ориентациям и позициям калибровочного паттерна относительно камеры. Предъявление почти одинаковых 50 калибровочных сэмплов даст невысокое качество калибровки камеры. Я использую этот калибровочный петтарн. Его просто можно драг-эн-дропнуть в Unity и задать его текстурой для 2д спрайта. Все размеры в программе я задаю в пикселях, размер стороны квадрата этого паттерна 167 пикселей. Для него уже есть функции поиска его на изображении в OpenCV. Для вдохновения можно использовать пример из opencv-source/samples/cpp/calibration.cpp. Собственно, я так и сделал.

Код основных функций калибровки

/** @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;
}

Стереопара

Теперь нам необходимо сделать из наших двух камер стереопару. Для этого используется функция stereoCalibrate. Для нее нам понадобятся сэмплы обеих камер. Мы воспользуемся сэмплами взятыми с шага калибровки камер. Поэтому важно сохранять только те сэмплы, где калибровочный паттерн бал найден на двух изображениях сразу. Матрицы камер и их параметры дисторсии нам потребуются как хорошие начальные приближения.

Код калибровки стереопары

/** @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);

Карта смещений

Теперь настала очередь рассчитать карту смещений. Даже если все предидущие шаги вы сделали правильно не факт, что у вас сразу получиться ее рассчитать. Дело в том, что у алгоритма расчета карты смещении порядка 10 параметров, которые нужно правильно подобрать чтобы получить хорошую картинку. Очень помогает понять в чем тут дело вот это видео

из этой статьи. По поводу настройки парамертров также не советую задирать параметр TextureThreshold больше 50, так как известно, что при этом алгоритм StereoBM начинает спонтанно падать.

Код получения карты смещений

//Создание алгоритмов получения карт смещений
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);
}

Также можно посмотреть на видео как это все работает у меня.

Алгоритм, использованный для расчета карты смещений довольно старый, поэтому результат не очень красивый получился. Я думаю, что его можно улучшить использовав какой-нибудь более современный алгоритм, но для этого одного OpenCV не достаточно.

За сцену нужно сказать спасибо ThomasKole
Код можно взять из гитхаба, ветка habr_part3_disparity_map_opencv_stereobm

Спасибо за внимание

This entry passed through the Full-Text RSS service - if this is your content and you're reading it on someone else's site, please read the FAQ at http://ift.tt/jcXqJW.