43 DiagnosticNodelet::onInit();
45 this->
pub_image_ = advertise<sensor_msgs::Image>(
46 *
pnh_,
"image_output", 1);
64 const sensor_msgs::Image::ConstPtr& image_msg)
72 ROS_ERROR(
"cv_bridge exception: %s", e.what());
75 cv::Mat image = cv_ptr->image;
78 out_msg->header = cv_ptr->header;
80 out_msg->image = image.clone();
91 if (image.type() == CV_8UC3) {
92 cv::cvtColor(image, image, CV_BGR2GRAY);
95 image.convertTo(img, CV_32F, 1/255.0);
96 cv::Mat prev = cv::Mat::zeros(img.size(), CV_32F);
101 cv::absdiff(img, prev, difference);
103 }
while (cv::countNonZero(difference) > 0);
108 cv::Mat&
img,
int iter)
111 ROS_ERROR(
"--CANNOT THIN EMPTY DATA...");
114 cv::Mat marker = cv::Mat::zeros(img.size(), CV_32F);
116 #pragma omp parallel for collapse(2) num_threads(this->num_threads_) 118 for (
int i = 1; i < img.rows-1; i++) {
119 for (
int j = 1; j < img.cols-1; j++) {
122 for (
int y = -1;
y <= 1;
y++) {
123 for (
int x = -1;
x <= 1;
x++) {
124 val[icounter] = img.at<
float>(i +
y, j +
x);
128 int A = ((val[1] == 0 && val[2] == 1) ?
ODD :
EVEN)
129 + ((val[2] == 0 && val[5] == 1) ?
ODD : EVEN)
130 + ((val[5] == 0 && val[8] == 1) ?
ODD : EVEN)
131 + ((val[8] == 0 && val[7] == 1) ?
ODD : EVEN)
132 + ((val[7] == 0 && val[6] == 1) ?
ODD : EVEN)
133 + ((val[6] == 0 && val[3] == 1) ?
ODD : EVEN)
134 + ((val[3] == 0 && val[0] == 1) ?
ODD : EVEN)
135 + ((val[0] == 0 && val[1] == 1) ?
ODD : EVEN);
136 int B = val[0] + val[1] + val[2] + val[3]
137 + val[5] + val[6] + val[7] + val[8];
138 int m1 = iter == EVEN ? (val[1] * val[5] * val[7])
139 : (val[1] * val[3] * val[5]);
140 int m2 = iter == EVEN ? (val[3] * val[5] * val[7])
141 : (val[1] * val[3] * val[7]);
142 if (A == 1 && (B >= 2 && B <= 6) && !m1 && !m2) {
143 marker.at<
float>(i, j) =
sizeof(
char);
147 cv::bitwise_not(marker, marker);
148 cv::bitwise_and(img, marker, img);
ros::Publisher pub_image_
void publish(const boost::shared_ptr< M > &message) const
virtual void unsubscribe()
virtual void imageCallback(const sensor_msgs::Image::ConstPtr &)
virtual void iterativeThinning(cv::Mat &, int)
const std::string TYPE_32FC1
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void skeletonization(cv::Mat &)
PLUGINLIB_EXPORT_CLASS(jsk_perception::Skeletonization, nodelet::Nodelet)
#define NODELET_DEBUG(...)