Jump to content
Compvision.ru
Sign in to follow this  
military_man

Работа с SolvePnP. Краткий пример.

Recommended Posts

Здравствуйте.

Начав изучать OpenCV и заодно программирование столкнулся с рядом банальных (для программеров) проблем при решении задачи определения ориентации объекта в пространстве по его фотографии. Одной из основных было отсутствие материалов на русском (или неумение найти). Здесь приведено описание решения этой задачи. Просьба сильно не пинать т.к. прогер из меня аховый.

Предполагается что читатель предварительно ознакомился с используемой математикой (например: Артем Конушин Геометрические свойства нескольких изображений), имеет установленную студию (у меня VS 2010) и OpenCV 2.2.

Итак, краткое содержание предыдущей серии.

Имеется мировая система координат (СК), имеется СК камеры. Переход между ними определяется матрицей внешнего ориентирования (иногда называют матрицей внешней калибровки) Е(4х4). Эти системы метрические, то есть координаты задаются в метрах, миллиметрах и т.д.

Имеется также система координат кадра, координаты в которой задаются в пикселях. Переход из нее в СК камеры задается матрицей калибровки К(3х3).

Постановка задачи: Дан объект. Известны координаты его некоторых точек (здесь и далее «особые точки») в мировой системе координат. Известны координаты изображения этих точек на снимке. Известна матрица калибровки камеры К.

Надо найти. Матрицу ориентирования камеры Е в мировой СК. Она может быть представлена в виде матрицы поворота R и вектора переноса T камеры.

Для начала представим данные в формате OpenCV.

Подход следующий: создаем массив, и привязываем к нему создаваемую матрицу. Тогда любые изменения можно будет отслеживать в массиве или изменяя массив изменять саму матрицу (делаю так по причине незнания работы с матрицами).

double Ktmp[9]={1, 0, 0,   0, 1,0,  0,0,1};

Mat K(3, 3, DataType<double>::type, Ktmp); // объявили матрицу и привязали к массиву
В качестве К задана единичная диагональная матрица для упрощения проверки результатов. При такой калибровке 1 пиксель соответствует 1 единице длины (пусть будет миллиметр). Теперь зададим координаты точек объекта в мировой СК. Пусть это будет фрагмент шахматной доски, то есть координаты точек соединения квадратов. Например, 5 точек
float modX[5]={0, 2, 4, 0, 2};

float modY[5]={0, 0, 0, 2, 2}; // массивы с координатами по осям

float modZ[5]={0, 0, 0, 0, 0}; 

vector<Point3f> model_points; //  массив «вектор» трехмерных точек. Сюда перепишем массивы.


for (int i=0;i<5;i++)

	{

	    model_points.push_back(Point3f(modX[i],modY[i],modZ[i]));// переписываем

	}

Аналогично зададим координаты точек на изображении. Предположим наша камера не повернута относительно мировой СК, а только сдвинута вниз на 10 мм. Тогда координаты точек на картинке будут в 10 раз ближе к оптической оси камеры (которая в соответствии с заданной К находится в точке (0,0) кадра).
float imX[5]={ 0, 0.2, 0.4, 0,   0.2};

float imY[5]={0,   0,    0,   0.2, 0,2};

vector<Point2f> image_points;


for (int i=0;i<5;i++)

	{

	    model_points.push_back(Point2f(imX[i],imY[i]));// переписываем

	}
Теперь для функции надо указать еще и дисторсию, Но ее пока не учитываем

Mat_<double> distCoeffs(1, 5);

distCoeffs << 0.0, 0.0, 0., 0., 0.; // дисторсия по нулям

Готовим переменные для выходнызх данных

double rvectmp[3]={0.0};// сами переменные

double tvectmp[3]={0.0};

Mat rvec(3, 1, DataType<double>::type, rvectmp); // привязываем матрицы (точнее вектор-столбцы)

Mat tvec(3, 1, DataType<double>::type, tvectmp);
Творим волшебство (Mat(image_points) видимо означает преобразование типа «вектор» к типу «Матрица» иначе компилятор ругается).
solvePnP(Mat(model_points), Mat(image_points), K, distCoeffs, rvec, tvec, false);
false – означает что расчеты ведутся с нуля, без начального приближения (см документацию) теперь что же делать с результатом? tvec, а вместе с ним и tvectmp теперь содержит координаты точки фокуса камеры в мировой СК. rvectmp содержит углы поворота СК камеры относительно мировой СК. Надо преобразовать в матрицу поворота R
double Rtmp[9]={0.0};

Mat R(3, 3, DataType<double>::type, Rtmp);

Rodrigues(rvec,R); //пересчет углов поворота в матрицу поворота

Теперь Rtmp содержит коэффициенты матрицы поворота. Задача решена.

Для текущих данных должно получаться: повороты по нулям, матрица поворота единичная диагональная, вектор переноса T=(0,0,-10)

Текущий код не компилил, писал по памяти и по старому варианту, так что могут быть синтаксические ошибки. Но подход работал точно. Как сохранить решение и прочитать входные массивы (например из файла) уже даже чайник вроде меня должен сообразить. Спасибо за внимание.

Ваши комменты.

  • Like 2

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this  

  • Recently Browsing   0 members

    No registered users viewing this page.

×