CRLib.cpp
Go to the documentation of this file.
00001 #include "CRLib.h"
00002 
00003 CRLib::CRLib ()
00004 {
00005   ipl_left_ = new IplImage();
00006   ipl_right_ = new IplImage();
00007 }
00008 
00009 void
00010 CRLib::setLeftImg (const sensor_msgs::ImageConstPtr &img,
00011                    const sensor_msgs::CameraInfoConstPtr &info)
00012 {
00013   if( (ipl_left_->width != (int)img->width) ||
00014       (ipl_left_->height != (int)img->height) )
00015   {
00016       ipl_left_ = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 3);
00017   }
00018   IplImage iplimg(cv_bridge::toCvCopy(img, "rgb8")->image);
00019   cvResize(&iplimg, ipl_left_);
00020   info_left_ = *info;
00021 }
00022 
00023 void
00024 CRLib::setRightImg (const sensor_msgs::ImageConstPtr &img,
00025                     const sensor_msgs::CameraInfoConstPtr &info)
00026 {
00027   if( (ipl_right_->width != (int)img->width) ||
00028       (ipl_right_->height != (int)img->height) )
00029   {
00030     ipl_right_ = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 3);
00031   }
00032   IplImage iplimg(cv_bridge::toCvCopy(img, "rgb8")->image);
00033   cvResize(&iplimg, ipl_right_);
00034   info_right_ = *info;
00035 }
00036 
00037 bool
00038 CRLib::calcColor (sensor_msgs::PointCloud &pts, int srwidth, int srheight,
00039                   cr_capture::PixelIndices *pidx)
00040 {
00041   if ( (ipl_right_->width != ipl_left_->width) ||
00042        (ipl_right_->height != ipl_left_->height) )
00043     return false;
00044 
00045   pts.channels.resize(1);
00046   pts.channels[0].name = "rgb";
00047   pts.channels[0].values.resize(srwidth*srheight);
00048 
00049   geometry_msgs::Point32 *point_ptr = &(pts.points[0]);
00050   float fx = info_left_.P[0];
00051   float cx = info_left_.P[2];
00052   float fy = info_left_.P[5];
00053   float cy = info_left_.P[6];
00054   float tr = info_right_.P[3]; // for ROS projection matrix (unit = m)
00055 
00056   int *lu_ptr = NULL, *ru_ptr = NULL, *v_ptr = NULL;
00057   if (pidx != NULL)
00058   {
00059     lu_ptr = new int[srheight*srwidth];
00060     ru_ptr = new int[srheight*srwidth];
00061     v_ptr = new int[srheight*srwidth];
00062       for(int i=0; i < srheight*srwidth; i++)
00063       {
00064         lu_ptr[i] = -1;
00065         ru_ptr[i] = -1;
00066         v_ptr[i] = -1;
00067       }
00068   }
00069 
00070   unsigned char *imgl = (unsigned char *)ipl_left_->imageData;
00071   unsigned char *imgr = (unsigned char *)ipl_right_->imageData;
00072   int w = ipl_left_->width;
00073   int h = ipl_left_->height;
00074   int step = ipl_left_->widthStep;
00075 
00076 #define getPixel(img_ptr, pix_x, pix_y, color_r, color_g, color_b)      \
00077   { color_r = img_ptr[step*pix_y + pix_x*3 + 0];                        \
00078     color_g = img_ptr[step*pix_y + pix_x*3 + 1];                        \
00079     color_b = img_ptr[step*pix_y + pix_x*3 + 2]; }
00080 
00081   int ypos[srwidth];
00082   int lxpos[srwidth];
00083   int rxpos[srwidth];
00084   int col_x[srwidth];
00085   int lr_use[srwidth];
00086 
00087   for (int y=0; y<srheight; y++)
00088   {
00089     for (int x=0; x<srwidth; x++)
00090     {
00091       int index = y*srwidth + x;
00092       // convert camera coordinates
00093       tf::Vector3 pos(point_ptr[index].x, point_ptr[index].y, point_ptr[index].z);
00094       tf::Vector3 tpos = cam_trans * pos;
00095 
00096       float posx = tpos[0];
00097       float posy = tpos[1];
00098       float posz = tpos[2];
00099       if (posz > 0.100)
00100       { // filtering near points
00101         lxpos[x] = (int)(fx/posz * posx + cx);      // left cam
00102         rxpos[x] = (int)((fx*posx + tr)/posz + cx); // right cam
00103         ypos[x] = (int)(fy/posz * posy + cy);
00104       } else {
00105         lxpos[x] = -1;
00106         rxpos[x] = -1;
00107         ypos[x] = -1;
00108       }
00109       //ROS_INFO("%d\t%d\t%d",lxpos[x],rxpos[x],ypos[x]);
00110     }
00111     memset(lr_use, 0, sizeof(int)*srwidth);
00112     memset(col_x, 0x01000000, sizeof(int)*srwidth);
00113 
00114     int max_lx = -1;
00115     int min_rx = w;
00116     for (int x=0; x<srwidth; x++)
00117     {
00118       int lx = lxpos[x];
00119       int ly = ypos[x];
00120 
00121       int pr = srwidth -x -1;
00122       int rx = rxpos[pr];
00123       int ry = ypos[pr];
00124 
00125       if ((w > lx ) && (lx >= 0)
00126           && (h > ly) && (ly >= 0)) {
00127         if(lx >= max_lx) {
00128           max_lx = lx;
00129         } else {
00130           lr_use[x] = -1; // use right
00131         }
00132       }
00133       if ((w > rx) && (rx >= 0)
00134           && (h > ry) && (ry >= 0)) {
00135         if(rx <= min_rx) {
00136           min_rx = rx;
00137         } else {
00138           lr_use[pr] = 1; // use left
00139         }
00140       }
00141     }
00142     // finding similar color
00143     unsigned char lcolr=0, lcolg=0, lcolb=0;
00144     unsigned char rcolr=0, rcolg=0, rcolb=0;
00145     for (int x=0; x<srwidth; x++) {
00146       if (lr_use[x]==0)
00147       {
00148         int lx = lxpos[x];
00149         int rx = rxpos[x];
00150         int yy = ypos[x];
00151         if ((w > lx ) && (lx >= 0)
00152             && (w > rx) && (rx >= 0)
00153             && (h > yy) && (yy >= 0))
00154         {
00155           getPixel(imgl, lx, yy, lcolr, lcolg, lcolb);
00156           getPixel(imgr, rx, yy, rcolr, rcolg, rcolb);
00157 
00158           double norm = 0.0;
00159           double norm_r = (double)(lcolr - rcolr);
00160           double norm_g = (double)(lcolg - rcolg);
00161           double norm_b = (double)(lcolb - rcolb);
00162           norm += norm_r * norm_r;
00163           norm += norm_g * norm_g;
00164           norm += norm_b * norm_b;
00165           norm = sqrt(norm);
00166 
00167           if(norm < 50.0) { // magic number for the same color
00168             col_x[x] = ((0xFF & ((lcolr + rcolr)/2)) << 16) |
00169               ((0xFF & ((lcolg + rcolg)/2)) << 8) |
00170               (0xFF & ((lcolb + rcolb)/2));
00171             if(pidx != NULL) {
00172               int ptr_pos = (x + y*srwidth);
00173               lu_ptr[ptr_pos] = lx;
00174               ru_ptr[ptr_pos] = rx;
00175               v_ptr[ptr_pos]  = yy;
00176             }
00177           } else {
00178             col_x[x] = 0x2000000;
00179             // find nearest one in next loop
00180           }
00181         } else if ((w > lx ) && (lx >= 0)
00182                    && (h > yy) && (yy >= 0))
00183         {
00184           // only left camera is viewing
00185           getPixel(imgl, lx, yy, lcolr, lcolg, lcolb);
00186           col_x[x] = ((0xFF & lcolr) << 16) | ((0xFF & lcolg) << 8) | (0xFF & lcolb);
00187 
00188           if(pidx != NULL) {
00189             int ptr_pos = (x + y*srwidth);
00190             lu_ptr[ptr_pos] = lx;
00191             v_ptr[ptr_pos]  = yy;
00192           }
00193         } else if ((w > rx ) && (rx >= 0)
00194                    && (h > yy) && (yy >= 0))
00195         {
00196           // only right camera is viewing
00197           getPixel(imgr, rx, yy, rcolr, rcolg, rcolb);
00198           col_x[x] = ((0xFF & rcolr) << 16) | ((0xFF & rcolg) << 8) | (0xFF & rcolb);
00199 
00200           if (pidx != NULL) {
00201             int ptr_pos = (x + y*srwidth);
00202             ru_ptr[ptr_pos] = rx;
00203             v_ptr[ptr_pos]  = yy;
00204           }
00205         } else {
00206           // did not find corresponding points in image
00207           col_x[x] = 0xFF0000;
00208           if (clear_uncolored_points) {
00209             int pidx = y*srwidth + x;
00210             point_ptr[pidx].x = 0.0;
00211             point_ptr[pidx].y = 0.0;
00212             point_ptr[pidx].z = 0.0;
00213           }
00214         }
00215       } else if (lr_use[x] > 0) {
00216         // use left
00217         int lx = lxpos[x];
00218         int ly = ypos[x];
00219         getPixel(imgl, lx, ly, lcolr, lcolg, lcolb);
00220         col_x[x] = ((0xFF & lcolr) << 16) | ((0xFF & lcolg) << 8) | (0xFF & lcolb);
00221 
00222         if (pidx != NULL) {
00223           int ptr_pos = (x + y*srwidth);
00224           lu_ptr[ptr_pos] = lx;
00225           v_ptr[ptr_pos]  = ly;
00226         }
00227       } else {
00228         // use right
00229         int rx = rxpos[x];
00230         int ry = ypos[x];
00231         getPixel(imgr, rx, ry, rcolr, rcolg, rcolb);
00232         col_x[x] = ((0xFF & rcolr) << 16) | ((0xFF & rcolg) << 8) | (0xFF & rcolb);
00233 
00234         if (pidx != NULL) {
00235           int ptr_pos = (x + y*srwidth);
00236           ru_ptr[ptr_pos] = rx;
00237           v_ptr[ptr_pos]  = ry;
00238         }
00239       }
00240     }
00241     // checking color of nearest one
00242     for (int x=0; x<srwidth; x++) {
00243       if (col_x[x] & 0x02000000) {
00244         int n = 0x02000000;
00245         for (int p=0; p<srwidth; p++) {
00246           if ((x+p >= srwidth) &&
00247               (x-p < 0)) {
00248             break;
00249           } else {
00250             if(x+p < srwidth) {
00251               if(!(col_x[x+p] & 0xFF000000)){
00252                 n = col_x[x+p];
00253                 break;
00254               }
00255             }
00256             if(x-p >= 0) {
00257               if(!(col_x[x-p] & 0xFF000000)){
00258                 n = col_x[x-p];
00259                 break;
00260               }
00261             }
00262           }
00263         }
00264         if(!(n & 0xFF000000)) {
00265           int lx = lxpos[x];
00266           int rx = rxpos[x];
00267           int yy = ypos[x];
00268           getPixel(imgl, lx, yy, lcolr, lcolg, lcolb);
00269           getPixel(imgr, rx, yy, rcolr, rcolg, rcolb);
00270           int clr = (n >> 16) & 0xFF;
00271           int clg = (n >> 8) & 0xFF;
00272           int clb = (n >> 0) & 0xFF;
00273           int dif_l = abs(clr - lcolr) + abs(clg - lcolg) + abs(clb - lcolb);
00274           int dif_r = abs(clr - rcolr) + abs(clg - rcolg) + abs(clb - rcolb);
00275           if(dif_l < dif_r) {
00276             col_x[x] = ((0xFF & lcolr) << 16) | ((0xFF & lcolg) << 8) | (0xFF & lcolb);
00277 
00278             if(pidx != NULL) {
00279               int ptr_pos = (x + y*srwidth);
00280               lu_ptr[ptr_pos] = lx;
00281               v_ptr[ptr_pos]  = yy;
00282             }
00283           } else {
00284             col_x[x] = ((0xFF & rcolr) << 16) | ((0xFF & rcolg) << 8) | (0xFF & rcolb);
00285 
00286             if(pidx != NULL) {
00287               int ptr_pos = (x + y*srwidth);
00288               ru_ptr[ptr_pos] = rx;
00289               v_ptr[ptr_pos]  = yy;
00290             }
00291           }
00292         }
00293       }
00294     }
00295     // setting color
00296     float *colv = &(pts.channels[0].values[y*srwidth]);
00297     for (int x=0; x<srwidth; x++)
00298       colv[x] = *reinterpret_cast<float*>(&(col_x[x]));
00299   } // y_loop
00300 
00301   if ( pidx != NULL ) {
00302     pidx->header = pts.header;
00303     pidx->indices.resize(srwidth*srheight*3);
00304     for (int i=0; i<srwidth*srheight; i++)
00305       {
00306         pidx->indices[3*i + 0] = lu_ptr[i];
00307         pidx->indices[3*i + 1] = ru_ptr[i];
00308         pidx->indices[3*i + 2] = v_ptr[i];
00309       }
00310     delete lu_ptr;
00311     delete ru_ptr;
00312     delete v_ptr;
00313   }
00314   return true;
00315 }


cr_capture
Author(s): youhei kakiuchi, JSK
autogenerated on Mon Oct 6 2014 01:16:16