skeletonization_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 
00037 #include <jsk_perception/skeletonization.h>
00038 
00039 namespace jsk_perception
00040 {
00041    
00042     void Skeletonization::onInit()
00043     {
00044        DiagnosticNodelet::onInit();
00045        
00046        this->pub_image_ = advertise<sensor_msgs::Image>(
00047           *pnh_, "image_output", 1);
00048     }
00049    
00050     void Skeletonization::subscribe()
00051     {
00052        this->sub_ = pnh_->subscribe(
00053         "input", 1,
00054         &Skeletonization::imageCallback, this);
00055     }
00056 
00057     void Skeletonization::unsubscribe()
00058     {
00059        JSK_NODELET_DEBUG("Unsubscribing from ROS topic.");
00060        this->sub_.shutdown();
00061     }
00062 
00063     void Skeletonization::imageCallback(
00064        const sensor_msgs::Image::ConstPtr& image_msg)
00065     {
00066        boost::mutex::scoped_lock lock(this->mutex_);
00067        cv_bridge::CvImagePtr cv_ptr;
00068        try {
00069           cv_ptr = cv_bridge::toCvCopy(
00070              image_msg, sensor_msgs::image_encodings::MONO8);
00071        } catch (cv_bridge::Exception& e) {
00072           JSK_ROS_ERROR("cv_bridge exception: %s", e.what());
00073           return;
00074        }
00075        cv::Mat image = cv_ptr->image;
00076        this->skeletonization(image);
00077        cv_bridge::CvImagePtr out_msg(new cv_bridge::CvImage);
00078        out_msg->header = cv_ptr->header;
00079        out_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00080        out_msg->image = image.clone();
00081        this->pub_image_.publish(out_msg->toImageMsg());
00082     }
00083 
00084     void Skeletonization::skeletonization(
00085        cv::Mat &image)
00086     {
00087        if (image.empty()) {
00088           JSK_ROS_ERROR("--CANNOT THIN EMPTY DATA...");
00089           return;
00090        }
00091        if (image.type() == CV_8UC3) {
00092           cv::cvtColor(image, image, CV_BGR2GRAY);
00093        }
00094        cv::Mat img;
00095        image.convertTo(img, CV_32F, 1/255.0);
00096        cv::Mat prev = cv::Mat::zeros(img.size(), CV_32F);
00097        cv::Mat difference;
00098        do {
00099           this->iterativeThinning(img, 0);
00100           this->iterativeThinning(img, 1);
00101           cv::absdiff(img, prev, difference);
00102           img.copyTo(prev);
00103        } while (cv::countNonZero(difference) > 0);
00104        image = img.clone();
00105     }
00106 
00107     void Skeletonization::iterativeThinning(
00108        cv::Mat& img, int iter)
00109     {
00110        if (img.empty()) {
00111           JSK_ROS_ERROR("--CANNOT THIN EMPTY DATA...");
00112           return;
00113        }
00114        cv::Mat marker = cv::Mat::zeros(img.size(), CV_32F);
00115        for (int i = 1; i < img.rows-1; i++) {
00116           for (int j = 1; j < img.cols-1; j++) {
00117              float val[9] = {};
00118              int icounter = 0;
00119              for (int y = -1; y <= 1; y++) {
00120                 for (int x = -1; x <= 1; x++) {
00121                    val[icounter] = img.at<float>(i + y, j + x);
00122                    icounter++;
00123                 }
00124              }
00125              int A = ((val[1] == 0 && val[2] == 1) ? ODD : EVEN)
00126                 + ((val[2] == 0 && val[5] == 1) ? ODD : EVEN)
00127                 + ((val[5] == 0 && val[8] == 1) ? ODD : EVEN)
00128                 + ((val[8] == 0 && val[7] == 1) ? ODD : EVEN)
00129                 + ((val[7] == 0 && val[6] == 1) ? ODD : EVEN)
00130                 + ((val[6] == 0 && val[3] == 1) ? ODD : EVEN)
00131                 + ((val[3] == 0 && val[0] == 1) ? ODD : EVEN)
00132                 + ((val[0] == 0 && val[1] == 1) ? ODD : EVEN);
00133              int B  = val[0] + val[1] + val[2] + val[3]
00134                 + val[5] + val[6] + val[7] + val[8];
00135              int m1 = iter == EVEN ? (val[1] * val[5] * val[7])
00136                 : (val[1] * val[3] * val[5]);
00137              int m2 = iter == EVEN ? (val[3] * val[5] * val[7])
00138                 : (val[1] * val[3] * val[7]);
00139              if (A == 1 && (B >= 2 && B <= 6) && !m1 && !m2) {
00140                 marker.at<float>(i, j) = sizeof(char);
00141              }
00142           }
00143        }
00144        cv::bitwise_not(marker, marker);
00145        cv::bitwise_and(img, marker, img);
00146     }
00147 }  // jsk_perception
00148 
00149 #include <pluginlib/class_list_macros.h>
00150 PLUGINLIB_EXPORT_CLASS (jsk_perception::Skeletonization, nodelet::Nodelet);


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Wed Sep 16 2015 04:36:15