Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00036
00037 #include <boost/thread.hpp>
00038 #include <ros/console.h>
00039 #include <ros/ros.h>
00040 #include <actionlib/server/simple_action_server.h>
00041 #include <image_cb_detector/image_cb_detector.h>
00042 #include <image_cb_detector/depth_to_pointcloud.h>
00043 #include <opencv2/core/core.hpp>
00044 #include <opencv2/core/core_c.h>
00045
00046 #include <sensor_msgs/Image.h>
00047 #include <sensor_msgs/CameraInfo.h>
00048 #include <image_cb_detector/ConfigAction.h>
00049 #include <calibration_msgs/Interval.h>
00050
00051 #include <message_filters/subscriber.h>
00052 #include <message_filters/synchronizer.h>
00053 #include <message_filters/sync_policies/approximate_time.h>
00054
00055 #include <sensor_msgs/image_encodings.h>
00056
00057 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> CameraSyncPolicy;
00058
00059 class RgbdCbDetectorAction
00060 {
00061 public:
00062 RgbdCbDetectorAction(ros::NodeHandle & n) : nh_(n),
00063 as_("cb_detector_config", false),
00064 image_sub_ (nh_, "image", 3),
00065 depth_sub_(nh_, "depth", 3),
00066 caminfo_sub_(nh_, "camera_info", 3),
00067 sync_(CameraSyncPolicy(10), image_sub_, depth_sub_, caminfo_sub_)
00068 {
00069 as_.registerGoalCallback( boost::bind(&RgbdCbDetectorAction::goalCallback, this) );
00070 as_.registerPreemptCallback( boost::bind(&RgbdCbDetectorAction::preemptCallback, this) );
00071
00072 pub_ = nh_.advertise<calibration_msgs::CalibrationPattern>("features",1);
00073 sync_.registerCallback(boost::bind(&RgbdCbDetectorAction::cameraCallback, this, _1, _2, _3));
00074
00075 as_.start();
00076
00077 last_sample_invalid_ = false;
00078 }
00079
00080 void goalCallback()
00081 {
00082 boost::mutex::scoped_lock lock(run_mutex_);
00083
00084
00085 if (as_.isActive())
00086 as_.setPreempted();
00087
00088
00089 image_cb_detector::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00090 assert(goal);
00091
00092
00093 bool success = detector_.configure(*goal);
00094
00095
00096 if (!success)
00097 as_.setAborted();
00098 }
00099
00100 void preemptCallback()
00101 {
00102 boost::mutex::scoped_lock lock(run_mutex_);
00103
00104
00105 as_.setPreempted();
00106 }
00107
00108 void cameraCallback ( const sensor_msgs::ImageConstPtr& image_msg,
00109 const sensor_msgs::ImageConstPtr& depth_msg,
00110 const sensor_msgs::CameraInfoConstPtr& caminfo_msg)
00111 {
00112 boost::mutex::scoped_lock lock(run_mutex_);
00113
00114 std::ostringstream s;
00115 s << image_sub_.getTopic() << ": ";
00116
00117 if (as_.isActive())
00118 {
00119
00120 calibration_msgs::CalibrationPattern features;
00121 bool success;
00122 success = detector_.detect(image_msg, features);
00123 if (!success)
00124 {
00125 ROS_ERROR_STREAM(s.str()<<"Error trying to detect checkerboard, not going to publish CalibrationPattern");
00126 last_sample_invalid_ = true;
00127 return;
00128 }
00129 if (depth_msg->encoding != sensor_msgs::image_encodings::TYPE_32FC1) {
00130 ROS_ERROR_STREAM("Depth image must be 32-bit floating point (encoding '32FC1'), but has encoding '" << depth_msg->encoding << "'");
00131 last_sample_invalid_ = true;
00132 return;
00133 }
00134
00135
00136 cloud_converter_.initialize( image_msg, caminfo_msg );
00137 const float* depth_ptr = reinterpret_cast<const float*>(&depth_msg->data[0]);
00138 std::size_t width = depth_msg->width;
00139 std::size_t height = depth_msg->height;
00140
00141
00142 std::vector<cv::Point3f> corner_cloud;
00143 for(size_t i = 0; i< features.image_points.size(); i++){
00144 geometry_msgs::Point pixel = features.image_points[i];
00145 float depth = *(depth_ptr+width*(unsigned int)pixel.y+(unsigned int)pixel.x);
00146 if ( isnan(depth) )
00147 {
00148 continue;
00149 }
00150
00151 cv::Point3f point;
00152 cloud_converter_.depthTo3DPoint( pixel, depth, point );
00153 corner_cloud.push_back( point );
00154 }
00155
00156 if( corner_cloud.size() < features.image_points.size()/2 ) {
00157 ROS_ERROR_STREAM(s.str() << "More than 50% missing 3d points in checkerboard, not going to publish CalibrationPattern");
00158 last_sample_invalid_ = true;
00159 return;
00160 }
00161
00162
00163 cv::Vec3f n;
00164 float d;
00165 float best_ratio = 0;
00166 cv::RNG rng;
00167 for(size_t i = 0; i < 100; ++i) {
00168
00169 for(int i=0;i<3;++i) {
00170 std::swap(corner_cloud[i], corner_cloud[rng.uniform(i+1, int(corner_cloud.size()))]);
00171 }
00172
00173 cv::Vec3f nrm = cv::Vec3f(corner_cloud[2]-corner_cloud[0]).cross(cv::Vec3f(corner_cloud[1])-cv::Vec3f(corner_cloud[0]));
00174 nrm = nrm / cv::norm(nrm);
00175 cv::Vec3f x0(corner_cloud[0]);
00176
00177 float p_to_plane_thresh = 0.01;
00178 int num_inliers = 0;
00179
00180
00181 for (size_t i=0; i<corner_cloud.size(); ++i) {
00182 cv::Vec3f w = cv::Vec3f(corner_cloud[i]) - x0;
00183 float D = std::fabs(nrm.dot(w));
00184 if(D < p_to_plane_thresh)
00185 ++num_inliers;
00186 }
00187 float ratio = float(num_inliers) / float(corner_cloud.size());
00188 if (ratio > best_ratio) {
00189 best_ratio = ratio;
00190 n = nrm;
00191 d = -x0.dot(nrm);
00192 }
00193 }
00194
00195 if(best_ratio < 0.9)
00196 {
00197 ROS_ERROR ("Could not estimate a planar model from the checkboard corners.");
00198 last_sample_invalid_ = true;
00199 return;
00200 }
00201
00202 ROS_DEBUG_STREAM( "Model coefficients: " << n[0] << " "
00203 << n[1] << " "
00204 << n[2] << " "
00205 << d );
00206
00207 unsigned vi=0;
00208 for(size_t i = 0; i< features.image_points.size(); i++)
00209 {
00210
00211 geometry_msgs::Point& pixel = features.image_points[i];
00212
00213 cv::Point3f ray_pt;
00214 cloud_converter_.depthTo3DPoint( pixel, 1.0, ray_pt );
00215 cv::Vec3f ray(ray_pt);
00216 ray = ray / cv::norm(ray);
00217
00218 float d1 = ray.dot(n);
00219 float lambda = d / d1;
00220
00221 ray = ray * lambda;
00222
00223 if ( ray[2] < 0 )
00224 {
00225 ray = -ray;
00226 }
00227
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00240
00241 pixel.z = ray[2];
00242 }
00243
00244
00245 if ( last_sample_invalid_ )
00246 {
00247 ROS_INFO_STREAM(s.str() << "Successfuly detected checkerboard.");
00248 }
00249
00250 last_sample_invalid_ = false;
00251 pub_.publish(features);
00252 }
00253 }
00254
00255 private:
00256 boost::mutex run_mutex_;
00257 ros::NodeHandle nh_;
00258 actionlib::SimpleActionServer<image_cb_detector::ConfigAction> as_;
00259 image_cb_detector::ImageCbDetector detector_;
00260
00261 message_filters::Subscriber<sensor_msgs::Image> image_sub_;
00262 message_filters::Subscriber<sensor_msgs::Image> depth_sub_;
00263 message_filters::Subscriber<sensor_msgs::CameraInfo> caminfo_sub_;
00264 message_filters::Synchronizer<CameraSyncPolicy> sync_;
00265
00266 ros::Publisher pub_;
00267
00268 bool last_sample_invalid_;
00269 DepthToPointCloud cloud_converter_;
00270 };
00271
00272 int main(int argc, char** argv)
00273 {
00274 ros::init(argc, argv, "rgbd_cb_detector_action");
00275 ros::NodeHandle n;
00276 RgbdCbDetectorAction detector_action(n);
00277 ros::spin();
00278 return 0;
00279 }
00280