void Stereoproc::imageCb( const sensor_msgs::ImageConstPtr& l_raw_msg, const sensor_msgs::CameraInfoConstPtr& l_info_msg, const sensor_msgs::ImageConstPtr& r_raw_msg, const sensor_msgs::CameraInfoConstPtr& r_info_msg) { boost::lock_guard<boost::recursive_mutex> config_lock(config_mutex_); boost::lock_guard<boost::mutex> connect_lock(connect_mutex_); int level = connected_.level(); NODELET_DEBUG("got images, level %d", level); // Update the camera model model_.fromCameraInfo(l_info_msg, r_info_msg); cv::cuda::Stream l_strm, r_strm; cv::cuda::GpuMat l_raw, r_raw; std::vector<GPUSender::Ptr> senders; // Create cv::Mat views onto all buffers const cv::Mat l_cpu_raw = cv_bridge::toCvShare( l_raw_msg, l_raw_msg->encoding)->image; cv::cuda::registerPageLocked(const_cast<cv::Mat&>(l_cpu_raw)); const cv::Mat r_cpu_raw = cv_bridge::toCvShare( r_raw_msg, l_raw_msg->encoding)->image; cv::cuda::registerPageLocked(const_cast<cv::Mat&>(r_cpu_raw)); l_raw.upload(l_cpu_raw, l_strm); r_raw.upload(r_cpu_raw, r_strm); cv::cuda::GpuMat l_mono; if(connected_.DebayerMonoLeft || connected_.RectifyMonoLeft || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud) { cv::cuda::demosaicing(l_raw, l_mono, CV_BayerRG2GRAY, 1, l_strm); } if(connected_.DebayerMonoLeft) { GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_left_)); senders.push_back(t); t->enqueueSend(l_mono, l_strm); } cv::cuda::GpuMat r_mono; if(connected_.DebayerMonoRight || connected_.RectifyMonoRight || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud) { cv::cuda::demosaicing(r_raw, r_mono, CV_BayerRG2GRAY, 1, r_strm); } if(connected_.DebayerMonoRight) { GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_right_)); senders.push_back(t); t->enqueueSend(r_mono, r_strm); } cv::cuda::GpuMat l_color; if(connected_.DebayerColorLeft || connected_.RectifyColorLeft || connected_.Pointcloud) { cv::cuda::demosaicing(l_raw, l_color, CV_BayerRG2BGR, 3, l_strm); } if(connected_.DebayerColorLeft) { GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_left_)); senders.push_back(t); t->enqueueSend(l_color, l_strm); } cv::cuda::GpuMat r_color; if(connected_.DebayerColorRight || connected_.RectifyColorRight) { cv::cuda::demosaicing(r_raw, r_color, CV_BayerRG2BGR, 3, r_strm); } if(connected_.DebayerColorRight) { GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_right_)); senders.push_back(t); t->enqueueSend(r_color, r_strm); } cv::cuda::GpuMat l_rect_mono, r_rect_mono; if(connected_.RectifyMonoLeft || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud) { model_.left().rectifyImageGPU(l_mono, l_rect_mono, cv::INTER_LINEAR, l_strm); } if(connected_.RectifyMonoRight || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud) { model_.right().rectifyImageGPU(r_mono, r_rect_mono, cv::INTER_LINEAR, r_strm); } if(connected_.RectifyMonoLeft) { GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_rect_left_)); senders.push_back(t); t->enqueueSend(l_rect_mono, l_strm); } if(connected_.RectifyMonoRight) { GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_rect_right_)); senders.push_back(t); t->enqueueSend(r_rect_mono, r_strm); } cv::cuda::GpuMat l_rect_color, r_rect_color; if(connected_.RectifyColorLeft || connected_.Pointcloud) { model_.left().rectifyImageGPU(l_color, l_rect_color, cv::INTER_LINEAR, l_strm); } if(connected_.RectifyColorRight) { model_.right().rectifyImageGPU(r_color, r_rect_color, cv::INTER_LINEAR, r_strm); } if(connected_.RectifyColorLeft) { GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_rect_left_)); senders.push_back(t); t->enqueueSend(l_rect_color, l_strm); } if(connected_.RectifyColorRight) { GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_rect_right_)); senders.push_back(t); t->enqueueSend(r_rect_color, r_strm); } cv::cuda::GpuMat disparity; cv::cuda::GpuMat disparity_16s; cv::cuda::GpuMat disparity_filtered; if(connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud) { r_strm.waitForCompletion(); block_matcher_->compute(l_rect_mono, r_rect_mono, disparity, l_strm); //allocate cpu-side resource filter_buf_.create(l_rect_mono.size(), CV_16SC1); //enqueueDownload disparity.convertTo(disparity_16s, CV_16SC1, 16, l_strm); disparity_16s.download(filter_buf_, l_strm); l_strm.waitForCompletion(); filterSpeckles(); //enqueueUpload disparity_16s.upload(filter_buf_, l_strm); if(bilateral_filter_enabled_) { bilateral_filter_->apply(disparity_16s, l_rect_mono, disparity_filtered); disparity_filtered.convertTo(disparity, CV_32FC1, 1/16.); } else { disparity_16s.convertTo(disparity, CV_32FC1, 1/16.); } } if(connected_.Disparity) { cv::cuda::GpuMat disparity_float; if(disparity.type() != CV_32F) disparity.convertTo(disparity_float, CV_32FC1); else disparity_float = disparity; // Adjust for any x-offset between the principal points: d' = d - (cx_l - cx_r) double cx_l = model_.left().cx(); double cx_r = model_.right().cx(); if (cx_l != cx_r) { cv::cuda::subtract(disparity_float, cv::Scalar(cx_l - cx_r), disparity_float, cv::cuda::GpuMat(), -1, l_strm); } // Allocate new disparity image message disp_msg_.reset(new stereo_msgs::DisparityImage()); disp_msg_->header = l_info_msg->header; disp_msg_->image.header = l_info_msg->header; // Compute window of (potentially) valid disparities int border = block_matcher_->getBlockSize() / 2; int left = block_matcher_->getNumDisparities() + block_matcher_min_disparity_ + border - 1; int wtf = (block_matcher_min_disparity_ >= 0) ? border + block_matcher_min_disparity_ : std::max(border, -block_matcher_min_disparity_); int right = disp_msg_->image.width - 1 - wtf; int top = border; int bottom = disp_msg_->image.height - 1 - border; disp_msg_->valid_window.x_offset = left; disp_msg_->valid_window.y_offset = top; disp_msg_->valid_window.width = right - left; disp_msg_->valid_window.height = bottom - top; disp_msg_->min_disparity = block_matcher_min_disparity_ + 1; disp_msg_->max_disparity = block_matcher_min_disparity_ + block_matcher_->getNumDisparities() - 1; disp_msg_->image.height = l_rect_mono.rows; disp_msg_->image.width = l_rect_mono.cols; disp_msg_->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; disp_msg_->image.step = l_rect_mono.cols * sizeof(float); disp_msg_->image.data.resize(disp_msg_->image.step*disp_msg_->image.height); disp_msg_data_ = cv::Mat_<float> (disp_msg_->image.height, disp_msg_->image.width, (float *)&disp_msg_->image.data[0], disp_msg_->image.step); cv::cuda::registerPageLocked(disp_msg_data_); disparity_float.download(disp_msg_data_, l_strm); l_strm.enqueueHostCallback( [](int status, void *userData) { (void)status; static_cast<Stereoproc*>(userData)->sendDisparity(); }, (void*)this); } if(connected_.DisparityVis) { GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::BGRA8, &pub_disparity_vis_)); senders.push_back(t); cv::cuda::GpuMat disparity_int(l_cpu_raw.size(), CV_16SC1); cv::cuda::GpuMat disparity_image(l_cpu_raw.size(), CV_8UC4); int ndisp = block_matcher_->getNumDisparities(); if(disparity.type() == CV_32F) { disparity.convertTo(disparity_int, CV_16SC1, 16, 0, l_strm); ndisp *= 16; } else disparity_int = disparity; try { cv::cuda::drawColorDisp(disparity_int, disparity_image, ndisp, l_strm); t->enqueueSend(disparity_image, l_strm); } catch(cv::Exception e) { NODELET_ERROR_STREAM("Unable to draw color disparity: " << e.err << "in " << e.file << ":" << e.func << " line " << e.line); } } cv::cuda::GpuMat xyz; if(connected_.Pointcloud) { model_.projectDisparityImageTo3dGPU(disparity, xyz, true, l_strm); cv::cuda::HostMem cuda_xyz(l_cpu_raw.size(), CV_32FC3); xyz.download(cuda_xyz, l_strm); cv::Mat l_cpu_color(l_cpu_raw.size(), CV_8UC3); l_color.download(l_cpu_color, l_strm); sensor_msgs::PointCloud2Ptr points_msg = boost::make_shared<sensor_msgs::PointCloud2>(); points_msg->header = l_raw_msg->header; points_msg->height = l_cpu_raw.rows; points_msg->width = l_cpu_raw.cols; points_msg->fields.resize (4); points_msg->fields[0].name = "x"; points_msg->fields[0].offset = 0; points_msg->fields[0].count = 1; points_msg->fields[0].datatype = sensor_msgs::PointField::FLOAT32; points_msg->fields[1].name = "y"; points_msg->fields[1].offset = 4; points_msg->fields[1].count = 1; points_msg->fields[1].datatype = sensor_msgs::PointField::FLOAT32; points_msg->fields[2].name = "z"; points_msg->fields[2].offset = 8; points_msg->fields[2].count = 1; points_msg->fields[2].datatype = sensor_msgs::PointField::FLOAT32; points_msg->fields[3].name = "rgb"; points_msg->fields[3].offset = 12; points_msg->fields[3].count = 1; points_msg->fields[3].datatype = sensor_msgs::PointField::FLOAT32; //points_msg->is_bigendian = false; ??? static const int STEP = 16; points_msg->point_step = STEP; points_msg->row_step = points_msg->point_step * points_msg->width; points_msg->data.resize (points_msg->row_step * points_msg->height); points_msg->is_dense = false; // there may be invalid points l_strm.waitForCompletion(); cv::Mat_<cv::Vec3f> cpu_xyz = cuda_xyz.createMatHeader(); float bad_point = std::numeric_limits<float>::quiet_NaN (); int offset = 0; for (int v = 0; v < cpu_xyz.rows; ++v) { for (int u = 0; u < cpu_xyz.cols; ++u, offset += STEP) { if (isValidPoint(cpu_xyz(v,u))) { // x,y,z,rgba memcpy (&points_msg->data[offset + 0], &cpu_xyz(v,u)[0], sizeof (float)); memcpy (&points_msg->data[offset + 4], &cpu_xyz(v,u)[1], sizeof (float)); memcpy (&points_msg->data[offset + 8], &cpu_xyz(v,u)[2], sizeof (float)); } else { memcpy (&points_msg->data[offset + 0], &bad_point, sizeof (float)); memcpy (&points_msg->data[offset + 4], &bad_point, sizeof (float)); memcpy (&points_msg->data[offset + 8], &bad_point, sizeof (float)); } } } // Fill in color offset = 0; const cv::Mat_<cv::Vec3b> color(l_cpu_color); for (int v = 0; v < cpu_xyz.rows; ++v) { for (int u = 0; u < cpu_xyz.cols; ++u, offset += STEP) { if (isValidPoint(cpu_xyz(v,u))) { const cv::Vec3b& bgr = color(v,u); int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0]; memcpy (&points_msg->data[offset + 12], &rgb_packed, sizeof (int32_t)); } else { memcpy (&points_msg->data[offset + 12], &bad_point, sizeof (float)); } } } pub_pointcloud_.publish(points_msg); } l_strm.waitForCompletion(); r_strm.waitForCompletion(); if(connected_.Disparity) cv::cuda::unregisterPageLocked(disp_msg_data_); //if(connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud) // cv::cuda::unregisterPageLocked(filter_buf_); cv::cuda::unregisterPageLocked( const_cast<cv::Mat&>(l_cpu_raw) ); cv::cuda::unregisterPageLocked( const_cast<cv::Mat&>(r_cpu_raw) ); }
void compute(InputArray leftarr, InputArray rightarr, OutputArray disparr) { int dtype = disparr.fixedType() ? disparr.type() : params.dispType; Size leftsize = leftarr.size(); if (leftarr.size() != rightarr.size()) CV_Error(Error::StsUnmatchedSizes, "All the images must have the same size"); if (leftarr.type() != CV_8UC1 || rightarr.type() != CV_8UC1) CV_Error(Error::StsUnsupportedFormat, "Both input images must have CV_8UC1"); if (dtype != CV_16SC1 && dtype != CV_32FC1) CV_Error(Error::StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format"); if (params.preFilterType != PREFILTER_NORMALIZED_RESPONSE && params.preFilterType != PREFILTER_XSOBEL) CV_Error(Error::StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE"); if (params.preFilterSize < 5 || params.preFilterSize > 255 || params.preFilterSize % 2 == 0) CV_Error(Error::StsOutOfRange, "preFilterSize must be odd and be within 5..255"); if (params.preFilterCap < 1 || params.preFilterCap > 63) CV_Error(Error::StsOutOfRange, "preFilterCap must be within 1..63"); if (params.kernelSize < 5 || params.kernelSize > 255 || params.kernelSize % 2 == 0 || params.kernelSize >= std::min(leftsize.width, leftsize.height)) CV_Error(Error::StsOutOfRange, "kernelSize must be odd, be within 5..255 and be not larger than image width or height"); if (params.numDisparities <= 0 || params.numDisparities % 16 != 0) CV_Error(Error::StsOutOfRange, "numDisparities must be positive and divisble by 16"); if (params.textureThreshold < 0) CV_Error(Error::StsOutOfRange, "texture threshold must be non-negative"); if (params.uniquenessRatio < 0) CV_Error(Error::StsOutOfRange, "uniqueness ratio must be non-negative"); int FILTERED = (params.minDisparity - 1) << DISPARITY_SHIFT; Mat left0 = leftarr.getMat(), right0 = rightarr.getMat(); Mat disp0 = disparr.getMat(); int width = left0.cols; int height = left0.rows; if(previous_size != width * height) { previous_size = width * height; speckleX.create(height,width,CV_32SC4); speckleY.create(height,width,CV_32SC4); puss.create(height,width,CV_32SC4); censusImage[0].create(left0.rows,left0.cols,CV_32SC4); censusImage[1].create(left0.rows,left0.cols,CV_32SC4); partialSumsLR.create(left0.rows + 1,(left0.cols + 1) * (params.numDisparities + 1),CV_16S); agregatedHammingLRCost.create(left0.rows + 1,(left0.cols + 1) * (params.numDisparities + 1),CV_16S); hammingDistance.create(left0.rows, left0.cols * (params.numDisparities + 1),CV_16S); preFilteredImg0.create(left0.size(), CV_8U); preFilteredImg1.create(left0.size(), CV_8U); aux.create(height,width,CV_8UC1); } Mat left = preFilteredImg0, right = preFilteredImg1; int ndisp = params.numDisparities; int wsz = params.kernelSize; int bufSize0 = (int)((ndisp + 2)*sizeof(int)); bufSize0 += (int)((height + wsz + 2)*ndisp*sizeof(int)); bufSize0 += (int)((height + wsz + 2)*sizeof(int)); bufSize0 += (int)((height + wsz + 2)*ndisp*(wsz + 2)*sizeof(uchar) + 256); int bufSize1 = (int)((width + params.preFilterSize + 2) * sizeof(int) + 256); if(params.usePrefilter == true) { uchar *_buf = slidingSumBuf.ptr(); parallel_for_(Range(0, 2), PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, ¶ms), 1); } else if(params.usePrefilter == false) { left = left0; right = right0; } if(params.kernelType == CV_SPARSE_CENSUS) { censusTransform(left,right,params.kernelSize,censusImage[0],censusImage[1],CV_SPARSE_CENSUS); } else if(params.kernelType == CV_DENSE_CENSUS) { censusTransform(left,right,params.kernelSize,censusImage[0],censusImage[1],CV_SPARSE_CENSUS); } else if(params.kernelType == CV_CS_CENSUS) { symetricCensusTransform(left,right,params.kernelSize,censusImage[0],censusImage[1],CV_CS_CENSUS); } else if(params.kernelType == CV_MODIFIED_CS_CENSUS) { symetricCensusTransform(left,right,params.kernelSize,censusImage[0],censusImage[1],CV_MODIFIED_CS_CENSUS); } else if(params.kernelType == CV_MODIFIED_CENSUS_TRANSFORM) { modifiedCensusTransform(left,right,params.kernelSize,censusImage[0],censusImage[1],CV_MODIFIED_CENSUS_TRANSFORM,0); } else if(params.kernelType == CV_MEAN_VARIATION) { parSumsIntensityImage[0].create(left0.rows, left0.cols,CV_32SC4); parSumsIntensityImage[1].create(left0.rows, left0.cols,CV_32SC4); Integral[0].create(left0.rows,left0.cols,CV_32SC4); Integral[1].create(left0.rows,left0.cols,CV_32SC4); integral(left, parSumsIntensityImage[0],CV_32S); integral(right, parSumsIntensityImage[1],CV_32S); imageMeanKernelSize(parSumsIntensityImage[0], params.kernelSize,Integral[0]); imageMeanKernelSize(parSumsIntensityImage[1], params.kernelSize, Integral[1]); modifiedCensusTransform(left,right,params.kernelSize,censusImage[0],censusImage[1],CV_MEAN_VARIATION,0,Integral[0], Integral[1]); } else if(params.kernelType == CV_STAR_KERNEL) { starCensusTransform(left,right,params.kernelSize,censusImage[0],censusImage[1]); } hammingDistanceBlockMatching(censusImage[0], censusImage[1], hammingDistance); costGathering(hammingDistance, partialSumsLR); blockAgregation(partialSumsLR, params.agregationWindowSize, agregatedHammingLRCost); dispartyMapFormation(agregatedHammingLRCost, disp0, 3); Median1x9Filter<uint8_t>(disp0, aux); Median9x1Filter<uint8_t>(aux,disp0); if(params.regionRemoval == CV_SPECKLE_REMOVAL_AVG_ALGORITHM) { smallRegionRemoval<uint8_t>(disp0,params.speckleWindowSize,disp0); } else if(params.regionRemoval == CV_SPECKLE_REMOVAL_ALGORITHM) { if (params.speckleRange >= 0 && params.speckleWindowSize > 0) filterSpeckles(disp0, FILTERED, params.speckleWindowSize, params.speckleRange, slidingSumBuf); } }