dump_full_angle_data.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011, Jeannette Bohg and Mårten Björkman 
 * ({bohg,celle}@csc.kth.se) 
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *  1.Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  2.Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.  
 *  3.The name of Jeannette Bohg or Mårten Björkman may not be used to 
 *    endorse or promote products derived from this software without 
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <std_msgs/String.h>

#include <fast_plane_detection/Plane.h>

#include <tf/transform_listener.h>

#include <stdio.h>

#define PI 3.1415926535897932384626433832795
#define BUFSIZE 512

class FullPlaneDataDumper
{

public:
  FullPlaneDataDumper( int buffer_size);
  ~FullPlaneDataDumper();
  
  void dumpData();
    
private:

  void planeCallback(const fast_plane_detection::Plane::ConstPtr& plane);

  
  void getGaussParameters(float &mean, 
                          float &var, 
                          int &norm,
                          const std::vector<int> &angle_hist,
                          float res,
                          float min_val);

  ros::NodeHandle root_nh_;
  ros::NodeHandle priv_nh_;
  
  ros::Subscriber plane_sub_;
  std::string plane_topic_;

  int buffer_size_;

  int post_fix_;

  tf::TransformListener listener_;
  tf::StampedTransform transform_;

  std::string camera_frame_;
  std::string source_frame_;

  int min_angle_, max_angle_;
  
  float res_angle_;

  int min_dist_, max_dist_;

  float res_dist_;

  std::vector<int> angle_hist_;
  
  std::vector<int> dist_hist_;
  

};

FullPlaneDataDumper::FullPlaneDataDumper( int buffer_size)
  : root_nh_("")
  , priv_nh_("~")
  , buffer_size_(buffer_size)
  , post_fix_(360)
{
  priv_nh_.param<std::string>("plane_topic", plane_topic_,
                              "region_plane_msg");

  priv_nh_.param<std::string>("source_frame", source_frame_,
                              "torso_lift_link");

  priv_nh_.param<std::string>("camera_frame", camera_frame_,
                              "narrow_stereo_optical_frame");

  priv_nh_.param<int>("min_angle", min_angle_,0);
  priv_nh_.param<int>("max_angle", max_angle_,360);

  priv_nh_.param<int>("min_dist", min_dist_,55);
  priv_nh_.param<int>("max_dist", max_dist_,80);
  
  plane_sub_ = root_nh_.subscribe(plane_topic_, 
                                  buffer_size_, 
                                  &FullPlaneDataDumper::planeCallback,
                                  this);

  res_angle_ = 1.0;
  res_dist_ = 0.1;

  int angle_size = (max_angle_ - min_angle_)/res_angle_;
  int dist_size = (max_dist_ - min_dist_)/res_dist_;
  
  ROS_INFO("Angle_size %i \t Dist_size %i\n", angle_size, dist_size);

  angle_hist_.resize(angle_size,0);
  dist_hist_.resize(dist_size,0);

}

FullPlaneDataDumper::~FullPlaneDataDumper()
{
}

void FullPlaneDataDumper::planeCallback(const fast_plane_detection::Plane::ConstPtr& plane)
{

  if(plane->result==fast_plane_detection::Plane::FEW_INLIERS || 
     plane->result==fast_plane_detection::Plane::NO_PLANE){
    ROS_WARN("OUTLIER\n");
    return;
  }
  

  // Transform into torso_lift_link
  // What is the current transform  from camera to source
  try {
    listener_.lookupTransform(source_frame_,
                              camera_frame_, 
                              ros::Time(0), 
                              transform_);
  } catch (tf::TransformException ex) {
    ROS_ERROR("%s", ex.what());
  }
  //  Plane Point (transformed to source frame)
  tf::Vector3 point1(plane->pose.pose.position.x, 
                   plane->pose.pose.position.y, 
                   plane->pose.pose.position.z);
  point1 = transform_.getBasis() * point1 + 
    transform_.getOrigin();
  geometry_msgs::PointStamped plane_point;
  plane_point.header.frame_id = source_frame_;
  plane_point.header.stamp = ros::Time::now();
  plane_point.point.x = point1.x();
  plane_point.point.y = point1.y();
  plane_point.point.z = point1.z();
  // Plane normal (transformed to source frame)
  tf::Vector3 point2(plane->normal.vector.x, 
                   plane->normal.vector.y, 
                   plane->normal.vector.z);
  point2 = transform_.getBasis() * point2;
  /*
    ROS_INFO("Normal of plane in camera_frame:  %f %f %f", 
    plane->normal.vector.x, plane->normal.vector.y, 
    plane->normal.vector.z);
  */
  geometry_msgs::Vector3Stamped plane_normal;
  plane_normal.header.frame_id = source_frame_;
  plane_normal.header.stamp = ros::Time::now();
  plane_normal.vector.x = point2.x();
  plane_normal.vector.y = point2.y();
  plane_normal.vector.z = point2.z();
  /*  
      ROS_INFO("Normal of plane in source_frame:  %f %f %f", 
      plane_normal.vector.x, plane_normal.vector.y, 
      plane_normal.vector.z);
  */
  //float angle = (tan(plane_normal.vector.y/plane_normal.vector.x)/PI*180.0f);
  float angle = (atan2(plane_normal.vector.y,plane_normal.vector.x)/PI*180.0f);
  
  angle = (int)angle%360;
  ROS_INFO("Angle %f\n", angle);

  int cell;
  if(angle>=min_angle_ && angle<max_angle_){
    cell = (angle - min_angle_)/res_angle_;
    angle_hist_[cell]++;
  }
  float length = std::sqrt(plane_point.point.x * plane_point.point.x +
                           plane_point.point.y * plane_point.point.y +
                           plane_point.point.z * plane_point.point.z) 
    * 100;
  ROS_INFO("Length %f\n", length);
  
  if(length>min_dist_ && length<max_dist_){
    cell = (int)((float)(length-min_dist_)/res_dist_);
    dist_hist_[cell]++;
  }
}

void FullPlaneDataDumper::getGaussParameters(float &mean, 
                                         float &var, 
                                         int &norm,
                                         const std::vector<int> &hist,
                                         float res,
                                         float min_val)
{
  norm = 0;
  mean = 0;
  for(size_t i = 0; i<hist.size(); ++i){
    norm +=hist[i];
    mean +=hist[i]*i;
  }
  
  mean /= (float)norm;
  mean *= res;
  mean += min_val;
  
  
  var = 0;
  for(size_t i = 0; i<hist.size(); ++i){
    float trans = i*res + min_val;
    var += (trans-mean) * (trans-mean) * hist[i]/(float)norm;
  }
  
  //ROS_INFO("Mean %f and Variance %f\n", mean, var);

}

void FullPlaneDataDumper::dumpData()
{

  int norm_angle;
  float mean_angle, var_angle;
  getGaussParameters(mean_angle, var_angle, 
                     norm_angle, angle_hist_,
                     res_angle_,
                     min_angle_);

  
  int norm_dist;
  float mean_dist, var_dist;
  getGaussParameters(mean_dist, var_dist, 
                     norm_dist, dist_hist_,
                     res_dist_,
                     min_dist_);

  printf("Number of Angle Measurements %i\n", norm_angle);
  printf("Mean Angle %f\n", mean_angle);
  printf("Variance Angle %f\n", var_angle);
  

  char name[BUFSIZE];
  sprintf(name, "../eval/angle_hist_%i.txt", post_fix_);
  FILE* fp = fopen(name, "w");
  
  for(size_t i = 0; i<angle_hist_.size(); ++i){
    float angle = (float)i*res_angle_+min_angle_;
    float val = (float)angle_hist_[i]/(float)norm_angle;
    fprintf(fp, "%f \t %f\n", angle, val);
  }
  fclose(fp);
  
  printf("Number of Dist Measurements %i\n", norm_dist);
  printf("Mean Distance %f\n", mean_dist);
  printf("Variance Distance %f\n", var_dist);

  sprintf(name, "../eval/dist_hist_%i.txt", post_fix_);
  FILE* fp2 = fopen(name, "w");

  for(size_t i = 0; i<dist_hist_.size(); ++i) {
    float dist = (float)i*res_dist_+min_dist_;
    float val = (float)dist_hist_[i]/(float)norm_dist;
    fprintf(fp2, "%f \t %f\n", dist, val);
  }
  fclose(fp2);

  sprintf(name, "../eval/mean_angle_%i.tex", post_fix_);
  FILE* fp3 = fopen(name, "w");
  fprintf(fp3, "\\def \\meanAngle {%f}\n", mean_angle);
  fclose(fp3);

  sprintf(name, "../eval/std_angle_%i.tex", post_fix_);
  FILE* fp4 = fopen(name, "w");
  fprintf(fp4, "\\def \\stdAngle {%f}\n", std::sqrt(var_angle));
  fclose(fp4);

  sprintf(name, "../eval/mean_dist_%i.tex", post_fix_);
  FILE* fp5 = fopen( name, "w");
  fprintf(fp5, "\\def \\meanDist {%f}\n", mean_dist);
  fclose(fp5);

  sprintf(name, "../eval/std_dist_%i.tex", post_fix_);
  FILE* fp6 = fopen( name, "w");
  fprintf(fp6, "\\def \\stdDist {%f}\n", std::sqrt(var_dist));
  fclose(fp6);

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "plane_data_dumper");
  
  int buffer_size_ = 1;
  FullPlaneDataDumper node_(buffer_size_);
  ros::spin();
  
  node_.dumpData();

  return 0;
}