| 260 | } |
| 261 | |
| 262 | void |
| 263 | imageCb(const ImageConstPtr& image_msg, |
| 264 | const DisparityImageConstPtr& disparity_msg, |
| 265 | const opt_msgs::DetectionArray::ConstPtr& detection_msg) |
| 266 | { |
| 267 | // Callback for people detection: |
| 268 | bool label_all; |
| 269 | vector<int> L_in; |
| 270 | vector<int> L_out; |
| 271 | vector<float> C_out; |
| 272 | vector<Rect> R_in; |
| 273 | vector<Rect> R_out; |
| 274 | string param_name; |
| 275 | string nn = ros::this_node::getName(); |
| 276 | string cfnm; |
| 277 | int numSamples; |
| 278 | double temp=0.0; |
| 279 | |
| 280 | // check encoding and create an intensity image from disparity image |
| 281 | assert(disparity_msg->image.encoding == image_encodings::TYPE_32FC1); |
| 282 | cv::Mat_<float> dmatrix(disparity_msg->image.height, |
| 283 | disparity_msg->image.width, |
| 284 | (float*) &disparity_msg->image.data[0], |
| 285 | disparity_msg->image.step); |
| 286 | |
| 287 | if(!node_.getParam("UseMissingDataMask",HDAC_.useMissingDataMask_)){ |
| 288 | HDAC_.useMissingDataMask_ = false; |
| 289 | } |
| 290 | |
| 291 | |
| 292 | // **********************************************************************// |
| 293 | // between these comments lies a hack that accounts for the difference // |
| 294 | // between the focal lengths of the kinect's color camera and ir cameras // |
| 295 | // TODO, parameterize to disable this hack for the stereo data // |
| 296 | bool kinect_disparity_fix; |
| 297 | if(!node_.getParam("Kinect_Disparity_Fix",kinect_disparity_fix)){ |
| 298 | kinect_disparity_fix = false; |
| 299 | } |
| 300 | |
| 301 | if(kinect_disparity_fix){ |
| 302 | int nrows = 434; |
| 303 | int ncols = 579; |
| 304 | int row_offset = (dmatrix.rows - nrows)/2; |
| 305 | int col_offset = (dmatrix.cols - ncols)/2; |
| 306 | cv::Mat Scaled_Disparity(nrows,ncols,CV_32FC1); |
| 307 | resize(dmatrix,Scaled_Disparity,cv::Size(ncols,nrows),0,0, CV_INTER_NN ); |
| 308 | for(int i=0;i<dmatrix.rows;i++){ |
| 309 | for(int j=0;j<dmatrix.cols;j++){ |
| 310 | dmatrix.at<float>(i,j) = 0.0; |
| 311 | } |
| 312 | } |
| 313 | for(int i=0;i<nrows;i++){ |
| 314 | for(int j=0;j<ncols;j++){ |
| 315 | dmatrix.at<float>(i+row_offset,j+col_offset) = Scaled_Disparity.at<float>(i,j); |
| 316 | } |
| 317 | } |
| 318 | } |
| 319 | // **********************************************************************// |