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);
}
}
