Библиотека cvQCodeBeacons
 |
Теория |
Теоретические выкладки по этой теме можно найти на странице Распознавание маяков типа "Q-Code" | |
 |
Внимание! |
При последнем использовании модуля выяснилось, что надо инвертировать res[][] при заполнении, если не будет получаться использовать программный код - попробуйте поправить этот момент. Об выявленной необходимости или отсутствии таковой инвертировать res[][] просьба сообщить в форуме в разделе Техническое зрение, если это будет массово - будем искать глюк в библиотеке. Благодарим за понимание. | |
Содержание
Назначение библиотеки
Поддержка визуальных маяков типа "QCode".
Функции (описание)
mycvGetQCodeBeacons
Определить на переданном изображении визуальные маяки типа "QCode" и их локальные координаты** относительно камеры;
Параметры функции:
- img - изображение на котором искать маяки;
- ThresholdSteps - сколько срезов изображения перебирать (чем больше тем лучше, но дольше, оптимально 4-7);
- k_front* - коэффициент размера маяка к расстоянию до него для рассчета координаты rx;
- k_side* - коэффициент размера маяка к расстоянию до него для рассчета координат ry,rz;
Примечание* Для камеры Genius Slim320 эти коэффициенты равны k_front=0.436, k_side=2.813
Примечание** Точность измерения расстояния до маяков и координат в ходе тестовых замеров лежала в пределах +/- 5% от корректного значения.
Файлы библиотеки
CvQCodeBeacons.h
<source lang="cpp">
- ifndef cvQCodeBeaconsH
- define cvQCodeBeaconsH
//---------------------------------------------------------------------------
- include "cv.h" // includes OpenCV definitions
- include "highgui.h" // includes highGUI definitions
struct CvQCodeBeacon {
//Номер маяка (0..2^15-1) long int id;
// Поля для заполнения внешней программой (все величины в метрах): long double gx,gy,gz; //Глобальные координаты маяка на карте
//Поля заполняемые функцией mycvGetQCodeBeacons (все величины в пикселях) long double bx,by; //Один из углов маяка в кадре long double x1,y1,x2,y2; //Опорные вектора параллелепипеда маяка на изображении long double x,y; //Координаты центра маяка в кадре
//Поля заполняемые функцией mycvGetQCodeBeacons (все величины в метрах) long double rx,ry,rz; //Локальные координаты маяка относительно робота long double d; //Расстояние от робота до маяка
//Ссылка на блок информации о следующем маяке CvQCodeBeacon* next; //NULL, если это последний маяк };
CvQCodeBeacon* mycvGetQCodeBeacons(IplImage *img, int ThresholdSteps, double k_front, double k_side);
- endif
</source>
CvQCodeBeacons.cpp
<source lang="cpp">
- pragma hdrstop
- include "CvQCodeBeacons.h"
//---------------------------------------------------------------------------
- pragma package(smart_init)
double angleF(uchar* datas, long int steps, CvSize szSrc, int bx,int by,int dx1,int dy1,int dx2,int dy2,int l, int b){
double inner=0, outer=0;
double d1=sqrt(dx1*dx1+dy1*dy1);
double d2=sqrt(dx2*dx2+dy2*dy2);
double dx1n=dx1/d1, dy1n=dy1/d1;
double dx2n=dx2/d2, dy2n=dy2/d2;
for(int j=1; j<=b; j++){
for(int i=1; i<=l; i++){
long int adr,d_x,d_y;
d_x=bx+j*(dx1n+dx2n)+i*dx1n;
d_y=by+j*(dy1n+dy2n)+i*dy1n;
if( d_x<0 || d_x>szSrc.width || d_y<0 || d_y>szSrc.height ){
inner+=0;
}else{
adr=d_x*3+(szSrc.height-d_y)*steps+2;
inner+=datas[adr];
};
d_x=bx+j*(dx1n+dx2n)+i*dx2n;
d_y=by+j*(dy1n+dy2n)+i*dy2n;
if( d_x<0 || d_x>szSrc.width || d_y<0 || d_y>szSrc.height ){
inner+=0;
}else{
adr=d_x*3+(szSrc.height-d_y)*steps+2;
inner+=datas[adr];
};
d_x=bx-j*(dx1n+dx2n)+i*dx1n;
d_y=by-j*(dy1n+dy2n)+i*dy1n;
if( d_x<0 || d_x>szSrc.width || d_y<0 || d_y>szSrc.height ){
outer+=0;
}else{
adr=d_x*3+(szSrc.height-d_y)*steps+2;
outer+=datas[adr];
};
d_x=bx-j*(dx1n+dx2n)+i*dx2n;
d_y=by-j*(dy1n+dy2n)+i*dy2n;
if( d_x<0 || d_x>szSrc.width || d_y<0 || d_y>szSrc.height ){
outer+=0;
}else{
adr=d_x*3+(szSrc.height-d_y)*steps+2;
outer+=datas[adr];
};
};};
return outer-inner;
};
double sqr(double a){
return a*a;
};
CvQCodeBeacon* mycvGetQCodeBeacons(IplImage *img, int ThresholdSteps, double k_front, double k_side){
CvQCodeBeacon* fbeacon = NULL; CvSize szSrc; szSrc.width=img->width; szSrc.height=img->height; CvSize szDiv2; szDiv2.width=szSrc.width/2; szDiv2.height=szSrc.height/2; IplImage* pyr = cvCreateImage(szDiv2, IPL_DEPTH_8U, 3); IplImage* tmp = cvCreateImage(szSrc, IPL_DEPTH_8U, 3); cvPyrDown( img, pyr, 7 ); cvPyrUp( pyr, tmp, 7 ); IplImage* gray = cvCreateImage( szSrc, 8, 1 ); IplImage* tgray = cvCreateImage( szSrc, 8, 1 ); cvSetImageCOI( tmp, 2 ); cvCopy( tmp, tgray, 0 );
CvMemStorage* storage=cvCreateMemStorage(0);
CvSeq* contours, result;
for(int l=0; l<ThresholdSteps; l++){
//Отфильтруем изображение
if(l==0){
cvCanny( tgray, gray, 0, 100, 5 );
cvDilate( gray, gray, 0, 1 );
}else{
cvThreshold( tgray, gray, (l+1)*255/ThresholdSteps, 255, CV_THRESH_BINARY );
};
//Найдем все контуры на изображении
cvFindContours( gray, storage, &contours, sizeof(CvContour),
CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
//Для каждого из найденных контуров
while( contours )
{
//Приблизим его многоугольником
CvSeq* result = cvApproxPoly( contours, sizeof(CvContour), storage,
CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 );
//Извлечем количество вершин многоугольника
int count = result->total;
if(
count == 4 //Это должен быть четырехугольник
&& fabs(cvContourArea(result,CV_WHOLE_SEQ)) > 100 //Достаточной площади
&& cvCheckContourConvexity(result) //Выпуклый
){
//Флаг, который будем сбрасывать, если что-то не так при проверке
int isPara=1;
//Выпишем координаты его вершин
double x[5],y[5];
for(int i=0; i < count; i++){
CvPoint *pt=(CvPoint*)cvGetSeqElem( result, i );
x[i]=pt->x;
y[i]=szSrc.height-pt->y;
}; x[count]=x[0]; y[count]=y[0];
//Посчитаем длины сторон полученного четырехугольника
double l[5];
for(int i=1; i<=4; i++){
l[i]=sqrt(sqr(x[i-1]-x[i])+sqr(y[i-1]-y[i]));
};
//Проверим все стороны, если они сильно отличаются по длине, то отбой
if( fabs(l[1]/(l[1]+l[3])-0.5)>0.1 ){ isPara=0; };
if( fabs(l[2]/(l[2]+l[4])-0.5)>0.1 ){ isPara=0; };
if( fabs(l[2]/(l[2]+l[3])-0.5)>0.2 ){ isPara=0; };
if( l[1]+l[2]>800 ){ isPara=0; };
//Проверим один из углов - если он сильно отличается от прямого, то отбой
double s_mul=(x[1]-x[0])*(x[2]-x[1])+(y[1]-y[0])*(y[2]-y[1]);
double cos_a=s_mul/(l[1]*l[2]);
if( cos_a > 0.3 ){ isPara=0; };
//Проверим, что в этих координатах еще не находили какой-либо маяк
if( isPara ){
double cx=(x[0]+x[1]+x[2]+x[3])/4;
double cy=(y[0]+y[1]+y[2]+y[3])/4;
CvQCodeBeacon *bc=fbeacon;
while( bc ){
double dist=sqrt(sqr(cx-bc->x)+sqr(cy-bc->y));
if( dist<5 ){ isPara=0; }; //Если что-то очень рядом было, то отбой
bc=bc->next;
};
};
//Если прошли все первичные проверки
if( isPara ){
double idx1,idy1,idx2,idy2,idx3,idy3;
uchar* datas=tmp->imageDataOrigin;
long int steps=tmp->widthStep;
double v1x=x[1]-x[2], v1y=y[1]-y[2];
double v2x=x[3]-x[2], v2y=y[3]-y[2];
int area=5;
int L=8;
int B=3;
double wasmax=0;
//Найдем правильную точку x[1],y[1] границы маяка перебором
//рядом с ориентировочными координатами
for(long int dx1=-area; dx1<=area; dx1++){
for(long int dy1=-area; dy1<=area; dy1++){
double a=angleF(datas,steps,szSrc,x[1]+dx1,y[1]+dy1,-v1x,-v1y,v2x,v2y,L,B);
if(a>wasmax){
idx1=dx1; idy1=dy1;
wasmax=a;
};
};};
wasmax=0;
//Найдем правильную точку x[2],y[2] границы маяка перебором
//рядом с ориентировочными координатами
for(long int dx2=-area; dx2<=area; dx2++){
for(long int dy2=-area; dy2<=area; dy2++){
double a=angleF(datas,steps,szSrc,x[2]+dx2,y[2]+dy2,v1x,v1y,v2x,v2y,L,B);
if(a>wasmax){
idx2=dx2; idy2=dy2;
wasmax=a;
};
};};
wasmax=0;
//Найдем правильную точку x[3],y[3] границы маяка перебором
//рядом с ориентировочными координатами
for(long int dx3=-area; dx3<=area; dx3++){
for(long int dy3=-area; dy3<=area; dy3++){
double a=angleF(datas,steps,szSrc,x[3]+dx3,y[3]+dy3,v1x,v1y,-v2x,-v2y,L,B);
if(a>wasmax){
idx3=dx3; idy3=dy3;
wasmax=a;
};
};};
double res[7][7];
int K=7;
double base_x=x[2]+idx2, base_y=y[2]+idy2;
v1x=x[1]-x[2]+idx1-idx2, v1y=y[1]-y[2]+idy1-idy2;
v2x=x[3]-x[2]+idx3-idx2, v2y=y[3]-y[2]+idy3-idy2;
double vmax=0,vmin=255;
//Заполним матрицу клеток маяка
for(long int d1=0; d1<K; d1++){
for(long int d2=0; d2<K; d2++){
long int d_x=base_x+(0.5+d1)*v1x/K+(0.5+d2)*v2x/K;
long int d_y=base_y+(0.5+d1)*v1y/K+(0.5+d2)*v2y/K;
if( d_x<0 || d_x>szSrc.width || d_y<0 || d_y>szSrc.height ){
res[d1][d2]=1000;
}else{
long int adr=d_x*3+(szSrc.height-d_y)*steps+2;
res[d1][d2]=datas[adr];
};
if( res[d1][d2]>vmax ){ vmax=res[d1][d2]; };
if( res[d1][d2]<vmin ){ vmin=res[d1][d2]; };
};};
for(int d1=0; d1<K; d1++){
for(int d2=0; d2<K; d2++){
res[d1][d2]= res[d1][d2]<(vmax+vmin)/2 ? 1 : 0;
};
};
int isGood=1;
//Проверим на то, что по периметру центрального квадрата маяка размером 7х7 идут черные клетки, иначе ошибка
for(int i=0; i<K; i++){
if( res[0][i]==0 ){ isGood=0; i=K; };
if( res[K-1][i]==0 ){ isGood=0; i=K; };
if( res[i][0]==0 ){ isGood=0; i=K; };
if( res[i][K-1]==0 ){ isGood=0; i=K; };
};
//Проверим, что во внутреннем квадрате 5х5 маяка в углах всего 1 клетка закрашена (маркер поворота)
if( res[1][1]+res[5][1]+res[5][5]+res[1][5] != 1 ){ isGood=0; };
//Если последние две проверки прошли, то движемся дальше.
if( isGood==1 ){
//Развернем нужным нам образом матрицу маяка
double tmp[7][7];
for(int d1=0; d1<K; d1++){
for(int d2=0; d2<K; d2++){
int dd1=res[1][1]+res[1][5]==1 ? d1 : K-1-d1;
int dd2=res[1][1]+res[5][1]==1 ? d2 : K-1-d2;
tmp[d1][d2]=res[dd1][dd2];
};};
for(int d1=0; d1<K; d1++){
for(int d2=0; d2<K; d2++){
int dd1=tmp[1][2]==1 ? d1 : d2;
int dd2=tmp[1][2]==1 ? d2 : d1;
res[d1][d2]=tmp[dd1][dd2];
};};
int code[20]; for(int i=0; i<20; i++){ code[i]=0; };
long int beacon=0;
//Рассчитаем номер маяка и проверим его контрольную сумму
if( isGood==1 ){
int checkbits=0;
for(int i=2; i<=4; i++){ code[i-1]=res[5][i]; };
for(int i=1; i<=5; i++){ code[i+3]=res[4][i]; code[i+8]=res[3][i]; };
for(int i=2; i<=5; i++){ code[i+12]=res[2][i]; };
for(int i=3; i<=4; i++){ code[i+15]=res[1][i]; };
for(int j=0; j<5; j++){
int bits=0;
for(int i=0; i<4; i++){ bits = (bits << 1) + code[j*4+i]; };
checkbits = checkbits ^ bits;
if( j<4 ){ beacon = (beacon << 4) + bits; };
};
if( checkbits != 13 ){ isGood=0; };
};
//Если контрольная сумма прошла, тогда зарегистрируем новый обнаруженный маяк
if( isGood==1 ){
CvQCodeBeacon* cur=fbeacon;
while( cur && cur->next && cur->id != beacon ){ cur=cur->next; };
if( !cur ){
cur=new CvQCodeBeacon();
fbeacon=cur;
cur->next=NULL;
}else if( cur->id != beacon ){
cur->next=new CvQCodeBeacon();
cur=cur->next;
cur->next=NULL;
};
cur->id=beacon;
cur->x=base_x+(v1x+v2x)/2;
cur->y=base_y+(v1y+v2y)/2;
cur->bx=base_x;
cur->by=base_y;
cur->x1=v1x;
cur->y1=v1y;
cur->x2=v2x;
cur->y2=v2y;
long double size=sqrt(sqr(v1x+v2x)+sqr(v1y+v2y));
//Рассчитаем коордитаны маяка относительно камеры
cur->rx=k_front*szDiv2.width/size;
cur->ry=((cur->x) - szDiv2.width)*(cur->rx)/(szDiv2.width*k_side);
cur->rz=((cur->y) - szDiv2.height)*(cur->rx)/(szDiv2.width*k_side);
cur->d=sqrt(sqr(cur->rx)+sqr(cur->ry)+sqr(cur->rz));
cur->d=floor(cur->d*100)/100;
};
};
};
};
contours = contours->h_next;
};
};
cvReleaseMemStorage( &storage );
cvReleaseImage(&pyr); cvReleaseImage(&tmp); cvReleaseImage(&gray); cvReleaseImage(&tgray);
return fbeacon; };
</source>
Примеры использования библиотеки
Под Linux - Realsystem
