fast_plane_detection: fast_plane_detection: dump_full_angle

00001 /*
00002  * Copyright (c) 2011, Jeannette Bohg and Mårten Björkman 
00003  * ({bohg,celle}@csc.kth.se) 
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are
00008  * met:
00009  *
00010  *  1.Redistributions of source code must retain the above copyright
00011  *    notice, this list of conditions and the following disclaimer.
00012  *  2.Redistributions in binary form must reproduce the above
00013  *    copyright notice, this list of conditions and the following
00014  *    disclaimer in the documentation and/or other materials provided
00015  *    with the distribution.  
00016  *  3.The name of Jeannette Bohg or Mårten Björkman may not be used to 
00017  *    endorse or promote products derived from this software without 
00018  *    specific prior written permission.
00019  *
00020  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00023  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
00024  * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00025  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00026  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00027  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00028  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00029  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00030  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031  */
00032 
00033 #include <ros/ros.h>
00034 #include <std_msgs/String.h>
00035 
00036 #include <fast_plane_detection/Plane.h>
00037 
00038 #include <tf/transform_listener.h>
00039 
00040 #include <stdio.h>
00041 
00042 #define PI 3.1415926535897932384626433832795
00043 #define BUFSIZE 512
00044 
00045 class FullPlaneDataDumper
00046 {
00047 
00048 public:
00049   FullPlaneDataDumper( int buffer_size);
00050   ~FullPlaneDataDumper();
00051   
00052   void dumpData();
00053     
00054 private:
00055 
00057   void planeCallback(const fast_plane_detection::Plane::ConstPtr& plane);
00058 
00059   
00060   void getGaussParameters(float &mean, 
00061                           float &var, 
00062                           int &norm,
00063                           const std::vector<int> &angle_hist,
00064                           float res,
00065                           float min_val);
00066 
00068   ros::NodeHandle root_nh_;
00070   ros::NodeHandle priv_nh_;
00071   
00073   ros::Subscriber plane_sub_;
00074   std::string plane_topic_;
00075 
00077   int buffer_size_;
00078 
00080   int post_fix_;
00081 
00083   tf::TransformListener listener_;
00084   tf::StampedTransform transform_;
00085 
00086   std::string camera_frame_;
00087   std::string source_frame_;
00088 
00090   int min_angle_, max_angle_;
00091   
00093   float res_angle_;
00094 
00096   int min_dist_, max_dist_;
00097 
00099   float res_dist_;
00100 
00102   std::vector<int> angle_hist_;
00103   
00105   std::vector<int> dist_hist_;
00106   
00107 
00108 };
00109 
00110 FullPlaneDataDumper::FullPlaneDataDumper( int buffer_size)
00111   : root_nh_("")
00112   , priv_nh_("~")
00113   , buffer_size_(buffer_size)
00114   , post_fix_(360)
00115 {
00116   priv_nh_.param<std::string>("plane_topic", plane_topic_,
00117                               "region_plane_msg");
00118 
00119   priv_nh_.param<std::string>("source_frame", source_frame_,
00120                               "torso_lift_link");
00121 
00122   priv_nh_.param<std::string>("camera_frame", camera_frame_,
00123                               "narrow_stereo_optical_frame");
00124 
00125   priv_nh_.param<int>("min_angle", min_angle_,0);
00126   priv_nh_.param<int>("max_angle", max_angle_,360);
00127 
00128   priv_nh_.param<int>("min_dist", min_dist_,55);
00129   priv_nh_.param<int>("max_dist", max_dist_,80);
00130   
00131   plane_sub_ = root_nh_.subscribe(plane_topic_, 
00132                                   buffer_size_, 
00133                                   &FullPlaneDataDumper::planeCallback,
00134                                   this);
00135 
00136   res_angle_ = 1.0;
00137   res_dist_ = 0.1;
00138 
00139   int angle_size = (max_angle_ - min_angle_)/res_angle_;
00140   int dist_size = (max_dist_ - min_dist_)/res_dist_;
00141   
00142   ROS_INFO("Angle_size %i \t Dist_size %i\n", angle_size, dist_size);
00143 
00144   angle_hist_.resize(angle_size,0);
00145   dist_hist_.resize(dist_size,0);
00146 
00147 }
00148 
00149 FullPlaneDataDumper::~FullPlaneDataDumper()
00150 {
00151 }
00152 
00153 void FullPlaneDataDumper::planeCallback(const fast_plane_detection::Plane::ConstPtr& plane)
00154 {
00155 
00156   if(plane->result==fast_plane_detection::Plane::FEW_INLIERS || 
00157      plane->result==fast_plane_detection::Plane::NO_PLANE){
00158     ROS_WARN("OUTLIER\n");
00159     return;
00160   }
00161   
00162 
00163   // Transform into torso_lift_link
00164   // What is the current transform  from camera to source
00165   try {
00166     listener_.lookupTransform(source_frame_,
00167                               camera_frame_, 
00168                               ros::Time(0), 
00169                               transform_);
00170   } catch (tf::TransformException ex) {
00171     ROS_ERROR("%s", ex.what());
00172   }
00173   //  Plane Point (transformed to source frame)
00174   btVector3 point1(plane->pose.pose.position.x, 
00175                    plane->pose.pose.position.y, 
00176                    plane->pose.pose.position.z);
00177   point1 = transform_.getBasis() * point1 + 
00178     transform_.getOrigin();
00179   geometry_msgs::PointStamped plane_point;
00180   plane_point.header.frame_id = source_frame_;
00181   plane_point.header.stamp = ros::Time::now();
00182   plane_point.point.x = point1.x();
00183   plane_point.point.y = point1.y();
00184   plane_point.point.z = point1.z();
00185   // Plane normal (transformed to source frame)
00186   btVector3 point2(plane->normal.vector.x, 
00187                    plane->normal.vector.y, 
00188                    plane->normal.vector.z);
00189   point2 = transform_.getBasis() * point2;
00190   /*
00191     ROS_INFO("Normal of plane in camera_frame:  %f %f %f", 
00192     plane->normal.vector.x, plane->normal.vector.y, 
00193     plane->normal.vector.z);
00194   */
00195   geometry_msgs::Vector3Stamped plane_normal;
00196   plane_normal.header.frame_id = source_frame_;
00197   plane_normal.header.stamp = ros::Time::now();
00198   plane_normal.vector.x = point2.x();
00199   plane_normal.vector.y = point2.y();
00200   plane_normal.vector.z = point2.z();
00201   /*  
00202       ROS_INFO("Normal of plane in source_frame:  %f %f %f", 
00203       plane_normal.vector.x, plane_normal.vector.y, 
00204       plane_normal.vector.z);
00205   */
00206   //float angle = (tan(plane_normal.vector.y/plane_normal.vector.x)/PI*180.0f);
00207   float angle = (atan2(plane_normal.vector.y,plane_normal.vector.x)/PI*180.0f);
00208   
00209   angle = (int)angle%360;
00210   ROS_INFO("Angle %f\n", angle);
00211 
00212   int cell;
00213   if(angle>=min_angle_ && angle<max_angle_){
00214     cell = (angle - min_angle_)/res_angle_;
00215     angle_hist_[cell]++;
00216   }
00217   float length = std::sqrt(plane_point.point.x * plane_point.point.x +
00218                            plane_point.point.y * plane_point.point.y +
00219                            plane_point.point.z * plane_point.point.z) 
00220     * 100;
00221   ROS_INFO("Length %f\n", length);
00222   
00223   if(length>min_dist_ && length<max_dist_){
00224     cell = (int)((float)(length-min_dist_)/res_dist_);
00225     dist_hist_[cell]++;
00226   }
00227 }
00228 
00229 void FullPlaneDataDumper::getGaussParameters(float &mean, 
00230                                          float &var, 
00231                                          int &norm,
00232                                          const std::vector<int> &hist,
00233                                          float res,
00234                                          float min_val)
00235 {
00236   norm = 0;
00237   mean = 0;
00238   for(size_t i = 0; i<hist.size(); ++i){
00239     norm +=hist[i];
00240     mean +=hist[i]*i;
00241   }
00242   
00243   mean /= (float)norm;
00244   mean *= res;
00245   mean += min_val;
00246   
00247   
00248   var = 0;
00249   for(size_t i = 0; i<hist.size(); ++i){
00250     float trans = i*res + min_val;
00251     var += (trans-mean) * (trans-mean) * hist[i]/(float)norm;
00252   }
00253   
00254   //ROS_INFO("Mean %f and Variance %f\n", mean, var);
00255 
00256 }
00257 
00258 void FullPlaneDataDumper::dumpData()
00259 {
00260 
00261   int norm_angle;
00262   float mean_angle, var_angle;
00263   getGaussParameters(mean_angle, var_angle, 
00264                      norm_angle, angle_hist_,
00265                      res_angle_,
00266                      min_angle_);
00267 
00268   
00269   int norm_dist;
00270   float mean_dist, var_dist;
00271   getGaussParameters(mean_dist, var_dist, 
00272                      norm_dist, dist_hist_,
00273                      res_dist_,
00274                      min_dist_);
00275 
00276   printf("Number of Angle Measurements %i\n", norm_angle);
00277   printf("Mean Angle %f\n", mean_angle);
00278   printf("Variance Angle %f\n", var_angle);
00279   
00280 
00281   char name[BUFSIZE];
00282   sprintf(name, "../eval/angle_hist_%i.txt", post_fix_);
00283   FILE* fp = fopen(name, "w");
00284   
00285   for(size_t i = 0; i<angle_hist_.size(); ++i){
00286     float angle = (float)i*res_angle_+min_angle_;
00287     float val = (float)angle_hist_[i]/(float)norm_angle;
00288     fprintf(fp, "%f \t %f\n", angle, val);
00289   }
00290   fclose(fp);
00291   
00292   printf("Number of Dist Measurements %i\n", norm_dist);
00293   printf("Mean Distance %f\n", mean_dist);
00294   printf("Variance Distance %f\n", var_dist);
00295 
00296   sprintf(name, "../eval/dist_hist_%i.txt", post_fix_);
00297   FILE* fp2 = fopen(name, "w");
00298 
00299   for(size_t i = 0; i<dist_hist_.size(); ++i) {
00300     float dist = (float)i*res_dist_+min_dist_;
00301     float val = (float)dist_hist_[i]/(float)norm_dist;
00302     fprintf(fp2, "%f \t %f\n", dist, val);
00303   }
00304   fclose(fp2);
00305 
00306   sprintf(name, "../eval/mean_angle_%i.tex", post_fix_);
00307   FILE* fp3 = fopen(name, "w");
00308   fprintf(fp3, "\\def \\meanAngle {%f}\n", mean_angle);
00309   fclose(fp3);
00310 
00311   sprintf(name, "../eval/std_angle_%i.tex", post_fix_);
00312   FILE* fp4 = fopen(name, "w");
00313   fprintf(fp4, "\\def \\stdAngle {%f}\n", std::sqrt(var_angle));
00314   fclose(fp4);
00315 
00316   sprintf(name, "../eval/mean_dist_%i.tex", post_fix_);
00317   FILE* fp5 = fopen( name, "w");
00318   fprintf(fp5, "\\def \\meanDist {%f}\n", mean_dist);
00319   fclose(fp5);
00320 
00321   sprintf(name, "../eval/std_dist_%i.tex", post_fix_);
00322   FILE* fp6 = fopen( name, "w");
00323   fprintf(fp6, "\\def \\stdDist {%f}\n", std::sqrt(var_dist));
00324   fclose(fp6);
00325 
00326 }
00327 
00328 int main(int argc, char **argv)
00329 {
00330   ros::init(argc, argv, "plane_data_dumper");
00331   
00332   int buffer_size_ = 1;
00333   FullPlaneDataDumper node_(buffer_size_);
00334   ros::spin();
00335   
00336   node_.dumpData();
00337 
00338   return 0;
00339 }
dump_full_angle_data.cpp
