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
00044 #include <sensor_msgs/Image.h>
00045 #include <sensor_msgs/CameraInfo.h>
00046 #include <image_cb_detector/ConfigAction.h>
00047 #include <calibration_msgs/Interval.h>
00048
00049 #include <pcl/point_types.h>
00050 #include <pcl/ModelCoefficients.h>
00051 #include <pcl/sample_consensus/method_types.h>
00052 #include <pcl/sample_consensus/model_types.h>
00053 #include <pcl/segmentation/sac_segmentation.h>
00054 #include <pcl_ros/point_cloud.h>
00055
00056 #include <message_filters/subscriber.h>
00057 #include <message_filters/synchronizer.h>
00058 #include <message_filters/sync_policies/approximate_time.h>
00059
00060 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> CameraSyncPolicy;
00061
00062 class RgbdCbDetectorAction
00063 {
00064 public:
00065 RgbdCbDetectorAction(ros::NodeHandle & n) : nh_(n),
00066 as_("cb_detector_config", false),
00067 image_sub_ (nh_, "image", 3),
00068 depth_sub_(nh_, "depth", 3),
00069 caminfo_sub_(nh_, "camera_info", 3),
00070 sync_(CameraSyncPolicy(10), image_sub_, depth_sub_, caminfo_sub_)
00071 {
00072 as_.registerGoalCallback( boost::bind(&RgbdCbDetectorAction::goalCallback, this) );
00073 as_.registerPreemptCallback( boost::bind(&RgbdCbDetectorAction::preemptCallback, this) );
00074
00075 pub_ = nh_.advertise<calibration_msgs::CalibrationPattern>("features",1);
00076 sync_.registerCallback(boost::bind(&RgbdCbDetectorAction::cameraCallback, this, _1, _2, _3));
00077
00078 as_.start();
00079
00080 last_sample_invalid_ = false;
00081 }
00082
00083 void goalCallback()
00084 {
00085 boost::mutex::scoped_lock lock(run_mutex_);
00086
00087
00088 if (as_.isActive())
00089 as_.setPreempted();
00090
00091
00092 image_cb_detector::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00093 assert(goal);
00094
00095
00096 bool success = detector_.configure(*goal);
00097
00098
00099 if (!success)
00100 as_.setAborted();
00101 }
00102
00103 void preemptCallback()
00104 {
00105 boost::mutex::scoped_lock lock(run_mutex_);
00106
00107
00108 as_.setPreempted();
00109 }
00110
00111 void cameraCallback ( const sensor_msgs::ImageConstPtr& image_msg,
00112 const sensor_msgs::ImageConstPtr& depth_msg,
00113 const sensor_msgs::CameraInfoConstPtr& caminfo_msg)
00114 {
00115 boost::mutex::scoped_lock lock(run_mutex_);
00116
00117 std::ostringstream s;
00118 s << image_sub_.getTopic() << ": ";
00119
00120 if (as_.isActive())
00121 {
00122
00123 calibration_msgs::CalibrationPattern features;
00124 bool success;
00125 success = detector_.detect(image_msg, features);
00126 if (!success)
00127 {
00128 ROS_ERROR_STREAM(s.str()<<"Error trying to detect checkerboard, not going to publish CalibrationPattern");
00129 last_sample_invalid_ = true;
00130 return;
00131 }
00132
00133
00134 cloud_converter_.initialize( image_msg, caminfo_msg );
00135 const float* depth_ptr = reinterpret_cast<const float*>(&depth_msg->data[0]);
00136 std::size_t width = depth_msg->width;
00137 std::size_t height = depth_msg->height;
00138
00139
00140 pcl::PointCloud<pcl::PointXYZ> corner_cloud;
00141 for(size_t i = 0; i< features.image_points.size(); i++){
00142 geometry_msgs::Point pixel = features.image_points[i];
00143 float depth = *(depth_ptr+width*(unsigned int)pixel.y+(unsigned int)pixel.x);
00144 if ( isnan(depth) )
00145 {
00146 continue;
00147 }
00148
00149 pcl::PointXYZ point;
00150 cloud_converter_.depthTo3DPoint( pixel, depth, point );
00151 corner_cloud.push_back( point );
00152 }
00153
00154 if( corner_cloud.size() < features.image_points.size()/2 ) {
00155 ROS_ERROR_STREAM(s.str() << "More than 50% missing 3d points in checkerboard, not going to publish CalibrationPattern");
00156 last_sample_invalid_ = true;
00157 return;
00158 }
00159
00160
00161 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00162 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00163
00164 pcl::SACSegmentation<pcl::PointXYZ> seg;
00165
00166 seg.setOptimizeCoefficients (true);
00167
00168 seg.setModelType (pcl::SACMODEL_PLANE);
00169 seg.setMethodType (pcl::SAC_RANSAC);
00170 seg.setDistanceThreshold (0.01);
00171 seg.setInputCloud (corner_cloud.makeShared ());
00172 seg.segment (*inliers, *coefficients);
00173
00174 if (inliers->indices.size () == 0)
00175 {
00176 ROS_ERROR ("Could not estimate a planar model from the checkboard corners.");
00177 last_sample_invalid_ = true;
00178 return;
00179 }
00180
00181 ROS_DEBUG_STREAM( "Model coefficients: " << coefficients->values[0] << " "
00182 << coefficients->values[1] << " "
00183 << coefficients->values[2] << " "
00184 << coefficients->values[3] );
00185
00186
00187 float nx = coefficients->values[0];
00188 float ny = coefficients->values[1];
00189 float nz = coefficients->values[2];
00190
00191
00192 float l = sqrt(nx*nx + ny*ny + nz*nz);
00193 nx /= l;
00194 ny /= l;
00195 nz /= l;
00196
00197 float d = coefficients->values[3];
00198
00199 unsigned vi=0;
00200 for(size_t i = 0; i< features.image_points.size(); i++)
00201 {
00202
00203 geometry_msgs::Point& pixel = features.image_points[i];
00204
00205 pcl::PointXYZ ray;
00206 cloud_converter_.depthTo3DPoint( pixel, 1.0, ray );
00207 l = sqrt(ray.x*ray.x + ray.y*ray.y + ray.z*ray.z);
00208 ray.x /= l;
00209 ray.y /= l;
00210 ray.z /= l;
00211
00212 float d1 = ray.x*nx + ray.y*ny + ray.z*nz;
00213 float lambda = d / d1;
00214
00215 ray.x *= lambda;
00216 ray.y *= lambda;
00217 ray.z *= lambda;
00218
00219 if ( ray.z < 0 )
00220 {
00221 ray.x *= -1.0f;
00222 ray.y *= -1.0f;
00223 ray.z *= -1.0f;
00224 }
00225
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00238
00239 pixel.z = ray.z;
00240 }
00241
00242
00243 if ( last_sample_invalid_ )
00244 {
00245 ROS_INFO_STREAM(s.str() << "Successfuly detected checkerboard.");
00246 }
00247
00248 last_sample_invalid_ = false;
00249 pub_.publish(features);
00250 }
00251 }
00252
00253 private:
00254 boost::mutex run_mutex_;
00255 ros::NodeHandle nh_;
00256 actionlib::SimpleActionServer<image_cb_detector::ConfigAction> as_;
00257 image_cb_detector::ImageCbDetector detector_;
00258
00259 message_filters::Subscriber<sensor_msgs::Image> image_sub_;
00260 message_filters::Subscriber<sensor_msgs::Image> depth_sub_;
00261 message_filters::Subscriber<sensor_msgs::CameraInfo> caminfo_sub_;
00262 message_filters::Synchronizer<CameraSyncPolicy> sync_;
00263
00264 ros::Publisher pub_;
00265
00266 bool last_sample_invalid_;
00267 DepthToPointCloud cloud_converter_;
00268 };
00269
00270 int main(int argc, char** argv)
00271 {
00272 ros::init(argc, argv, "rgbd_cb_detector_action");
00273 ros::NodeHandle n;
00274 RgbdCbDetectorAction detector_action(n);
00275 ros::spin();
00276 return 0;
00277 }
00278