| 241 | } |
| 242 | |
| 243 | void |
| 244 | imageCb(const ImageConstPtr& image_msg, |
| 245 | const ImageConstPtr& disparity_msg, |
| 246 | const opt_msgs::DetectionArray::ConstPtr& detection_msg) |
| 247 | { |
| 248 | // Callback for people detection: |
| 249 | bool label_all; |
| 250 | vector<int> L_in; |
| 251 | vector<int> L_out; |
| 252 | vector<float> C_out; |
| 253 | vector<Rect> R_in; |
| 254 | vector<Rect> R_out; |
| 255 | string param_name; |
| 256 | string nn = ros::this_node::getName(); |
| 257 | string cfnm; |
| 258 | int numSamples; |
| 259 | double temp=0.0; |
| 260 | |
| 261 | // check encoding and create an intensity image from disparity image |
| 262 | assert(disparity_msg->encoding == image_encodings::TYPE_32FC1); |
| 263 | cv::Mat_<float> dmatrix(disparity_msg->height, |
| 264 | disparity_msg->width, |
| 265 | (float*) &disparity_msg->data[0], |
| 266 | disparity_msg->step); |
| 267 | |
| 268 | if(!node_.getParam(nn + "/UseMissingDataMask",HDAC_.useMissingDataMask_)){ |
| 269 | HDAC_.useMissingDataMask_ = false; |
| 270 | } |
| 271 | |
| 272 | cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(disparity_msg, image_encodings::MONO8); |
| 273 | // cv::Mat distance_image = cv_ptr->image; |
| 274 | // cv::imwrite("/home/matteo/distance.png", distance_image); |
| 275 | |
| 276 | // **********************************************************************// |
| 277 | // between these comments lies a hack that accounts for the difference // |
| 278 | // between the focal lengths of the kinect's color camera and ir cameras // |
| 279 | // TODO, parameterize to disable this hack for the stereo data // |
| 280 | bool kinect_disparity_fix; |
| 281 | if(!node_.getParam(nn + "/Kinect_Disparity_Fix",kinect_disparity_fix)){ |
| 282 | kinect_disparity_fix = false; |
| 283 | } |
| 284 | |
| 285 | if(kinect_disparity_fix){ |
| 286 | int nrows = 434; |
| 287 | int ncols = 579; |
| 288 | int row_offset = (dmatrix.rows - nrows)/2; |
| 289 | int col_offset = (dmatrix.cols - ncols)/2; |
| 290 | cv::Mat Scaled_Disparity(nrows,ncols,CV_32FC1); |
| 291 | resize(dmatrix,Scaled_Disparity,cv::Size(ncols,nrows),0,0, CV_INTER_NN ); |
| 292 | for(int i=0;i<dmatrix.rows;i++){ |
| 293 | for(int j=0;j<dmatrix.cols;j++){ |
| 294 | dmatrix.at<float>(i,j) = 0.0; |
| 295 | } |
| 296 | } |
| 297 | for(int i=0;i<nrows;i++){ |
| 298 | for(int j=0;j<ncols;j++){ |
| 299 | dmatrix.at<float>(i+row_offset,j+col_offset) = Scaled_Disparity.at<float>(i,j); |
| 300 | } |