void ConvertMetricNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg)
{
if (raw_msg->encoding != enc::TYPE_16UC1)
{
NODELET_ERROR_THROTTLE(2, "Expected data of type [%s], got [%s]", enc::TYPE_16UC1.c_str(),
raw_msg->encoding.c_str());
return;
}
// Allocate new Image message
sensor_msgs::ImagePtr depth_msg( new sensor_msgs::Image );
depth_msg->header = raw_msg->header;
depth_msg->encoding = enc::TYPE_32FC1;
depth_msg->height = raw_msg->height;
depth_msg->width = raw_msg->width;
depth_msg->step = raw_msg->width * sizeof (float);
depth_msg->data.resize( depth_msg->height * depth_msg->step);
float bad_point = std::numeric_limits<float>::quiet_NaN ();
// Fill in the depth image data, converting mm to m
const uint16_t* raw_data = reinterpret_cast<const uint16_t*>(&raw_msg->data[0]);
float* depth_data = reinterpret_cast<float*>(&depth_msg->data[0]);
for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index)
{
uint16_t raw = raw_data[index];
depth_data[index] = (raw == 0) ? bad_point : (float)raw * 0.001f;
}
pub_depth_.publish(depth_msg);
}
void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
image_mutex_.lock();
// May want to view raw bayer data, which CvBridge doesn't know about
if (msg->encoding.find("bayer") != std::string::npos)
{
last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1,
const_cast<uint8_t*>(&msg->data[0]), msg->step);
}
// We want to scale floating point images so that they display nicely
else if(msg->encoding.find("F") != std::string::npos)
{
cv::Mat float_image_bridge = cv_bridge::toCvShare(msg, msg->encoding)->image;
cv::Mat_<float> float_image = float_image_bridge;
float max_val = 0;
for(int i = 0; i < float_image.rows; ++i)
{
for(int j = 0; j < float_image.cols; ++j)
{
max_val = std::max(max_val, float_image(i, j));
}
}
if(max_val > 0)
{
float_image /= max_val;
}
last_image_ = float_image;
}
else
{
// Convert to OpenCV native BGR color
try {
last_image_ = cv_bridge::toCvShare(msg, "bgr8")->image;
}
catch (cv_bridge::Exception& e) {
NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image to bgr8: '%s'",
msg->encoding.c_str(), e.what());
}
}
// last_image_ may point to data owned by last_msg_, so we hang onto it for
// the sake of mouseCb.
last_msg_ = msg;
// Must release the mutex before calling cv::imshow, or can deadlock against
// OpenCV's window mutex.
image_mutex_.unlock();
if (!last_image_.empty())
cv::imshow(window_name_, last_image_);
}
void ClusterPointIndicesToPointIndices::convert(
const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_indices_msg)
{
vital_checker_->poke();
PCLIndicesMsg indices_msg;
indices_msg.header = cluster_indices_msg->header;
int cluster_size = cluster_indices_msg->cluster_indices.size();
if (index_ < 0) {
for(int i = 0;i < cluster_size; ++i) {
indices_msg.indices.insert(indices_msg.indices.end(),
cluster_indices_msg->cluster_indices[i].indices.begin(),
cluster_indices_msg->cluster_indices[i].indices.end());
}
} else if (index_ < cluster_size) {
indices_msg.indices = cluster_indices_msg->cluster_indices[index_].indices;
} else {
NODELET_ERROR_THROTTLE(10, "Invalid ~index %d is specified for cluster size %d.", index_, cluster_size);
}
pub_.publish(indices_msg);
}
void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
image_mutex_.lock();
// We want to scale floating point images so that they display nicely
bool do_dynamic_scaling = (msg->encoding.find("F") != std::string::npos);
// Convert to OpenCV native BGR color
try {
last_image_ = cvtColorForDisplay(cv_bridge::toCvShare(msg), "", do_dynamic_scaling)->image;
}
catch (cv_bridge::Exception& e) {
NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
msg->encoding.c_str(), e.what());
}
// Must release the mutex before calling cv::imshow, or can deadlock against
// OpenCV's window mutex.
image_mutex_.unlock();
if (!last_image_.empty())
cv::imshow(window_name_, last_image_);
}
void GeometricConsistencyGrouping::recognize(
const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
NODELET_ERROR_THROTTLE(1.0, "Not yet reference is available");
return;
}
pcl::PointCloud<pcl::SHOT352>::Ptr
scene_feature (new pcl::PointCloud<pcl::SHOT352>);
pcl::PointCloud<pcl::PointNormal>::Ptr
scene_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*scene_cloud_msg, *scene_cloud);
pcl::fromROSMsg(*scene_feature_msg, *scene_feature);
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<pcl::SHOT352> match_search;
match_search.setInputCloud (reference_feature_);
for (size_t i = 0; i < scene_feature->size(); ++i)
{
std::vector<int> neigh_indices(1);
std::vector<float> neigh_sqr_dists(1);
if (!pcl_isfinite (scene_feature->at(i).descriptor[0])) { //skipping NaNs
continue;
}
int found_neighs
= match_search.nearestKSearch(scene_feature->at(i), 1,
neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than 0.25
// (SHOT descriptor distances are between 0 and 1 by design)
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) {
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
pcl::GeometricConsistencyGrouping<pcl::PointNormal, pcl::PointNormal> gc_clusterer;
gc_clusterer.setGCSize(gc_size_);
gc_clusterer.setGCThreshold(gc_thresh_);
gc_clusterer.setInputCloud(reference_cloud_);
gc_clusterer.setSceneCloud(scene_cloud);
gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
std::vector<pcl::Correspondences> clustered_corrs;
std::vector<Eigen::Matrix4f,
Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
gc_clusterer.recognize(rototranslations, clustered_corrs);
if (rototranslations.size() > 0) {
NODELET_INFO("detected %lu objects", rototranslations.size());
Eigen::Matrix4f result_mat = rototranslations[0];
Eigen::Affine3f affine(result_mat);
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(affine, ros_pose.pose);
ros_pose.header = scene_cloud_msg->header;
pub_output_.publish(ros_pose);
}
else {
NODELET_WARN("Failed to find object");
}
}
void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg)
{
/// @todo Check image dimensions match info_msg
/// @todo Publish tweaks to config_ so they appear in reconfigure_gui
Config config;
{
boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
config = config_;
}
/// @todo Could support odd offsets for Bayer images, but it's a tad complicated
config.x_offset = (config.x_offset / 2) * 2;
config.y_offset = (config.y_offset / 2) * 2;
int max_width = image_msg->width - config.x_offset;
int max_height = image_msg->height - config.y_offset;
int width = config.width;
int height = config.height;
if (width == 0 || width > max_width)
width = max_width;
if (height == 0 || height > max_height)
height = max_height;
// On no-op, just pass the messages along
if (config.decimation_x == 1 &&
config.decimation_y == 1 &&
config.x_offset == 0 &&
config.y_offset == 0 &&
width == (int)image_msg->width &&
height == (int)image_msg->height)
{
pub_.publish(image_msg, info_msg);
return;
}
/// @todo Support 16-bit encodings
if (sensor_msgs::image_encodings::bitDepth(image_msg->encoding) != 8)
{
NODELET_ERROR_THROTTLE(2, "Only 8-bit encodings are currently supported");
return;
}
// Create updated CameraInfo message
sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
int binning_x = std::max((int)info_msg->binning_x, 1);
int binning_y = std::max((int)info_msg->binning_y, 1);
out_info->binning_x = binning_x * config.decimation_x;
out_info->binning_y = binning_y * config.decimation_y;
out_info->roi.x_offset += config.x_offset * binning_x;
out_info->roi.y_offset += config.y_offset * binning_y;
out_info->roi.height = height * binning_y;
out_info->roi.width = width * binning_x;
// If no ROI specified, leave do_rectify as-is. If ROI specified, set do_rectify = true.
if (width != (int)image_msg->width || height != (int)image_msg->height)
out_info->roi.do_rectify = true;
// Create new image message
sensor_msgs::ImagePtr out_image = boost::make_shared<sensor_msgs::Image>();
out_image->header = image_msg->header;
out_image->height = height / config.decimation_y;
out_image->width = width / config.decimation_x;
// Don't know encoding, step, or data size yet
if (config.decimation_x == 1 && config.decimation_y == 1)
{
// Crop only, preserving original encoding
int num_channels = sensor_msgs::image_encodings::numChannels(image_msg->encoding);
out_image->encoding = image_msg->encoding;
out_image->step = out_image->width * num_channels;
out_image->data.resize(out_image->height * out_image->step);
const uint8_t* input_buffer = &image_msg->data[config.y_offset*image_msg->step + config.x_offset*num_channels];
uint8_t* output_buffer = &out_image->data[0];
for (int y = 0; y < (int)out_image->height; ++y)
{
memcpy(output_buffer, input_buffer, out_image->step);
input_buffer += image_msg->step;
output_buffer += out_image->step;
}
}
else if (sensor_msgs::image_encodings::isMono(image_msg->encoding))
{
// Output is also monochrome
out_image->encoding = sensor_msgs::image_encodings::MONO8;
out_image->step = out_image->width;
out_image->data.resize(out_image->height * out_image->step);
// Hit only the pixel groups we need
int input_step = config.decimation_y * image_msg->step;
int input_skip = config.decimation_x;
const uint8_t* input_row = &image_msg->data[config.y_offset*image_msg->step + config.x_offset];
uint8_t* output_buffer = &out_image->data[0];
// Downsample
for (int y = 0; y < (int)out_image->height; ++y, input_row += input_step)
{
const uint8_t* input_buffer = input_row;
for (int x = 0; x < (int)out_image->width; ++x, input_buffer += input_skip, ++output_buffer)
*output_buffer = *input_buffer;
}
}
else
{
// Output is color
out_image->encoding = sensor_msgs::image_encodings::BGR8;
out_image->step = out_image->width * 3;
out_image->data.resize(out_image->height * out_image->step);
if (sensor_msgs::image_encodings::isBayer(image_msg->encoding))
{
if (config.decimation_x % 2 != 0 || config.decimation_y % 2 != 0)
{
NODELET_ERROR_THROTTLE(2, "Odd decimation not supported for Bayer images");
return;
}
// Compute offsets to color elements
int R, G1, G2, B;
if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8)
{
R = 0;
G1 = 1;
G2 = image_msg->step;
B = image_msg->step + 1;
}
else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8)
{
B = 0;
G1 = 1;
G2 = image_msg->step;
R = image_msg->step + 1;
}
else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8)
{
G1 = 0;
B = 1;
R = image_msg->step;
G2 = image_msg->step + 1;
}
else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8)
{
G1 = 0;
R = 1;
B = image_msg->step;
G2 = image_msg->step + 1;
}
else
{
NODELET_ERROR_THROTTLE(2, "Unrecognized Bayer encoding '%s'", image_msg->encoding.c_str());
return;
}
// Hit only the pixel groups we need
int input_step = config.decimation_y * image_msg->step;
int input_skip = config.decimation_x;
const uint8_t* input_row = &image_msg->data[config.y_offset*image_msg->step + config.x_offset];
uint8_t* output_buffer = &out_image->data[0];
// Downsample and debayer at once
for (int y = 0; y < (int)out_image->height; ++y, input_row += input_step)
{
const uint8_t* input_buffer = input_row;
for (int x = 0; x < (int)out_image->width; ++x, input_buffer += input_skip, output_buffer += 3)
{
output_buffer[0] = input_buffer[B];
output_buffer[1] = (input_buffer[G1] + input_buffer[G2]) / 2;
output_buffer[2] = input_buffer[R];
}
}
}
else
{
// Support RGB-type encodings
int R, G, B, channels;
if (image_msg->encoding == sensor_msgs::image_encodings::BGR8)
{
B = 0;
G = 1;
R = 2;
channels = 3;
}
else if (image_msg->encoding == sensor_msgs::image_encodings::RGB8)
{
R = 0;
G = 1;
B = 2;
channels = 3;
}
else if (image_msg->encoding == sensor_msgs::image_encodings::BGRA8)
{
B = 0;
G = 1;
R = 2;
channels = 4;
}
else if (image_msg->encoding == sensor_msgs::image_encodings::RGBA8)
{
R = 0;
G = 1;
B = 2;
channels = 4;
}
else
{
NODELET_ERROR_THROTTLE(2, "Unsupported encoding '%s'", image_msg->encoding.c_str());
return;
}
// Hit only the pixel groups we need
int input_step = config.decimation_y * image_msg->step;
int input_skip = config.decimation_x * channels;
const uint8_t* input_row = &image_msg->data[config.y_offset*image_msg->step + config.x_offset*channels];
uint8_t* output_buffer = &out_image->data[0];
// Downsample
for (int y = 0; y < (int)out_image->height; ++y, input_row += input_step)
{
const uint8_t* input_buffer = input_row;
for (int x = 0; x < (int)out_image->width; ++x, input_buffer += input_skip, output_buffer += 3)
{
output_buffer[0] = input_buffer[B];
output_buffer[1] = input_buffer[G];
output_buffer[2] = input_buffer[R];
}
}
}
}
pub_.publish(out_image, out_info);
}
