RS003: show_map.c Source File

Go to the documentation of this file.
00001 #include <stdio.h>
00002 #ifndef WIN32
00003 #include <unistd.h>
00004 #endif
00005 
00006 #ifdef WIN32
00007 #define _USE_MATH_DEFINES
00008 #endif
00009 #include <math.h>
00010 
00011 #include <cv.h>
00012 #include <highgui.h>
00013 
00014 #include "../Common/sharedmem.h"
00015 
00016 #ifdef WIN32
00017 #include "../Common/Linux4Win.h"
00018 #endif
00019 
00020 #include "define.h"
00021 
00022 
00023 #define MAP_WINDOW "ceil map"
00024 //#define BIG_WINDOW "Localization" //これを定義するとceil mapと同じものがscale倍されて表示される
00025 
00026 
00027 static void on_mouse(int event, int x, int y, int flags, void* param);
00028 
00029 //共有メモリから読み取るロボットの状態 ../Common/sharedmem.hで定義
00030 static robot_status *rstat;
00031 
00032 int main (int argc, char **argv )
00033 {
00034 #ifdef BIG_WINDOW
00035   double scale = 3.0;
00036 #endif
00037   int key = 0;
00038   IplImage *map_img;//天井地図画像
00039   IplImage *output_img;//出力画像（天井地図画像と同サイズ）
00040   IplImage *capture_img;//共有メモリから読んだカメラ画像
00041   IplImage *capture_tmp;//共有メモリから読んだカメラ画像
00042 #ifdef SAVE_TRACK
00043   char fname[256];
00044   unsigned int count = 0;
00045 #endif
00046   CvRect roi;
00047 
00048   //共有メモリ初期化  書き込み側のRTC(CeilingNavigationLinux)とSHM_KEYを合わせておくこと．
00049   rstat = (robot_status *)InitSharedMem(SHM_KEY, sizeof(robot_status));
00050 
00051   //地図となる天井画像の読み込み
00052   map_img = cvLoadImage (MAP_SHOW, CV_LOAD_IMAGE_COLOR /* CV_LOAD_IMAGE_GRAYSCALE */);//define.hで地図画像を定義
00053   if (!map_img)
00054     {//読み込みエラー処理
00055       fprintf(stderr, "%s: LOAD ERROR\n", MAP_SHOW);
00056       return 1;
00057     }
00058   
00059   //地図画像のサイズ確認表示
00060   printf("mapsize: x=%3d y=%3d\n",map_img->width,map_img->height);
00061 
00062   //各種イメージデータのクリエイト
00063   output_img = cvCreateImage( cvSize(map_img->width, map_img->height), IPL_DEPTH_8U, 3);
00064   capture_img = cvCreateImage( cvSize(TEMPLATE_SIZE, TEMPLATE_SIZE), IPL_DEPTH_8U, 1);//こいつだけ白黒画像であることに注意
00065   capture_tmp = cvCreateImage( cvSize(TEMPLATE_SIZE, TEMPLATE_SIZE), IPL_DEPTH_8U, 3);//TEMPLATE_SIZEはdefine.hで定義
00066 
00067 #ifdef BIG_WINDOW
00068   IplImage *big_img = cvCreateImage (cvSize ((int)(output_img->width*scale),(int)(output_img->height*scale)), output_img->depth, output_img->nChannels);
00069   cvNamedWindow (BIG_WINDOW, CV_WINDOW_AUTOSIZE);
00070 #endif
00071 
00072   cvNamedWindow (MAP_WINDOW, CV_WINDOW_AUTOSIZE);
00073   cvNamedWindow("CameraWindow", 0);
00074   cvSetMouseCallback(MAP_WINDOW, on_mouse, 0); //MAP_WINDOWはマウスクリックに対応
00075 
00076   while(1)
00077     {
00078       cvCopy (map_img, output_img,NULL);//地図画像を出力画像にコピー
00079       //共有メモリのカメラ画像をcapture_imgにコピー
00080       memcpy (capture_img->imageData, rstat->buf, sizeof(unsigned char)*TEMPLATE_SIZE*TEMPLATE_SIZE);
00081       //      cvConvertImage( capture_img, output_img, 0);
00082       //白黒画像をカラー画像に変換
00083       cvConvertImage( capture_img, capture_tmp, 0);
00084       
00085       //出力画像の左上にcaputure_img(capture_tmp)を表示させる処理
00086       roi = cvRect (0, 0, capture_img->width, capture_img->height);
00087       cvSetImageROI (output_img, roi);
00088       cvCopy (capture_tmp, output_img, 0);
00089       cvResetImageROI(output_img);
00090       //左上の表示領域に枠をつける
00091       cvRectangle ( output_img, cvPoint(roi.x,roi.y), cvPoint(roi.x+roi.width,roi.y+roi.height), CV_RGB(255,0,0), 1, 4, 0 );
00092 
00093 #ifdef _DEBUG
00094       printf("%d, %d, %f ( %d )\n", rstat->x, rstat->y, rstat->r, rstat->s);
00095 #endif
00096 
00097       //以下，ロボットの位置・方位角・マッチした画像枠の描画
00098       // robot point
00099       cvCircle ( map_img, 
00100                  cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2),
00101                           (int)(rstat->y+TEMPLATE_SIZE/2) ),
00102                  1, CV_RGB(255,0,0), -1, 4, 0 );
00103 
00104       // robot point
00105 /*      cvCircle ( map_img, 
00106                  cvPoint( (int)(rstat->x+TEMPLATE_SIZE),
00107                           (int)(rstat->y+TEMPLATE_SIZE) ),
00108                  1, CV_RGB(255,0,0), -1, 4, 0 );*/
00109 
00110       // robot point
00111 /*      cvCircle ( map_img, 
00112                  cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2),
00113                           (int)(rstat->y) ),
00114                  1, CV_RGB(255,0,0), -1, 4, 0 );*/
00115 
00116       // deg
00117       cvLine ( output_img,
00118                cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2),
00119                         (int)(rstat->y+TEMPLATE_SIZE/2) ),
00120                cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2+sin(rstat->r)*10),
00121                         (int)(rstat->y+TEMPLATE_SIZE/2+cos(rstat->r)*10) ),
00122                CV_RGB(255,140,0), 1, CV_AA, 0 );
00123 
00124 /*
00125       // deg
00126       cvLine ( output_img,
00127                cvPoint( (int)(rstat->x+TEMPLATE_SIZE),
00128                         (int)(rstat->y+TEMPLATE_SIZE) ),
00129                cvPoint( (int)(rstat->x+TEMPLATE_SIZE+sin(rstat->r)*10),
00130                         (int)(rstat->y+TEMPLATE_SIZE+cos(rstat->r)*10) ),
00131                CV_RGB(255,140,0), 1, CV_AA, 0 );*/
00132 
00133       // deg
00134 /*      cvLine ( output_img,
00135                cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2),
00136                         (int)(rstat->y) ),
00137                cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2+sin(rstat->r)*10),
00138                         (int)(rstat->y+cos(rstat->r)*10) ),
00139                CV_RGB(255,140,0), 1, CV_AA, 0 );*/
00140 
00141       // detect rect
00142       cvLine ( output_img,
00143                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
00144                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
00145                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
00146                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
00147                CV_RGB(255,255,0), 1, CV_AA, 0 );
00148       cvLine ( output_img,
00149                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
00150                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
00151                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
00152                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
00153                CV_RGB(255,255,0), 1, CV_AA, 0 );
00154       cvLine ( output_img,
00155                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
00156                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
00157                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
00158                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
00159                CV_RGB(255,255,0), 1, CV_AA, 0 );
00160       cvLine ( output_img,
00161                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
00162                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
00163                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
00164                         (int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
00165                CV_RGB(255,255,0), 1, CV_AA, 0 );
00166       
00167       // detect rect
00168 /*      cvLine ( output_img,
00169                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
00170                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
00171                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
00172                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
00173                CV_RGB(255,255,0), 1, CV_AA, 0 );
00174       cvLine ( output_img,
00175                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
00176                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
00177                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
00178                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
00179                CV_RGB(255,255,0), 1, CV_AA, 0 );
00180       cvLine ( output_img,
00181                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
00182                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
00183                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
00184                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
00185                CV_RGB(255,255,0), 1, CV_AA, 0 );
00186       cvLine ( output_img,
00187                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
00188                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
00189                cvPoint( (int)(rstat->x + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
00190                         (int)(rstat->y + TEMPLATE_SIZE + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
00191                CV_RGB(255,255,0), 1, CV_AA, 0 );*/
00192       
00193       // detect rect
00194 /*      cvLine ( output_img,
00195                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
00196                         (int)(rstat->y  + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
00197                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
00198                         (int)(rstat->y  + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
00199                CV_RGB(255,255,0), 1, CV_AA, 0 );
00200       cvLine ( output_img,
00201                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
00202                         (int)(rstat->y  + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
00203                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
00204                         (int)(rstat->y  + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
00205                CV_RGB(255,255,0), 1, CV_AA, 0 );
00206       cvLine ( output_img,
00207                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
00208                         (int)(rstat->y  + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
00209                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
00210                         (int)(rstat->y  + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
00211                CV_RGB(255,255,0), 1, CV_AA, 0 );
00212       cvLine ( output_img,
00213                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
00214                         (int)(rstat->y  + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
00215                cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
00216                         (int)(rstat->y  + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
00217                CV_RGB(255,255,0), 1, CV_AA, 0 );*/
00218       
00219 
00220 
00221       //出力画像表示
00222       cvShowImage(MAP_WINDOW, output_img);
00223       //カメラのキャプチャ画像（共有メモリから取得）を表示
00224       cvShowImage("CameraWindow",capture_img);
00225 
00226 #ifdef BIG_WINDOW
00227       //出力画像を引き伸ばして表示
00228       cvResize (output_img, big_img, CV_INTER_NN);
00229       cvShowImage(BIG_WINDOW, big_img);
00230 #endif
00231 
00232 //以下，画像をファイルに書き出し
00233 #ifdef SAVE_TRACK
00234       sprintf( fname, SAVE_TRACK"map_%05d.png", count++ );
00235 //      cvSaveImage( fname, output_img );
00236 #endif
00237       //cvSaveImage( "test.png", output_img );
00238 
00239       //usleep(2000);
00240 
00241       key = cvWaitKey(50) & 0xff; //なぜか& 0xffがないとキー入力が効かない
00242       switch (key)
00243         {
00244         case 0x1b: // exit
00245           goto exit;
00246         case 'q':  // exit
00247           goto exit;
00248         case 'r':  // reset
00249           map_img = cvLoadImage (MAP_SHOW, CV_LOAD_IMAGE_COLOR /* CV_LOAD_IMAGE_GRAYSCALE */);
00250           printf("Map reset!");
00251           break;
00252         }
00253     }
00254 
00255  exit:;
00256 
00257 
00258 #ifdef BIG_WINDOW
00259   cvDestroyWindow (BIG_WINDOW);
00260 #endif
00261   cvDestroyWindow (MAP_WINDOW);
00262 
00263   return 0;
00264 }
00265 
00266 //出力画像上でのマウスクリックに対する動作の定義
00267 static CvPoint lbuttondown_pt;
00268 
00269 void on_mouse(int event, int x, int y, int flags, void* param)
00270 {
00271   //左ボタンだけを使用
00272   //押したところが位置の中心
00273   //そこからドラッグしてリリース
00274   //中心点（押した点）とリリース点を結ぶ線分がロボットの方位角を示す
00275   switch (event)
00276     {
00277     case CV_EVENT_LBUTTONDOWN:
00278       lbuttondown_pt.x = x;
00279       lbuttondown_pt.y = y;
00280       rstat->x = x-TEMPLATE_SIZE/2;
00281       rstat->y = y-TEMPLATE_SIZE/2;
00282       break;
00283     case CV_EVENT_LBUTTONUP:
00284       rstat->r = atan2(x-lbuttondown_pt.x,y-lbuttondown_pt.y);
00285       printf("rstat->(x,y,r)=(%4d,%4d, %3.3f )\n", lbuttondown_pt.x-TEMPLATE_SIZE/2, lbuttondown_pt.y-TEMPLATE_SIZE/2, rstat->r );
00286       break;
00287     }
00288 }