RS003: BlockMatching.cpp Source File

Go to the documentation of this file.
00001 #include "BlockMatching.h"
00002 #include "CeilingMap.h"
00003 #ifdef _DEBUG
00004 #ifdef WIN32
00005 #include <crtdbg.h>
00006 #endif
00007 #endif
00008 
00009 #ifdef _USE_SHOW_MAP
00010 #include "Common/sharedmem.h"
00011 extern robot_status *rstat;
00012 #endif
00013 
00014 #define SEARCH_RANGE    4
00015 #define SIZE_HALF       (block_size / 2) //モジュール間共通の値．何とかすべし
00016 #define ANGLE_MAX       (m_Resolution * 2 + 1)
00017 
00021 BlockMatching::BlockMatching(void) : m_BlockTable(0)
00022 {
00023 }
00024 
00029 BlockMatching::~BlockMatching(void)
00030 {
00031         if(m_BlockTable != 0)
00032                 DeleteBlockTable();
00033 }
00034 
00047 void BlockMatching::CreateBlockMap(const long center_x,
00048                                                    const long center_y,
00049                                                    const unsigned long image_width,
00050                                                    const unsigned long image_height,
00051                                                    const unsigned short block_size,
00052                                                    const double Coefficient,
00053                                                    const unsigned short Resolution,
00054                                                    const unsigned short BlackWhiteValue)
00055 {
00056         int             i;
00057         int             x,      y;
00058         int             x2,     y2;
00059         int             sx,     sy;
00060         int             degree_min, degree_max;
00061         double  cx,     cy, dx1, dy1;
00062         unsigned long width, height;
00063 
00064         /*
00065                 中心点が定義されていない場合は画像データの中心点を採用する
00066                 (最終的な中心座標とするため1/4サイズの中心にする)
00067         */
00068 
00069         width = image_width / 2;
00070         if(center_x == -1)
00071                 cx = width / 2;
00072         else
00073                 cx = center_x;  /* カメラセンター設定 */
00074 
00075         height = image_height / 2;
00076         if(center_y == -1)
00077                 cy = height / 2;
00078         else
00079                 cy = center_y;  /* カメラセンター設定 */
00080 
00081         sx = cx - (int)((double)SIZE_HALF / Coefficient);
00082         sy = cy - (int)((double)SIZE_HALF / Coefficient);
00083 
00084         m_Resolution = 360 / 2 / Resolution;
00085         degree_max = m_Resolution; //回転角のインデックスの数(正の方向)
00086         degree_min = m_Resolution * -1;//回転角のインデックスの数(負の方向)
00087 
00088         m_LatticeSize   = block_size;
00089         m_BlackWhiteValue = BlackWhiteValue;
00090 
00091         CreateBlockTable();
00092 
00093         for (y = 0; y < block_size; y++){
00094                 for (x = 0; x < block_size; x++){
00095                         for (i = degree_min; i <= degree_max; i++){
00096                                 x2 = sx + ( (double)((double)( x - SIZE_HALF )) * cos((double)i * M_PI / (double)m_Resolution) - 
00097                                                         (double)((double)( y - SIZE_HALF )) * sin((double)i * M_PI / (double)m_Resolution) + (double)SIZE_HALF ) / Coefficient;
00098 
00099                                 y2 = sy + ( (double)((double)( x - SIZE_HALF )) * sin((double)i * M_PI / (double)m_Resolution) +
00100                                                         (double)((double)( y - SIZE_HALF )) * cos((double)i * M_PI / (double)m_Resolution) + (double)SIZE_HALF ) / Coefficient;
00101 
00102 /*                              /\* 離散座標に変換にしてからでないと... *\/ */
00103 
00104                                 dx1 = x2;
00105                                 dy1 = y2;
00106 
00107                                 if (dx1 <   0 )                   dx1 = 0;
00108                                 if (dx1 > (width - 1) )   dx1 = (width - 1);
00109                                 if (dy1 <   0 )                 { dy1 = 0;                                      dx1=0; }
00110                                 if (dy1 > (height - 1) ){ dy1 = (height - 1); dx1=0; }
00111 
00112                                 m_BlockTable[ i + m_Resolution][x][y] = (int)((int)dx1 + (int)dy1 * (image_width / 2));
00113                         }
00114                 }
00115         }
00116 }
00117 
00121 void BlockMatching::CreateBlockTable()
00122 {
00123         m_BlockTable = new long**[ANGLE_MAX];
00124         for(int cnt_z = 0; cnt_z < ANGLE_MAX ; cnt_z++){
00125                 m_BlockTable[cnt_z] = new long*[m_LatticeSize];
00126                 for(int cnt_x_y = 0; cnt_x_y < m_LatticeSize; cnt_x_y++){
00127                         m_BlockTable[cnt_z][cnt_x_y] = new long[m_LatticeSize];
00128                 }
00129         }
00130 }
00131 
00135 void BlockMatching::DeleteBlockTable()
00136 {
00137         for(int cnt_z = 0; cnt_z < ANGLE_MAX ; cnt_z++){
00138                 for(int cnt_x_y = 0; cnt_x_y < m_LatticeSize; cnt_x_y++){
00139                         delete[] m_BlockTable[cnt_z][cnt_x_y];
00140                 }
00141                 delete[] m_BlockTable[cnt_z];
00142         }
00143         delete[] m_BlockTable;
00144         m_BlockTable = 0;
00145 }
00146 
00153 void BlockMatching::MapTracking(CeilingMap &CeilingMap, ImageData &CurImage)
00154 {
00155         ImageData               capImage;
00156         SignedOdometry_st       odometry;
00157 #ifdef _USE_SHOW_MAP
00158         int                             x,              y;
00159 #endif
00160         static int reach = 0; //マッチングの探索範囲を調整するパラメータと思われるがとりあえず不使用に
00161 
00162         /* マップマッチング */
00163         /* マッチングするには、キャプチャ画像を1/4スケールに縮める必要がある */
00164         capImage = CurImage.PyrDown();
00165 
00166         odometry = Matching(CeilingMap, capImage, reach);
00167         CeilingMap.IncrementOdometry(odometry);
00168 
00169 #ifdef _USE_SHOW_MAP
00170         ImageData               PackImage;
00171         PackImage.CreateImage(m_LatticeSize, m_LatticeSize);
00172         Packing(capImage, PackImage, 0/*CeilingMap.GetTheta()*/);//ここで画像を回転している
00173 
00174         for (y = 0; y < m_LatticeSize; y++) {
00175                 for (x = 0; x < m_LatticeSize; x++) {
00176                         rstat->buf[x+y*m_LatticeSize] = (PackImage[x+y*m_LatticeSize]);
00177                 }
00178         }  
00179         rstat->x = CeilingMap.GetPosX();
00180         rstat->y = CeilingMap.GetPosY();
00181         rstat->r = CeilingMap.GetTheta();
00182 #endif
00183 }
00184 
00191 SignedOdometry_st BlockMatching::Matching(CeilingMap &CeilingMap, ImageData &CurImage, int reach)
00192 {
00193         SignedOdometry_st       result;
00194         ImageData               PackImage;
00195         int                             min;
00196         int                             i,              s;
00197         int                             x,              y;
00198 
00199         result.m_x = 0;
00200         result.m_y = 0;
00201         result.m_theta = 0.0;
00202 
00203         PackImage.CreateImage(m_LatticeSize, m_LatticeSize);
00204         Packing(CurImage, PackImage, CeilingMap.GetTheta());
00205 
00206         // 評価基準値を求めておく・・・現在の位置より良い値を探すため
00207         min = SumOfAbsoluteDifference(CeilingMap, PackImage, CeilingMap.GetPosX(), CeilingMap.GetPosY());
00208 
00209         //角度方向探索 ±m_Resolution * SEARCH_RANGE
00210         for ( i = -(SEARCH_RANGE + reach); i <= (SEARCH_RANGE + reach); i++) { 
00211 
00212                 Packing(CurImage, PackImage, CeilingMap.GetTheta() + (double)( i * M_PI / m_Resolution ));
00213                 //X方向探索 ±SEARCH_RANGE
00214                 for ( x = -(SEARCH_RANGE + reach); x <= (SEARCH_RANGE + reach); x++) {
00215 
00216                         //Y方向探索 ±SEARCH_RANGE
00217                         for ( y = -(SEARCH_RANGE + reach); y <= (SEARCH_RANGE + reach); y++) {
00218                                 s = SumOfAbsoluteDifference(CeilingMap, PackImage,  CeilingMap.GetPosX() + x, CeilingMap.GetPosY() + y);
00219 
00220                                 if (s < min) {
00221                                         //現在地の更新
00222                                         min = s;
00223                                         result.m_x = x;
00224                                         result.m_y = y;
00225                                         result.m_theta = i;
00226                                 }
00227                         }
00228                 }
00229         }
00230         result.m_theta = result.m_theta * M_PI / m_Resolution;
00231 
00232         return result;
00233 }
00234 
00235 
00245 int BlockMatching::SumOfAbsoluteDifference(ImageData &src, ImageData &data, unsigned long offset_x, unsigned long offset_y)
00246 {
00247         int cell_x, cell_y;
00248         int result, diff;
00249 
00250         result = 0;
00251         if ( src.GetWidth() == 0 || src.GetHeight() == 0 || data.GetWidth() == 0 || data.GetHeight() == 0 ) {
00252           fprintf(stderr, "Warning : %ssrc or data image is 0 size\n", __PRETTY_FUNCTION__);
00253           return result;
00254         }
00255         for( cell_y = 0; cell_y < m_LatticeSize; cell_y++) {
00256                 for( cell_x = 0; cell_x < m_LatticeSize; cell_x++) {
00257                         diff = src(cell_x + offset_x, cell_y + offset_y) - data(cell_x, cell_y);
00258                         if ( diff < 0)
00259                                 result -= diff;
00260                 }
00261         }
00262         return result;
00263 }
00264 
00274 void BlockMatching::Packing(ImageData &org, ImageData &packData, double theta)
00275 {
00276         int x, y;
00277         int rotation;
00278 
00279         if (theta > M_PI)               /* 180度を越えている場合、マイナスに変更する */
00280                 theta = -M_PI * 2 + theta;
00281         else if (theta < -M_PI) /* -180を下回る場合、プラスに変更する */
00282                 theta = M_PI * 2 + theta;
00283 
00284         rotation = (int)(theta / ( M_PI / m_Resolution ) ) + m_Resolution;      /* ラジアン->度 変換 */
00285 
00286         for (x=0; x < m_LatticeSize; x++) {
00287                 for (y=0; y < m_LatticeSize; y++) {
00288                         if (m_BlockTable[rotation][x][y] >= 0 && m_BlockTable[rotation][x][y] < org.GetWidth() * org.GetHeight())
00289                                 packData[x+y*m_LatticeSize] = org[m_BlockTable[rotation][x][y]];        /* 回転した場合の座標位置にあるデータを取り出す */
00290                         else
00291                                 packData[x+y*m_LatticeSize] = 0;
00292 
00293                         if (packData[x+y*m_LatticeSize] > m_BlackWhiteValue )   /* 2値化の閾値判定を行い、白/黒に変更する */
00294                                 packData[x+y*m_LatticeSize] = MAX_BRIGHTNESS;
00295                         else
00296                                 packData[x+y*m_LatticeSize] = 0;
00297                 }
00298         }
00299 }