image_cb_detector.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008-2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
37 #include <ros/console.h>
38 #include <ros/ros.h>
39 #include <cv_bridge/cv_bridge.h>
40 #include <opencv2/imgproc/imgproc.hpp>
41 #include <opencv2/calib3d/calib3d.hpp>
43 
44 using namespace image_cb_detector;
45 using namespace std;
46 
47 bool ImageCbDetector::configure(const ConfigGoal& config)
48 {
49  config_ = config;
50  return true;
51 }
52 
53 bool ImageCbDetector::detect(const sensor_msgs::ImageConstPtr& ros_image,
54  calibration_msgs::CalibrationPattern& result)
55 {
56  cv::Mat image;
57  try
58  {
59  image = cv_bridge::toCvShare(ros_image, "mono8")->image;
60  }
61  catch (cv_bridge::Exception error)
62  {
63  ROS_ERROR("error: %s", error.what());
64  return false;
65  }
66 
67  // \todo This code has been pretty much copied from laser_cb_detector. (Wow... this is poor software design)
68 
69  // ***** Resize the image based on scaling parameters in config *****
70  const int scaled_width = (int) (.5 + image.cols * config_.width_scaling);
71  const int scaled_height = (int) (.5 + image.rows * config_.height_scaling);
72  cv::Mat image_scaled;
73  cv::resize(image, image_scaled, cv::Size(scaled_width, scaled_height), 0, 0, cv::INTER_LINEAR);
74 
75  // ***** Allocate vector for found corners *****
76  vector<cv::Point2f> cv_corners;
77  cv_corners.resize(config_.num_x*config_.num_y);
78 
79  // ***** Do the actual checkerboard extraction *****
80  cv::Size board_size(config_.num_x, config_.num_y);
81  int found = cv::findChessboardCorners( image_scaled, board_size, cv_corners, cv::CALIB_CB_ADAPTIVE_THRESH) ;
82 
83  if(found)
84  {
85  ROS_DEBUG("Found CB");
86  //ROS_INFO("Found checkerboard. Subpixel window: %i", config_.subpixel_window);
87 
88 
89  cv::Size subpixel_window(config_.subpixel_window,
90  config_.subpixel_window);
91  cv::Size subpixel_zero_zone(config_.subpixel_zero_zone,
92  config_.subpixel_zero_zone);
93 
94  // Subpixel fine-tuning stuff
95  cv::cornerSubPix( image_scaled, cv_corners,
96  subpixel_window,
97  subpixel_zero_zone,
98  cv::TermCriteria(cv::TermCriteria::MAX_ITER,20,1e-2));
99  }
100  else
101  ROS_DEBUG("Didn't find CB");
102 
103  // ***** Downscale the detected corners and generate the CalibrationPattern message *****
104  result.header.stamp = ros_image->header.stamp;
105  result.header.frame_id = ros_image->header.frame_id;
106 
107  result.object_points.resize(config_.num_x * config_.num_y);
108  for (unsigned int i=0; i < config_.num_y; i++)
109  {
110  for (unsigned int j=0; j < config_.num_x; j++)
111  {
112  result.object_points[i*config_.num_x + j].x = j*config_.spacing_x;
113  result.object_points[i*config_.num_x + j].y = i*config_.spacing_y;
114  result.object_points[i*config_.num_x + j].z = 0.0;
115  }
116  }
117 
118  result.image_points.resize(cv_corners.size());
119 
120  for (size_t i=0; i < cv_corners.size(); i++)
121  {
122  result.image_points[i].x = cv_corners[i].x / config_.width_scaling;
123  result.image_points[i].y = cv_corners[i].y / config_.height_scaling;
124  }
125 
126  result.success = found;
127 
128  return true;
129 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
bool detect(const sensor_msgs::ImageConstPtr &image, calibration_msgs::CalibrationPattern &result)
bool configure(const ConfigGoal &config)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


image_cb_detector
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Thu Jun 6 2019 19:17:19