ObjectLocalizer.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ias_drawer_executive/ObjectLocalizer.h>
00003 
00004 
00005 localizer_map ObjectLocalizer::localizers;
00006 
00007 ObjectLocalizer::ObjectLocalizer(std::string object_type)
00008 {
00009     object_type_ = object_type;
00010     //ROS_INFO("construced %s", object_type_.c_str());
00011     //ROS_INFO("localizers.size %zu", localizers.size());
00012     localizers[object_type] = this;
00013     //ROS_INFO("localizers.size %zu", localizers.size());
00014     //ROS_INFO("localizers[%s] %i", object_type.c_str(), localizers[object_type]);
00015     //ROS_INFO("localizers[%s] %i", "blabla", localizers["blabla"]);
00016     ROS_INFO("Adding Object Localizer for type %s", object_type.c_str());
00017 }
00018 
00019 bool ObjectLocalizer::localize(std::string object_class,tf::Stamped<tf::Pose> *poses, int numHits, Keywords keys)
00020 {
00021     ROS_INFO("Looking for a %s", object_class.c_str());
00022     localizer_map::const_iterator hit;
00023 
00024     hit = localizers.find(object_class);
00025 
00026     if (hit == localizers.end())
00027     {
00028         ROS_ERROR("No Localizer registered for object type\"%s\"", object_class.c_str());
00029         return false;
00030     }
00031     else
00032         return (*hit).second->localize_imp(object_class,poses,numHits,keys);
00033 }
00034 
00035 bool ObjectLocalizer::localize_imp(std::string object_class, tf::Stamped<tf::Pose> *poses, int numHits, Keywords keys)
00036 {
00037     ROS_ERROR("Not Implemented");
00038     return false;
00039 }
00040 
00041 
00042 // ============================================
00043 
00044 #include <ias_drawer_executive/Kinect.h>
00045 
00046 PotLocalizer::PotLocalizer()  : ObjectLocalizer("Pot")
00047 {
00048     ROS_INFO("PotLocalizer constructor");
00049 }
00050 
00051 bool PotLocalizer::localize_imp(std::string object_class, tf::Stamped<tf::Pose> *poses, int numHits, Keywords keys)
00052 {
00053     ROS_INFO("PotLocalizer::localize_imp");
00054     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00055     Kinect::getInstance()->getCloud(cloud,"/map");
00056 
00057     PotLocalizer::getPotPoseViaLid(cloud,poses[0],keys.lookup("min"),keys.lookup("max"));
00058 
00059     return true;
00060 }
00061 
00062 
00063 #include <pcl/common/common.h>
00064 #include <pcl/filters/extract_indices.h>
00065 #include <pcl/ModelCoefficients.h>
00066 #include <pcl/point_types.h>
00067 #include <pcl/sample_consensus/method_types.h>
00068 #include <pcl/sample_consensus/model_types.h>
00069 #include <pcl/segmentation/sac_segmentation.h>
00070 //#include <pcl/features/boundary.h>
00071 #include <pcl/features/normal_3d.h>
00072 
00073 #include <pcl/ros/conversions.h>
00074 #include <pcl_ros/point_cloud.h>
00075 #include <pcl_ros/transforms.h>
00076 
00077 #include <ias_drawer_executive/Geometry.h>
00078 
00079 
00080 
00081 void PotLocalizer::getPointsInBox(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox, btVector3 min, const btVector3 max)
00082 {
00083 
00084     Eigen::Vector4f min_pt, max_pt;
00085 
00086     min_pt = Eigen::Vector4f(min.x(),min.y(),min.z(), 1);
00087     max_pt = Eigen::Vector4f(max.x(),max.y(),max.z(), 1);
00088 
00089     ROS_INFO("min %f %f %f" ,min_pt[0],min_pt[1],min_pt[2]);
00090     ROS_INFO("max %f %f %f" ,max_pt[0],max_pt[1],max_pt[2]);
00091 
00092     ROS_INFO("cloud size : %zu", cloud->points.size());
00093 
00094     boost::shared_ptr<std::vector<int> > indices( new std::vector<int> );
00095     pcl::getPointsInBox(*cloud,min_pt,max_pt,*indices);
00096 
00097     pcl::ExtractIndices<pcl::PointXYZRGB> ei;
00098     ei.setInputCloud(cloud);
00099     ei.setIndices(indices);
00100     ei.filter(*inBox);
00101 
00102     //pubCloud(inBox);
00103 
00104     ROS_INFO("cloud size after box filtering: %zu", inBox->points.size());
00105 }
00106 
00107 
00108 
00109 bool PotLocalizer::getCirclesFromCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pot_cyl_cloud, double radius_goal, double radius_tolerance,
00110                                        std::vector<tf::Vector3> &center,
00111                                        std::vector<double> &radius,
00112                                        std::vector<int> &numinliers,
00113                                        size_t  iterations)
00114 {
00115 
00116 
00117     ros::NodeHandle node_handle;
00118 
00119     //ros::Publisher cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
00120 
00121     center.clear();
00122     radius.clear();
00123     numinliers.clear();
00124 
00125     center.resize(iterations);
00126     radius.resize(iterations);
00127     numinliers.resize(iterations);
00128 
00129     pcl::PointCloud<pcl::PointXYZRGB> act = *pot_cyl_cloud;
00130     pcl::PointCloud<pcl::PointXYZRGB> flip;
00131 
00132     // in each iteration, the inliers of the last circle found are taken out and the sacsegmentation is run again
00133     for (size_t k = 0; k < iterations; k++)
00134     {
00135 
00136         //sensor_msgs::PointCloud2 out; //in map frame
00137         //pcl::toROSMsg(act,out);
00138         //out.header.frame_id = "/map";
00139         //cloud_pub.publish(out);
00140 
00141         //pubCloud(act.makeShared());
00142 
00143         ros::NodeHandle node_handle;
00144         ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
00145 
00146         pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00147         pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00148         // Create the segmentation object
00149         pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00150         // Optional
00151         seg.setOptimizeCoefficients (true);
00152         // Mandatory
00153         seg.setModelType (pcl::SACMODEL_CIRCLE2D);
00154         seg.setMethodType (pcl::SAC_RANSAC);
00155         seg.setDistanceThreshold (0.01);
00156 
00157         seg.setInputCloud (act.makeShared());
00158         seg.segment (*inliers, *coefficients);
00159 
00160         seg.setRadiusLimits(radius_goal - radius_tolerance, radius_goal + radius_tolerance);
00161 
00162         if (inliers->indices.size () == 0)
00163         {
00164             PCL_ERROR ("Could not estimate a planar model for the given dataset.");
00165             return false;
00166         }
00167         else
00168         {
00169             double z = 0;
00170             for (size_t g=0; g < inliers->indices.size(); ++g)
00171             {
00172                 z += act.points[inliers->indices[g]].z;
00173             }
00174             z /=  inliers->indices.size();
00175             ROS_INFO("MODEL :");
00176             //coefficients.get()
00177             ROS_INFO("%f %f %f %f SIZE : %zu", coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3], inliers->indices.size());
00178             center[k] = tf::Vector3(coefficients->values[0], coefficients->values[1],z);
00179             radius[k] = coefficients->values[2];
00180             numinliers[k] = inliers->indices.size();
00181 
00182             tf::Stamped<tf::Pose> res;
00183             res.setOrigin(center[k]);
00184             res.setRotation(btQuaternion(0,0,0,1));
00185             res.frame_id_ = "/map";
00186 
00187             geometry_msgs::PoseStamped res_msg;
00188             tf::poseStampedTFToMsg(res,res_msg);
00189 
00190             pose_pub.publish(res_msg);
00191 
00192         }
00193         flip = act;
00194         pcl::ExtractIndices<pcl::PointXYZRGB> ei;
00195         ei.setInputCloud(flip.makeShared());
00196         ei.setIndices(inliers);
00197         ei.setNegative(true);
00198         ei.filter(act);
00199         //ros::Duration(1).sleep();
00200     }
00201     return true;
00202 }
00203 
00204 
00205 bool PotLocalizer::getHandleRotation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, tf::Stamped<tf::Pose> &potPose,double pot_handle_radius_min, double pot_handle_radius_max, btVector3 min, btVector3 max)
00206 {
00207     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
00208     getPointsInBox(cloud,cloud_filtered,min,max);
00209 
00210     // we will calculate the angle of each point that we think belongs to the handle via arctan and then average over that angles
00211     double atansum = 0;
00212     double atannum = 0;
00213 
00214     for (size_t k = 0; k < cloud_filtered->points.size(); ++k)
00215     {
00216         tf::Vector3 act(cloud_filtered->points[k].x,cloud_filtered->points[k].y,cloud_filtered->points[k].z);
00217         tf::Vector3 rel = potPose.getOrigin() - act;
00218         rel.setZ(0);
00219         double radius = rel.length();
00220 
00221         if ((radius > pot_handle_radius_min) && (radius < pot_handle_radius_max))
00222         {
00223             tf::Vector3 act(cloud_filtered->points[k].x- potPose.getOrigin().x(),cloud_filtered->points[k].y - potPose.getOrigin().y(),0);
00224             if (act.x() != 0.0)
00225             {
00226                 double at = atan2(act.y(),act.x());
00227 
00228                 //we want the rotation modulo 180deg
00229                 if (at < 0)
00230                     at += M_PI;
00231 
00232                 atansum += at;
00233                 atannum += 1;
00234             }
00235         }
00236     }
00237 
00238     if (atannum < 20)
00239     {
00240         ROS_ERROR("did not get enough handle inliers");
00241         return false;
00242     }
00243 
00244     atansum /= atannum;
00245 
00246     potPose.setRotation(btQuaternion(tf::Vector3(0,0,1),atansum));
00247     return true;
00248 }
00249 
00250 bool PotLocalizer::getLidPose(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, tf::Stamped<tf::Pose> &lid, double radius_goal, tf::Vector3 min, tf::Vector3 max)
00251 {
00252 
00253     pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox (new pcl::PointCloud<pcl::PointXYZRGB>);
00254 
00255     getPointsInBox(cloud,inBox,min,max);
00256 
00257     //pubCloud(inBox);
00258 
00259     std::vector<tf::Vector3> center;
00260     std::vector<double> radius;
00261     std::vector<int> numinliers;
00262 
00263     // for now we assume we see only one circle fitting and this will be the one, todo: get multiple hyptheses and preserve them
00264     getCirclesFromCloud(inBox, radius_goal, .04,center,radius,numinliers,1);
00265 
00266     // sanity check, we get will usually see a lot more points
00267     if (numinliers[0] < 300)
00268         return false;
00269 
00270     lid.setOrigin(center[0]);
00271     lid.setRotation(btQuaternion(0,0,0,1));
00272     lid.frame_id_ = "/map";
00273 
00274     // TODO: dont do this here, for now this should be independent of the robot frame
00275     //lid = Geometry::getPoseIn("base_link", lid);
00276     //lid.setRotation(btQuaternion(0,0,1,0));
00277     //lid = Geometry::getPoseIn("map", lid);
00278 
00279     //announce("Lid", lid);
00280 
00281     return true;
00282 }
00283 
00284 
00285 bool PotLocalizer::getPotPoseViaLid(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, tf::Stamped<tf::Pose> &potPose, tf::Vector3 min, tf::Vector3 max)
00286 {
00287     double table_height = min.z();
00288 
00289     // some values for our magical pot, todo: move to parameters, xml or something
00290     double pot_cyl_min = 0.02;
00291     double pot_cyl_max = 0.05;
00292     double pot_handle_min = 0.06;
00293     double pot_handle_max = 0.09;
00294     double pot_radius = 0.20 / 2.0;
00295     double pot_handle_radius_max = 0.28 / 2.0;
00296     double pot_handle_radius_min = 0.25 / 2.0;
00297 
00298     //announce_box("pot search", roi_min, roi_max);
00299     //announce_box("pot search", roi_min, roi_max);
00300 
00301     ros::NodeHandle node_handle;
00302 
00303     ros::Publisher cloud_pub = node_handle.advertise<sensor_msgs::PointCloud2>("/debug_cloud",0,true);
00304     ros::Publisher pose_pub = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detection", 0 , true);
00305     ros::Publisher pose_pubr = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionr", 0 , true);
00306     ros::Publisher pose_publ = node_handle.advertise<geometry_msgs::PoseStamped>( "/cop_detectionl", 0 , true);
00307 
00308 
00310     tf::Vector3 center;
00311 
00312     tf::Stamped<tf::Pose> lidPose;
00313     //if (!getLidPose(cloud,lidPose, min + tf::Vector3(0,0,pot_handle_max), max))
00314     if (!getLidPose(cloud,lidPose, pot_radius, min + tf::Vector3(0,0,pot_cyl_min), tf::Vector3(max.x(), max.y(), min.z() + pot_cyl_max)))
00315     {
00316         ROS_ERROR("Could not find LID in Search Region!");
00317         return false;
00318     }
00319     center = lidPose.getOrigin();
00320 
00321     ROS_INFO("LID CENTER %f %f %f",center.x(),center.y(),center.z());
00322 
00323 
00325     center.setZ(table_height);
00326 
00327     potPose.setOrigin(center);
00328 
00329     if (!getHandleRotation(cloud,potPose,pot_handle_radius_min, pot_handle_radius_max, center - tf::Vector3(pot_handle_radius_max,pot_handle_radius_max,-pot_handle_min), center + tf::Vector3(pot_handle_radius_max,pot_handle_radius_max,pot_handle_max)))
00330     {
00331         ROS_ERROR("Could not get Hanndle Orientation");
00332         return false;
00333     }
00334 
00335     Geometry::printPose("Pot Pose", potPose);
00336 
00337     return true;
00338 }
00339 
00340 
00341 
00342 
00343 
00344 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 09:44:57