ImageProcessingNodelet.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <ros/package.h>
#include <nodelet/nodelet.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <dynamic_reconfigure/server.h>
#include <sensor_msgs/image_encodings.h>
#include <pluginlib/class_list_macros.h>
#include <image_transport/image_transport.h>
#include <nifti_image_proc/ProcessingConfig.h>

//#define USE_MT_NODE_HANDLE
#define MIN_INTENSITY 0
#define MAX_INTENSITY 255
#define LOOKUP_SIZE 256

namespace nifti_image_proc {

class ImageProcessingNodelet: public nodelet::Nodelet {
private:
  int min_intensity;
  int max_intensity;
  double gamma;
  double percent_saturated;
  boost::shared_ptr<image_transport::ImageTransport> it;
  image_transport::Publisher cameraPub;
  image_transport::Subscriber cameraSub;
  nifti_image_proc::ProcessingConfig processingConfig;
  boost::shared_ptr<dynamic_reconfigure::Server<nifti_image_proc::ProcessingConfig> > server;
  dynamic_reconfigure::Server<nifti_image_proc::ProcessingConfig>::CallbackType f;
public:
  ImageProcessingNodelet();
  void onInit();
  void processingCallback(nifti_image_proc::ProcessingConfig &config, uint32_t level);
  void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
  cv::Mat cumulativeHist(cv::Mat hist);
};

ImageProcessingNodelet::ImageProcessingNodelet():
    min_intensity(MIN_INTENSITY),
    max_intensity(MAX_INTENSITY),
    gamma(1.0),
    percent_saturated(5.0) {
}

void ImageProcessingNodelet::onInit() {
  NODELET_INFO("Initializing ImageProcessingNodelet...");
#ifdef USE_MT_NODE_HANDLE
  ros::NodeHandle &nh = getMTNodeHandle();
  ros::NodeHandle &pnh = getMTPrivateNodeHandle();
#else
  ros::NodeHandle &nh = getNodeHandle();
  ros::NodeHandle &pnh = getPrivateNodeHandle();
#endif

  pnh.param("min_intensity", min_intensity, min_intensity);
  pnh.param("max_intensity", max_intensity, max_intensity);
  pnh.param("gamma", gamma, gamma);
  pnh.param("percent_saturated", percent_saturated, percent_saturated);

  f = boost::bind(&ImageProcessingNodelet::processingCallback, this, _1, _2);
  server.reset(new dynamic_reconfigure::Server<nifti_image_proc::ProcessingConfig>(pnh));
  server->setCallback(f);

  it.reset(new image_transport::ImageTransport(nh));
  cameraSub = it->subscribe("image", 5, &ImageProcessingNodelet::cameraCallback, this);
  cameraPub = it->advertise("processed_image", 5);
  NODELET_INFO("ImageProcessingNodelet initialized.");
}

void ImageProcessingNodelet::processingCallback(nifti_image_proc::ProcessingConfig &config, uint32_t level) {
  processingConfig = config;
}

void ImageProcessingNodelet::cameraCallback(const sensor_msgs::ImageConstPtr& msg) {
  // Avoid processing the image if there is no subscribers.
  if (cameraPub.getNumSubscribers() <= 0) {
    return;
  }

  cv_bridge::CvImagePtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  gamma = processingConfig.gamma;
  percent_saturated = processingConfig.percent_saturated;
  double width = cv_ptr->image.cols, height = cv_ptr->image.rows;

  if (percent_saturated != 0.0) {
    cv::Mat grey;
    cv::cvtColor(cv_ptr->image, grey, CV_BGR2GRAY);
    cv::Mat hist;

    int histSize = 256;
    float range[] = { 0, 256 };
    const float* histRange = { range };
    calcHist(&grey, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, true, false);

    hist /= width * height;
    hist *= 100;
    hist = cumulativeHist(hist);

    int l_min = 255, l_max = 0, sum_min = 0.0, sum_max = 0.0;
    for (; l_min >= 0; l_min--) {
      if (hist.at<float>(l_min) <= percent_saturated / 2.0) {
        break;
      }
    }
    for (; l_max < 256; l_max++) {
      if (100 - hist.at<float>(l_max) <= percent_saturated / 2.0) {
        break;
      }
    }
    min_intensity = l_min;
    max_intensity = l_max;
  } else {
    min_intensity = processingConfig.min_intensity;
    max_intensity = processingConfig.max_intensity;
  }

  cv::Mat lookup(1, LOOKUP_SIZE, CV_8U);
  for (int i = 0; i < LOOKUP_SIZE; i++) {
    if (i < min_intensity) {
      lookup.at<uchar>(i) = MIN_INTENSITY;
    } else if (i > max_intensity) {
      lookup.at<uchar>(i) = MAX_INTENSITY;
    } else {
      lookup.at<uchar>(i) = static_cast<uchar>(255.0
          * pow(((double) (i - min_intensity) / (double) (max_intensity - min_intensity)), 1 / gamma) + 0.5);
    }
  }

  cv::Mat result;
  cv::LUT(cv_ptr->image, lookup, result);

  cv_bridge::CvImagePtr image_ptr(new cv_bridge::CvImage);
  image_ptr->header = msg->header;
//  image_ptr->header.stamp = ros::Time::now();
//  image_ptr->header.frame_id = "processed_image";
  image_ptr->encoding = sensor_msgs::image_encodings::BGR8;
  image_ptr->image = result;

  cameraPub.publish(image_ptr->toImageMsg());
}

cv::Mat ImageProcessingNodelet::cumulativeHist(cv::Mat hist) {
  cv::Mat cumulative(hist.size(), hist.type());
  float sum = 0.0;
  for (int i = 0; i < 256; i++) {
    sum += hist.at<float>(i);
    cumulative.at<float>(i) = sum;
  }
  return cumulative;
}

}

PLUGINLIB_DECLARE_CLASS(nifti_image_proc, image_processing_nodelet,
    nifti_image_proc::ImageProcessingNodelet, nodelet::Nodelet)