SRCalibratedLib.cpp
Go to the documentation of this file.
00001 #include "SRCalibratedLib.h"
00002 
00003 void
00004 SRCalibratedLib::makeConvertMap ()
00005 {
00006   if ((info_depth_.D[0] != cvmGet(dist_coeff, 0, 0)) ||
00007       (info_depth_.D[1] != cvmGet(dist_coeff, 0, 1)) ||
00008       (info_depth_.D[2] != cvmGet(dist_coeff, 0, 2)) ||
00009       (info_depth_.D[3] != cvmGet(dist_coeff, 0, 3)) ||
00010       (info_depth_.D[4] != cvmGet(dist_coeff, 0, 4)) ||
00011       ((depth_scale*info_depth_.K[3*0 + 0]) != cvmGet(cam_matrix, 0, 0)) ||
00012       ((depth_scale*info_depth_.K[3*0 + 2]) != cvmGet(cam_matrix, 0, 2)) ||
00013       ((depth_scale*info_depth_.K[3*1 + 1]) != cvmGet(cam_matrix, 1, 1)) ||
00014       ((depth_scale*info_depth_.K[3*1 + 2]) != cvmGet(cam_matrix, 1, 2)))
00015   {
00016     //
00017     if ( info_depth_.D.size() == 5 ) {
00018       for(int i=0; i < 5; i++) {
00019         cvmSet(dist_coeff, 0, i, info_depth_.D[i]);
00020       }
00021     } else if ( info_depth_.D.size() == 8 ) {
00022       ROS_INFO("using rational polynomial model");
00023       dist_coeff = cvCreateMat(1, 8, CV_64F);
00024       for(int i=0; i < 8; i++) {
00025         cvmSet(dist_coeff, 0, i, info_depth_.D[i]);
00026       }
00027     }
00028     //
00029     cvSetZero(cam_matrix);
00030     cvmSet(cam_matrix, 2, 2, 1.0);
00031     cvmSet(cam_matrix, 0, 0, depth_scale*(info_depth_.K[3*0 + 0]));//kx
00032     cvmSet(cam_matrix, 0, 2, depth_scale*(info_depth_.K[3*0 + 2]));//cx
00033     cvmSet(cam_matrix, 1, 1, depth_scale*(info_depth_.K[3*1 + 1]));//ky
00034     cvmSet(cam_matrix, 1, 2, depth_scale*(info_depth_.K[3*1 + 2]));//cy
00035     //
00036     CvMat *src = cvCreateMat(srheight*srwidth, 1, CV_32FC2);
00037     CvMat *dst = cvCreateMat(srheight*srwidth, 1, CV_32FC2);
00038     CvPoint2D32f *ptr = (CvPoint2D32f *)src->data.ptr;
00039     for (int v=0; v<srheight; v++)
00040       for (int u=0; u<srwidth; u++)
00041       {
00042         ptr->x = u;
00043         ptr->y = v;
00044         ptr++;
00045       }
00046 
00047     if(map_x != 0) delete map_x;
00048     if(map_y != 0) delete map_y;
00049     if(map_z != 0) delete map_z;
00050     map_x = new float[srwidth*srheight];
00051     map_y = new float[srwidth*srheight];
00052     map_z = new float[srwidth*srheight];
00053 
00054     cvUndistortPoints(src, dst, cam_matrix, dist_coeff, NULL, NULL);
00055     ptr = (CvPoint2D32f *)dst->data.ptr;
00056     for (int i=0; i<srheight*srwidth; i++)
00057       {
00058         float xx = ptr->x;
00059         float yy = ptr->y;
00060         ptr++;
00061         double norm = sqrt(xx * xx + yy * yy + 1.0);
00062         map_x[i] = xx / norm;
00063         map_y[i] = yy / norm;
00064         map_z[i] = 1.0 / norm;
00065       }
00066     cvReleaseMat(&src);
00067     cvReleaseMat(&dst);
00068     ROS_INFO("make conversion map");
00069   } else {
00070     //ROS_WARN("do nothing!");
00071   }
00072 }
00073 
00074 void
00075 SRCalibratedLib::convert3DPos (sensor_msgs::PointCloud &pts)
00076 {
00077   int lng=(srwidth*srheight);
00078   unsigned short *ibuf = (unsigned short*)ipl_depth_->imageData;
00079   geometry_msgs::Point32 *pt = &(pts.points[0]);
00080 
00081   for (int i=0; i<lng; i++){
00082     double scl = (max_range *  ibuf[i]) / (double)0xFFFF;
00083 
00084     if (short_range)
00085     {
00086       // magic number in this process from calibration results
00087       if (scl < 1.0)
00088       {
00089         scl *= ((-7.071927e+02*pow(scl,3) + 1.825608e+03*pow(scl,2) - 1.571370E+03*scl + 1.454931e+03)/1000.0);
00090       } else if (scl < 1.1) {
00091         scl *= ((1000 + (1.9763 / 100.0) * (1100.0 - 1000.0*scl)) /1000.0);
00092       }
00093     }
00094 
00095     if (ibuf[i] >= 0xFFF8) { // saturate
00096       pt->x = 0.0;
00097       pt->y = 0.0;
00098       pt->z = 0.0;
00099     } else {
00100       pt->x = (map_x[i] * scl);
00101       pt->y = (map_y[i] * scl);
00102       pt->z = (map_z[i] * scl);
00103     }
00104     pt++;
00105   }
00106 }
00107 
00108 SRCalibratedLib::SRCalibratedLib () : map_x(0), map_y(0), map_z(0)
00109 {
00110     ipl_depth_ = new IplImage();
00111     cam_matrix = cvCreateMat(3, 3, CV_64F);
00112     dist_coeff = cvCreateMat(1, 5, CV_64F);
00113     cvSetZero(cam_matrix);
00114     cvSetZero(dist_coeff);
00115     cvmSet(cam_matrix, 2, 2, 1.0);
00116 }
00117 
00118 void
00119 SRCalibratedLib::setRengeImg (const sensor_msgs::ImageConstPtr &img_conf,
00120                               const sensor_msgs::ImageConstPtr &img_intent,
00121                               const sensor_msgs::ImageConstPtr &img_depth,
00122                               const sensor_msgs::CameraInfoConstPtr &info)
00123 {
00124   if ( (ipl_depth_->width != (int)img_depth->width) ||
00125        (ipl_depth_->height != (int)img_depth->height) )
00126   {
00127     ipl_depth_ = cvCreateImage(cvSize(img_depth->width, img_depth->height), IPL_DEPTH_16U, 1);
00128     srwidth = img_depth->width;
00129     srheight = img_depth->height;
00130   }
00131 
00132   sensor_msgs::CvBridge bridge;
00133   cvResize(bridge.imgMsgToCv(img_depth), ipl_depth_); // pass through
00134   info_depth_ = *info;
00135 
00136   if ( img_conf != img_depth && img_intent != img_depth )
00137   {
00138     const unsigned char *conf_buf = &(img_conf->data[0]);
00139     const unsigned char *intent_buf = &(img_intent->data[0]);
00140     unsigned short *depth_buf = (unsigned short*)ipl_depth_->imageData;
00141     int size = img_conf->data.size();
00142     for(int i=0;i<size;i++)
00143       {
00144         if( (conf_buf[i] < confidence_threshold) ||
00145             (intent_buf[i] < intensity_threshold) )
00146           depth_buf[i] = 0;
00147       }
00148   }
00149 
00150   if (depth_scale != 1.0)
00151   {
00152     srwidth = (int) (srwidth * depth_scale);
00153     srheight = (int) (srheight * depth_scale);
00154     IplImage *tmp_ipl_ = cvCreateImage(cvSize(srwidth, srheight), IPL_DEPTH_16U, 1);
00155     cvResize(ipl_depth_, tmp_ipl_);
00156     ipl_depth_ = tmp_ipl_;
00157   }
00158 }
00159 
00160 void
00161 SRCalibratedLib::calc3DPoints (sensor_msgs::PointCloud &pts_)
00162 {
00163   // smooth birateral filter
00164   if (use_smooth)
00165   {
00166     cv::Mat in_img16(ipl_depth_);
00167     cv::Mat in_imgf32(ipl_depth_->height, ipl_depth_->width, CV_32FC1);
00168     cv::Mat out_imgf32(ipl_depth_->height, ipl_depth_->width, CV_32FC1);
00169     in_img16.convertTo(in_imgf32, CV_32FC1);
00170     cv::bilateralFilter(in_imgf32, out_imgf32, smooth_size, smooth_depth, smooth_space, cv::BORDER_REPLICATE);
00171     out_imgf32.convertTo(in_img16, CV_16UC1);
00172   }
00173 
00174   // filter outlier
00175   if (use_filter)
00176   {
00177     cv::Mat in_img16(ipl_depth_);
00178     cv::Mat in_img(in_img16.rows, in_img16.cols, CV_8UC1);
00179     cv::Mat out_img(in_img16.rows, in_img16.cols, CV_8UC1);
00180     in_img16.convertTo(in_img, CV_8UC1, 1.0 / ( 1.0 * 256.0));
00181 
00182     cv::Canny(in_img, out_img, edge1, edge2);
00183     if(dilate_times >= 1)
00184       cv::dilate(out_img, out_img, cv::Mat(), cv::Point(-1, -1), dilate_times);
00185 
00186     unsigned short *sptr = (unsigned short *)in_img16.data;
00187     unsigned char *cptr = (unsigned char *)out_img.data;
00188     for(int i=0;i<in_img16.rows*in_img16.cols;i++)
00189       if(*cptr++ > 128) sptr[i] = 0;
00190   }
00191 
00192   // check info and make map
00193   makeConvertMap();
00194 
00195   //convert
00196   pts_.points.resize(srwidth*srheight);
00197   convert3DPos(pts_);
00198 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


cr_capture
Author(s): youhei kakiuchi, JSK
autogenerated on Sun Mar 24 2013 02:36:21