disp_masker.cpp

Go to the documentation of this file.

/*
  velo2cam_calibration - Automatic calibration algorithm for extrinsic
  parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge
  Beltran, Carlos Guindel

  This file is part of velo2cam_calibration.

  velo2cam_calibration is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 2 of the License, or
  (at your option) any later version.

  velo2cam_calibration is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with velo2cam_calibration.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
  disp_masker: Mask the disparity map according to the edges image
*/

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/exact_time.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <stereo_msgs/DisparityImage.h>

#include <opencv2/opencv.hpp>

class Masker {
 public:
  ros::NodeHandle nh_;

  message_filters::Subscriber<stereo_msgs::DisparityImage> image_sub_;
  message_filters::Subscriber<sensor_msgs::Image> mask_sub_;
  ros::Publisher masked_pub_;

  bool isfreeobs_;
  int edges_threshold_;

  typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage,
                                                    sensor_msgs::Image>
      ExSync;
  message_filters::Synchronizer<ExSync> sync;

  Masker()
      : nh_("~"),
        image_sub_(nh_, "image", 1),
        mask_sub_(nh_, "mask", 1),
        sync(ExSync(100), image_sub_, mask_sub_) {
    masked_pub_ = nh_.advertise<stereo_msgs::DisparityImage>("output", 1);

    nh_.param("isFreeobs", isfreeobs_, false);
    nh_.param("edges_threshold", edges_threshold_, 16);

    sync.registerCallback(boost::bind(&Masker::callback, this, _1, _2));
  }

  void callback(const stereo_msgs::DisparityImageConstPtr &disp,
                const sensor_msgs::ImageConstPtr &ma) {
    cv::Mat mask, binary_mask, output;
    cv_bridge::CvImageConstPtr cv_im;

    try {
      cv_im = cv_bridge::toCvShare(disp->image, disp,
                                   sensor_msgs::image_encodings::TYPE_32FC1);
      mask = cv_bridge::toCvShare(ma, "mono8")->image;
    } catch (cv_bridge::Exception &e) {
      ROS_ERROR("CvBridge failed");
    }

    static cv::Mat disparity32(cv_im->image.rows, cv_im->image.cols, CV_32FC1);
    disparity32 = cv_im->image;

    if (isfreeobs_) {
      const static int OBSTACLE_LABEL = 32;
      cv::Mat obs_pattern(mask.rows, mask.cols, CV_8UC1,
                          cv::Scalar(OBSTACLE_LABEL));
      cv::bitwise_and(mask, obs_pattern, binary_mask);
      binary_mask = binary_mask * (255.0 / OBSTACLE_LABEL);
    } else {
      cv::threshold(mask, binary_mask, edges_threshold_, 255, 0);
    }

    // Copy input disparity to another DisparityImage variable
    stereo_msgs::DisparityImagePtr copy_disp =
        boost::make_shared<stereo_msgs::DisparityImage>();
    copy_disp->valid_window.x_offset = disp->valid_window.x_offset;
    copy_disp->valid_window.y_offset = disp->valid_window.y_offset;
    copy_disp->valid_window.width = disp->valid_window.width;
    copy_disp->valid_window.height = disp->valid_window.height;
    copy_disp->header = disp->header;
    copy_disp->image.header = disp->header;
    copy_disp->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
    copy_disp->image.height = disp->image.height;
    copy_disp->image.width = disp->image.width;
    copy_disp->image.step = disp->image.step;
    copy_disp->T = disp->T;
    copy_disp->f = disp->f;

    copy_disp->min_disparity = disp->min_disparity;
    copy_disp->max_disparity = disp->max_disparity;
    copy_disp->delta_d = disp->delta_d;

    // Create cv::Mat from the copies DisparityImage input
    sensor_msgs::Image &d_image = copy_disp->image;
    d_image.height = disparity32.rows;
    d_image.width = disparity32.cols;
    d_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
    d_image.step = d_image.width * sizeof(float);

    d_image.data.resize(d_image.step * d_image.height);

    cv::Mat_<float> dmat(d_image.height, d_image.width,
                         (float *)&d_image.data[0], d_image.step);

    // Check data
    ROS_ASSERT(dmat.data == &d_image.data[0]);

    disparity32.copyTo(dmat, binary_mask);

    // Publish obstacle disparity
    masked_pub_.publish(copy_disp);
  }
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "image_masker");

  Masker im;

  ROS_INFO("Ready");
  ros::spin();
}