Hi everyone,
Electric Emy includes a nodelet to perform crop and binning of images.
In our research group we find this very useful and were wondering why it
was missing.
So very good news to see it there. Congratulations! We hope to see this
node documented in the image_proc wiki soon.
If the input image encoding is bayer, this nodelet performs crop/binning
and debayering.
We would like to discuss with the ROS community if it would be more
convenient perform only the crop/binning, and do NOT perform the
debayering. That debayering process, with possibly other image_proc
tasks, could be performed by another image_proc node in the subsampled
image topic namespace. From our point of view, it will have two main
advantages:
1. It will allow more complex debayering methods to be aplied to the
scaled images by image_proc, with zero mantainment cost of the
crop/binning nodelet.
2. It will allow subsampling bayer images to reduce its size and send
them through the network without losing the color information (if
sended in mono) nor triplicating the required bandwith (if sended
in color)
The subsampling is quite easy to implement. It just changes depending on
the encoding. Subsampling of mono and color encoded images is quite
intuitive. In Bayer encodings, the idea is to consider each set of 2x2
pixels as a color cell, and these color cells are the subsampling units.
The resulting image has the same encoding than the original one.
What do you think of it?
The attached file crop_decimate.cpp includes the needed modifications to
implement the explained functionality, also solving some issues and
todo's in the current implementation:
1. It should handle correctly 16 bit encodings (and any other bit
depth), since in that implementation different bit depths do no
need specific methods. I say it should because I could not test
it. I do not have any camera or bagfile with 16 bit images.
2. Offsets are checked to be smaller than image size, and an error is
reported if it is not the case. In contrast, if the demanded ROI
is bigger than the image (with or height too large for the given
the offset and image size), it is SILENTLY adjusted. Should this
adjustment be warned?
3. On Bayer images, not only the offsets are checked to be even, but
also the resulting image size. This is needed to end up with a
well formed Bayer image with full color cells.
The file crop_decimate2.cpp is a slightly different implementation of
the same idea, but performing the subsampling in just one function for
all the encodings (given the right parameters). The function takes as
targuments the cell size (2x2 for Bayer, 1x1 for other encodings, but
any other size could be used).
Probably the code is a little more elegant, but it may have a little
overhead because of the inner loop (probably insignificant). A part from
that, and to present an alternative approach, it also solves the
inconsistent image - ROI offsets/size in a different way. The output
image size is always the same than the given ROI once subsampled, and
the region that falls out of the image is zeroed. This is similar to
what happens to the rectified image when a zero alpha value is set in
the calibration.
It should be said that this subsampling is the crudest one, and it can
introduce artifacts in the resulting image. Another completely different
approach will be to use OpenCV resizing functions to do the job. In that
case, as in the debayering process, a set of methods could be supported
with an extra dynamic_reconfigure parameter. I think that last OpenCV
versions only understand images as bgr8 encoded cv::Mat's (even mono
images), so the approach will be:
1. subscribe to the color image
2. subsample it to a color scaled image
3. from this color scaled image build the mono scaled image if necessary
And it won't be possible to scale a bayer image, debayer it first will
be mandatory. Is there any OpenCV expert that can confirm or refutate
these statements? Any information about the resizing functions will be
appreciated, since the documentation is quite poor.
That's all, folks. Sorry for the long mail. If the attached code could
be useful, please feel free to merge it into the ROS image_proc branch,
with any convenient modifications.
Other comments, advice or suggestions will also be appreciated.
Thanks!
P.D: BTW, the connectCb in the code has no argument, but in the
NodeHandle API it has a SingleSubscriberPublisher as unique argument.
Does anybody know what is this argument? I asked the same question to
answer.ros.org, so probably the best place to answer would be there:
http://answers.ros.org/question/2288/argument-for-subscriberstatuscallback-in-advertise
--
Joan Pau Beltran
Grup de Sistemes, Robòtica i Visió - DMI
Universitat de les Illes Balears
Ctra. Valldemossa, km 7.5 07122 Palma
Campus Universitari, edifici Anselm Turmeda
Telf (+34) 971 17 28 13
Fax (+34) 971 17 30 03
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <dynamic_reconfigure/server.h>
#include <image_proc/CropDecimateConfig.h>
namespace image_proc {
class CropDecimateNodelet : public nodelet::Nodelet
{
// ROS communication
boost::shared_ptr<image_transport::ImageTransport> it_in_, it_out_;
image_transport::CameraSubscriber sub_;
image_transport::CameraPublisher pub_;
int queue_size_;
// Dynamic reconfigure
typedef image_proc::CropDecimateConfig Config;
typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
boost::shared_ptr<ReconfigureServer> reconfigure_server_;
Config config_;
virtual void onInit();
void connectCb();
void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg);
void configCb(Config &config, uint32_t level);
void subsample(const uint8_t* const in, const int32_t& in_step,
const int& in_width, const int& in_height,
const int& bytes_per_pixel,
const int& cell_width, const int& cell_height,
const int& roi_offset_x, const int& roi_offset_y,
const int& roi_width, const int& roi_height,
const int& binning_x, const int& binning_y,
uint8_t* const out, const int& out_step,
const int& out_width, const int& out_height);
};
void CropDecimateNodelet::onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
ros::NodeHandle nh_in (nh, "camera");
ros::NodeHandle nh_out(nh, "camera_out");
it_in_ .reset(new image_transport::ImageTransport(nh_in));
it_out_.reset(new image_transport::ImageTransport(nh_out));
// Read parameters
private_nh.param("queue_size", queue_size_, 5);
// Monitor whether anyone is subscribed to the output
image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropDecimateNodelet::connectCb, this);
ros::SubscriberStatusCallback connect_cb_info = boost::bind(&CropDecimateNodelet::connectCb, this);
pub_ = it_out_->advertiseCamera("image_raw", 1, connect_cb, connect_cb, connect_cb_info, connect_cb_info);
// Set up dynamic reconfigure
reconfigure_server_.reset(new ReconfigureServer(private_nh));
ReconfigureServer::CallbackType f = boost::bind(&CropDecimateNodelet::configCb, this, _1, _2);
reconfigure_server_->setCallback(f);
}
// Handles (un)subscribing when clients (un)subscribe
void CropDecimateNodelet::connectCb()
{
if (pub_.getNumSubscribers() == 0)
sub_.shutdown();
else if (!sub_)
sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this);
}
void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg)
{
const unsigned int config_width = (config_.width == 0) ? image_msg->width : config_.width;
const unsigned int config_height = (config_.height == 0) ? image_msg->height : config_.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 &&
config_width == image_msg->width &&
config_height == image_msg->height )
{
pub_.publish(image_msg, info_msg);
return;
}
const unsigned int num_channels = sensor_msgs::image_encodings::numChannels(image_msg->encoding);
const unsigned int bit_depth = sensor_msgs::image_encodings::bitDepth(image_msg->encoding);
const unsigned int bytes_per_pixel = num_channels*(bit_depth/8);
const bool is_bayer = sensor_msgs::image_encodings::isBayer(image_msg->encoding);
const unsigned int cell_size = (is_bayer) ? 2 : 1;
const unsigned int width = ( (config_width / cell_size) / config_.decimation_x ) * cell_size;
const unsigned int height = ( (config_height / cell_size) / config_.decimation_y ) * cell_size;
const unsigned int roi_width = width * config_.decimation_x;
const unsigned int roi_height = height * config_.decimation_y;
const unsigned int offset_x = (config_.x_offset / cell_size) * cell_size;
const unsigned int offset_y = (config_.y_offset / cell_size) * cell_size;
// Create new image message
sensor_msgs::ImagePtr out_image = boost::make_shared<sensor_msgs::Image>();
out_image->header = image_msg->header;
out_image->encoding = image_msg->encoding;
out_image->width = width;
out_image->height = height;
out_image->step = width*bytes_per_pixel;
out_image->data.resize(out_image->height * out_image->step, 0);
subsample(image_msg->data.data(), image_msg->step, image_msg->width, image_msg->height,
bytes_per_pixel, cell_size, cell_size,
offset_x, offset_y, roi_width, roi_height,
config_.decimation_x, config_.decimation_y,
out_image->data.data(), out_image->step, out_image->width, out_image->height);
// Create updated CameraInfo message
sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
const unsigned int binning_x = std::max<unsigned int>(info_msg->binning_x, 1);
const unsigned int binning_y = std::max<unsigned 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 += offset_x * binning_x;
out_info->roi.y_offset += offset_y * binning_y;
out_info->roi.height = roi_height * binning_y;
out_info->roi.width = roi_width * binning_x;
pub_.publish(out_image, out_info);
}
void CropDecimateNodelet::configCb(Config &config, uint32_t level)
{
config_ = config;
}
void CropDecimateNodelet::subsample(const uint8_t* const in, const int& in_step,
const int& in_width, const int& in_height,
const int& bytes_per_pixel,
const int& cell_width, const int& cell_height,
const int& roi_offset_x, const int& roi_offset_y,
const int& roi_width, const int& roi_height,
const int& binning_x, const int& binning_y,
uint8_t* const out, const int& out_step,
const int& out_width, const int& out_height)
{
const int cell_step = bytes_per_pixel * cell_width;
const int in_skip_y = binning_y * in_step * cell_height;
const int in_skip_x = binning_x * cell_step;
const int max_width = std::min(roi_width, in_width - roi_offset_x);
const int max_height = std::min(roi_height, in_height - roi_offset_y);
const int binned_width = ( (max_width / cell_width ) / binning_x ) * cell_width;
const int binned_height = ( (max_height / cell_height) / binning_y ) * cell_height;
const int width = std::min(binned_width , out_width );
const int height = std::min(binned_height, out_height);
const uint8_t* in_row = in + roi_offset_y*in_step + roi_offset_x*bytes_per_pixel;
uint8_t* out_row = out;
for (int i = 0; i < height; i+=cell_height, in_row += in_skip_y)
{
const uint8_t* in_cell_row = in_row;
for (int k = 0; k < cell_height; k++, in_cell_row+=in_step, out_row+=out_step)
{
const uint8_t* in_buffer = in_cell_row;
uint8_t* out_buffer = out_row;
for (int j = 0; j < width; j+=cell_width, in_buffer += in_skip_x, out_buffer += cell_step)
{
memcpy(out_buffer, in_buffer, cell_step);
}
}
}
}
} // namespace image_proc
// Register nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS(image_proc_custom, crop_decimate, image_proc::CropDecimateNodelet, nodelet::Nodelet)
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <dynamic_reconfigure/server.h>
#include <image_proc/CropDecimateConfig.h>
namespace image_proc {
class CropDecimateNodelet : public nodelet::Nodelet
{
// ROS communication
boost::shared_ptr<image_transport::ImageTransport> it_in_, it_out_;
image_transport::CameraSubscriber sub_;
image_transport::CameraPublisher pub_;
int queue_size_;
// Dynamic reconfigure
typedef image_proc::CropDecimateConfig Config;
typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
boost::shared_ptr<ReconfigureServer> reconfigure_server_;
Config config_;
virtual void onInit();
void connectCb();
void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg);
void configCb(Config &config, uint32_t level);
};
void CropDecimateNodelet::onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
ros::NodeHandle nh_in (nh, "camera");
ros::NodeHandle nh_out(nh, "camera_out");
it_in_ .reset(new image_transport::ImageTransport(nh_in));
it_out_.reset(new image_transport::ImageTransport(nh_out));
// Read parameters
private_nh.param("queue_size", queue_size_, 5);
// Monitor whether anyone is subscribed to the output
image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropDecimateNodelet::connectCb, this);
ros::SubscriberStatusCallback connect_cb_info = boost::bind(&CropDecimateNodelet::connectCb, this);
pub_ = it_out_->advertiseCamera("image_raw", 1, connect_cb, connect_cb, connect_cb_info, connect_cb_info);
// Set up dynamic reconfigure
reconfigure_server_.reset(new ReconfigureServer(private_nh));
ReconfigureServer::CallbackType f = boost::bind(&CropDecimateNodelet::configCb, this, _1, _2);
reconfigure_server_->setCallback(f);
}
// Handles (un)subscribing when clients (un)subscribe
void CropDecimateNodelet::connectCb()
{
if (pub_.getNumSubscribers() == 0)
sub_.shutdown();
else if (!sub_)
sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this);
}
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
/// @todo Check if offsets are larger than image size.
const int max_width = image_msg->width - config_.x_offset;
const int max_height = image_msg->height - config_.y_offset;
if (max_width <= 0 || max_height <= 0)
{
NODELET_ERROR_STREAM("ROI offset " << config_.x_offset << "," << config_.y_offset
<< " out of image size " << image_msg->width << "x" << image_msg->height << "." );
return;
}
const int roi_width = (config_.width == 0 || config_.width > max_width) ? max_width : config_.width;
const int roi_height = (config_.height == 0 || config_.height > max_height) ? max_height : config_.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 &&
roi_width == (int) image_msg->width &&
roi_height == (int) image_msg->height)
{
pub_.publish(image_msg, info_msg);
return;
}
const unsigned int num_channels = sensor_msgs::image_encodings::numChannels(image_msg->encoding);
const unsigned int bit_depth = sensor_msgs::image_encodings::bitDepth(image_msg->encoding);
const unsigned int bytes_per_pixel = num_channels*(bit_depth/8);
const bool is_bayer = sensor_msgs::image_encodings::isBayer(image_msg->encoding);
// Create new image message
sensor_msgs::ImagePtr out_image = boost::make_shared<sensor_msgs::Image>();
out_image->header = image_msg->header;
out_image->encoding = image_msg->encoding;
if (is_bayer)
{
// Subsample Bayer squares of 2x2 pixels instead if single pixels
// Offsets and binned ROI dimensions should be even to fit Bayer squares
out_image->width = ( (roi_width / 2) / config_.decimation_x ) * 2;
out_image->height = ( (roi_height / 2) / config_.decimation_y ) * 2;
config_.x_offset = (config_.x_offset / 2) * 2;
config_.y_offset = (config_.y_offset / 2) * 2;
}
else
{
out_image->width = roi_width / config_.decimation_x;
out_image->height = roi_height / config_.decimation_y;
}
out_image->step = out_image->width*bytes_per_pixel;
out_image->data.resize(out_image->height * out_image->step);
if (config_.decimation_x == 1 && config_.decimation_y == 1)
{
// Copy ROI without subsampling
const uint8_t* input_buffer = &image_msg->data[config_.y_offset*image_msg->step + config_.x_offset*bytes_per_pixel];
uint8_t* output_buffer = &out_image->data[0];
for (unsigned int y = 0; y < out_image->height; y++, input_buffer += image_msg->step, output_buffer += out_image->step)
{
memcpy(output_buffer, input_buffer, out_image->step);
}
}
else if (is_bayer)
{
// Subsample Bayer squares
const unsigned int input_step = config_.decimation_y * image_msg->step * 2;
const unsigned int input_skip = config_.decimation_x * bytes_per_pixel * 2;
const unsigned int chunk_size = bytes_per_pixel * 2;
const uint8_t* input_row = &image_msg->data[config_.y_offset*image_msg->step + config_.x_offset*bytes_per_pixel];
uint8_t* output_buffer = &out_image->data[0];
for (unsigned int y = 0; y < out_image->height; y += 2, input_row += input_step)
{
const uint8_t* input_buffer = input_row;
for (unsigned int x = 0; x < out_image->width; x += 2, input_buffer += input_skip, output_buffer += chunk_size)
{
memcpy(output_buffer, input_buffer, chunk_size);
memcpy(output_buffer+out_image->step, input_buffer+image_msg->step, chunk_size);
}
output_buffer += out_image->step;
}
}
else
{
// Subsample single pixels
const unsigned int input_step = config_.decimation_y * image_msg->step;
const unsigned int input_skip = config_.decimation_x * bytes_per_pixel;
const uint8_t* input_row = &image_msg->data[config_.y_offset*image_msg->step + config_.x_offset*bytes_per_pixel];
uint8_t* output_buffer = &out_image->data[0];
for (unsigned int y = 0; y < out_image->height; y++, input_row += input_step)
{
const uint8_t* input_buffer = input_row;
for (unsigned int x = 0; x < out_image->width; x++, input_buffer += input_skip, output_buffer += bytes_per_pixel)
{
memcpy(output_buffer, input_buffer, bytes_per_pixel);
}
}
}
// Create updated CameraInfo message
sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
const int binning_x = std::max((int)info_msg->binning_x, 1);
const 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 = roi_height * binning_y;
out_info->roi.width = roi_width * binning_x;
// Publish new image and new camera info
pub_.publish(out_image, out_info);
}
void CropDecimateNodelet::configCb(Config &config, uint32_t level)
{
config_ = config;
}
} // namespace image_proc
// Register nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS(image_proc_custom, crop_decimate, image_proc::CropDecimateNodelet, nodelet::Nodelet)
(text/plain)