fuse_images.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2017, Kentaro Wada and JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Kentaro Wada and JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <limits>
00036 #include <string>
00037 #include <vector>
00038 
00039 #include <opencv2/opencv.hpp>
00040 
00041 #include <cv_bridge/cv_bridge.h>
00042 #include <pcl_conversions/pcl_conversions.h>
00043 #include "jsk_pcl_ros/fuse_images.h"
00044 
00045 
00046 namespace jsk_pcl_ros
00047 {
00048   void FuseImages::onInit()
00049   {
00050     DiagnosticNodelet::onInit();
00051     pub_out_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00052     onInitPostProcess();
00053   }
00054 
00055   void FuseImages::subscribe()
00056   {
00057     pnh_->param("approximate_sync", approximate_sync_, true);
00058     pnh_->param("queue_size", queue_size_, 10);
00059     pnh_->param("averaging", averaging_, true);  // TODO: -> dynparam
00060 
00061     XmlRpc::XmlRpcValue input_topics;
00062     if (!pnh_->getParam("input_topics", input_topics))
00063     {
00064       NODELET_ERROR("Rosparam '~input_topics' is required.");
00065       return;
00066     }
00067     if (input_topics.getType() != XmlRpc::XmlRpcValue::TypeArray)
00068     {
00069       NODELET_ERROR("Invalid type of '~input_topics' is specified. String array is expected.");
00070       return;
00071     }
00072 
00073     // Subscribe to the filters
00074     filters_.resize(input_topics.size());
00075 
00076     // 8 topics
00077     if (approximate_sync_)
00078     {
00079       async_.reset(new message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<
00080           sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image,
00081           sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> >(queue_size_));
00082     }
00083     else
00084     {
00085       sync_.reset(new message_filters::Synchronizer<message_filters::sync_policies::ExactTime<
00086           sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image,
00087           sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> > (queue_size_));
00088     }
00089 
00090     // First input_topics.size() filters are valid
00091     ROS_INFO("Subscribing to %d topics user given topics as inputs:", input_topics.size());
00092     for (size_t i = 0; i < input_topics.size (); i++)
00093     {
00094       ROS_INFO_STREAM(" - " << (std::string)(input_topics[i]));
00095       filters_[i].reset(new message_filters::Subscriber<sensor_msgs::Image>());
00096       filters_[i]->subscribe(*pnh_, (std::string)(input_topics[i]), queue_size_);
00097     }
00098 
00099     // Bogus null filter
00100     filters_[0]->registerCallback(bind(&FuseDepthImages::input_callback, this, _1));
00101 
00102     if (input_topics.size() == 2)
00103     {
00104       if (approximate_sync_)
00105       {
00106         async_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
00107       }
00108       else
00109       {
00110         sync_->connectInput(*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
00111       }
00112     }
00113     else if (input_topics.size() == 3)
00114     {
00115       if (approximate_sync_)
00116       {
00117         async_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
00118       }
00119       else
00120       {
00121         sync_->connectInput(*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
00122       }
00123     }
00124     else if (input_topics.size() == 4)
00125     {
00126       if (approximate_sync_)
00127       {
00128         async_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
00129       }
00130       else
00131       {
00132         sync_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
00133       }
00134     }
00135     else if (input_topics.size() == 5)
00136     {
00137       if (approximate_sync_)
00138       {
00139         async_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
00140       }
00141       else
00142       {
00143         sync_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
00144       }
00145     }
00146     else if (input_topics.size() == 6)
00147     {
00148       if (approximate_sync_)
00149       {
00150         async_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
00151                              *filters_[5], nf_, nf_);
00152       }
00153       else
00154       {
00155         sync_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
00156                             *filters_[5], nf_, nf_);
00157       }
00158     }
00159     else if (input_topics.size() == 7)
00160     {
00161       if (approximate_sync_)
00162       {
00163         async_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
00164                              *filters_[5], *filters_[6], nf_);
00165       }
00166       else
00167       {
00168         sync_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
00169                             *filters_[5], *filters_[6], nf_);
00170       }
00171     }
00172     else if (input_topics.size() == 8)
00173     {
00174       if (approximate_sync_)
00175       {
00176         async_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
00177                              *filters_[5], *filters_[6], *filters_[7]);
00178       }
00179       else
00180       {
00181         sync_->connectInput(*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4],
00182                             *filters_[5], *filters_[6], *filters_[7]);
00183       }
00184     }
00185     else
00186     {
00187       NODELET_ERROR("Invalid number of topics is specified: %d. It must be > 1 and <= 8.", input_topics.size());
00188       return;
00189     }
00190 
00191     if (approximate_sync_)
00192     {
00193       async_->registerCallback(boost::bind(&FuseDepthImages::inputCb, this, _1, _2, _3, _4, _5, _6, _7, _8));
00194     }
00195     else
00196     {
00197       sync_->registerCallback(boost::bind(&FuseDepthImages::inputCb, this, _1, _2, _3, _4, _5, _6, _7, _8));
00198     }
00199   }
00200 
00201   void FuseImages::unsubscribe()
00202   {
00203     for (size_t i = 0; i < filters_.size(); i++) {
00204       filters_[i]->unsubscribe();
00205     }
00206   }
00207 
00208   bool FuseImages::validateInput(
00209       const sensor_msgs::Image::ConstPtr& in,
00210       const int height_expected,
00211       const int width_expected,
00212       std::vector<cv::Mat>& inputs)
00213   {
00214     if (in->height == 0 && in->width == 0)
00215     {
00216       // No subscription.
00217       return false;
00218     }
00219     if (in->height != height_expected || in->width != width_expected)
00220     {
00221       NODELET_ERROR_THROTTLE(10, "Input images must have same size: height=%d, width=%d.",
00222                              height_expected, width_expected);
00223       return false;
00224     }
00225     if (in->encoding != encoding_)
00226     {
00227       NODELET_ERROR_THROTTLE(10, "Input images must have same encoding. Expected: %s, Actual: %s",
00228                              encoding_.c_str(), in->encoding.c_str());
00229       return false;
00230     }
00231     inputs.push_back(cv_bridge::toCvShare(in, encoding_)->image);
00232     return true;
00233   }
00234 
00235   void FuseImages::inputCb(
00236     const sensor_msgs::Image::ConstPtr& in1, const sensor_msgs::Image::ConstPtr& in2,
00237     const sensor_msgs::Image::ConstPtr& in3, const sensor_msgs::Image::ConstPtr& in4,
00238     const sensor_msgs::Image::ConstPtr& in5, const sensor_msgs::Image::ConstPtr& in6,
00239     const sensor_msgs::Image::ConstPtr& in7, const sensor_msgs::Image::ConstPtr& in8)
00240   {
00241     int height = in1->height;
00242     int width = in1->width;
00243     std::vector<cv::Mat> inputs;
00244     validateInput(in1, height, width, inputs);
00245     validateInput(in2, height, width, inputs);
00246     validateInput(in3, height, width, inputs);
00247     validateInput(in4, height, width, inputs);
00248     validateInput(in5, height, width, inputs);
00249     validateInput(in6, height, width, inputs);
00250     validateInput(in7, height, width, inputs);
00251     validateInput(in8, height, width, inputs);
00252 
00253     cv::Mat out = fuseInputs(inputs);
00254     pub_out_.publish(cv_bridge::CvImage(in1->header, encoding_, out).toImageMsg());
00255   }
00256 
00257   cv::Mat FuseDepthImages::fuseInputs(const std::vector<cv::Mat> inputs)
00258   {
00259     cv::Mat out(inputs[0].rows, inputs[0].cols, cv_bridge::getCvType(encoding_));
00260     out.setTo(std::numeric_limits<float>::quiet_NaN());
00261     for (size_t j = 0; j < inputs[0].rows; j++)
00262     {
00263       for (size_t i = 0; i < inputs[0].cols; i++)
00264       {
00265         int n_fused = 0;
00266         float value_fused = 0;
00267         for (size_t k = 0; k < inputs.size(); k++)
00268         {
00269           float value = inputs[k].at<float>(j, i);
00270           if (!std::isnan(value))
00271           {
00272             value_fused += value;
00273             n_fused += 1;
00274           }
00275         }
00276         if (n_fused > 0)
00277         {
00278           out.at<float>(j, i) = value_fused / n_fused;
00279         }
00280       }
00281     }
00282     return out;
00283   }
00284 
00285   cv::Mat FuseRGBImages::fuseInputs(const std::vector<cv::Mat> inputs)
00286   {
00287     cv::Mat out(inputs[0].rows, inputs[0].cols, cv_bridge::getCvType(encoding_));
00288     out.setTo(cv::Scalar(0, 0, 0));
00289     for (size_t j = 0; j < inputs[0].rows; j++)
00290     {
00291       for (size_t i = 0; i < inputs[0].cols; i++)
00292       {
00293         int n_fused = 0;
00294         cv::Vec3b value_fused(0, 0, 0);
00295         for (size_t k = 0; k < inputs.size(); k++)
00296         {
00297           cv::Vec3b value = inputs[k].at<cv::Vec3b>(j, i);
00298           if (value[0] != 0 || value[1] != 0 || value[2] != 0)
00299           {
00300             if (!averaging_ && n_fused > 0)
00301             {
00302               continue;
00303             }
00304             value_fused += value;
00305             n_fused += 1;
00306           }
00307         }
00308         if (n_fused > 0)
00309         {
00310           out.at<cv::Vec3b>(j, i) = value_fused / n_fused;
00311         }
00312       }
00313     }
00314     return out;
00315   }
00316 
00317 } // namespace jsk_pcl_ros
00318 
00319 #include <pluginlib/class_list_macros.h>
00320 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::FuseDepthImages, nodelet::Nodelet);
00321 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::FuseRGBImages, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49