plane_transform.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 "fast_plane_detector/plane_transform.h"
#include <geometry_msgs/Pose.h>

namespace fast_plane_detection {
  
  PlaneTransform::PlaneTransform(const sensor_msgs::CameraInfo &camera_info, 
                                 float baseline, float up_direction)    
    : baseline_(baseline)
    , up_direction_(up_direction)
  {
      fx_ = camera_info.P[0*4+0]; 
      fy_ = camera_info.P[1*4+1]; 
      cx_ = camera_info.P[0*4+2]; 
      cy_ = camera_info.P[1*4+2]; 
  }

    PlaneTransform::PlaneTransform(float fx, float fy, 
                                   float cx, float cy,  
                                   float baseline, float up_direction)  
        : fx_(fx)
        , fy_(fy)
        , cx_(cx)
        , cy_(cy)
        , baseline_(baseline)
        , up_direction_(up_direction)
  {}
  
  PlaneTransform::~PlaneTransform()
  {}
  
  void PlaneTransform::setParameters( float alpha, 
                                      float beta, 
                                      float d,
                                      int min_x, int max_x, 
                                      int min_y, int max_y, 
                                      std_msgs::Header header)
  {
    alpha_ = alpha;
    beta_  = beta;
    d_     = d;
    
    min_x_ = min_x;
    max_x_ = max_x;
    
    min_y_ = min_y;
    max_y_ = max_y;

    header_ = header;  
  }

  bool PlaneTransform::getPlane( Plane &plane ){
    
    //pcl::ModelCoefficients plane_coefficients 
    std::vector<double> plane_coefficients
      = convertTo3DPlane( alpha_, beta_, d_);
    
    tf::Transform plane_trans 
      = getPlaneTransform (plane_coefficients, up_direction_);

    sensor_msgs::PointCloud plane_limits;
    plane_limits.header.frame_id = header_.frame_id;
    plane_limits.header.stamp = ros::Time::now();
    plane_limits.points.resize(4);

    get3DTableLimits(plane_limits);
    
    if(!transformPlanePoints(plane_trans, plane_limits))
      return false;
    
    plane = constructPlaneMsg(plane_limits, plane_coefficients, plane_trans);
    
    return true;
  }

  
    std::vector<double> PlaneTransform::convertTo3DPlane( float alpha, 
                                                          float beta, 
                                                          float d )
  {
      /*
    float fx = camera_info.P[0*4+0]; 
    float cx = camera_info.P[0*4+2]; 
    float cy = camera_info.P[1*4+2]; 
      */
    float A = alpha/baseline_;
    float B = beta/baseline_;
    float C = d/(baseline_ * fx_) + (A*cx_ + B*cy_)/fx_;
    
    ROS_DEBUG("Camera Parameters:");
    ROS_DEBUG("Focal length: %f", fx_);
    ROS_DEBUG("Principal point: %f %f", cx_, cy_);
    ROS_DEBUG("Baseline %f", baseline_);
    ROS_DEBUG("alpha, beta and d %f %f %f", alpha, beta, d);

    // calc disparity for principal point
    float disp = alpha*cx_ + beta*cy_ + d;
    // convert point to 3D
    float Z = baseline_*fx_/disp;
    float D = C * Z;
    ROS_DEBUG("Apart from rounding error D should be 1 and is %f", D);

    
    float NMod = sqrt(A*A+B*B+C*C);
    
    A/=NMod;
    B/=NMod;
    C/=NMod;
    D/=NMod;
    
    ROS_DEBUG("3D Table plane parameters: %f %f %f %f", A, B, C, D);
 
    //    pcl::ModelCoefficients table_coefficients;
    std::vector<double> table_coefficients;
    table_coefficients.push_back(A);
    table_coefficients.push_back(B);
    table_coefficients.push_back(C); 
    table_coefficients.push_back(D);

    return table_coefficients;
  }
    
  tf::Transform 
  PlaneTransform::getPlaneTransform ( const std::vector<double> &coeffs,
                                     //const pcl::ModelCoefficients &coeffs, 
                                     double up_direction)
  {
      ROS_ASSERT(coeffs.size() > 3);
      double a = coeffs[0], b = coeffs[1], 
        c = coeffs[2], d = coeffs[3];
      //asume plane coefficients are normalized
      tf::Vector3 position(a*d, b*d, c*d);
      tf::Vector3 z(a, b, c);
      //make sure z points "up"
      ROS_DEBUG("z.dot: %0.3f", z.dot(tf::Vector3(0,0,1)));
      ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", 
                z[0], z[1], z[2]);
      if ( z.dot( tf::Vector3(0, 0, up_direction) ) < 0)
        {
          z = -1.0 * z;
          ROS_DEBUG("flipped z");
        }
      ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", 
                z[0], z[1], z[2]);
    
      //try to align the x axis with the x axis of the original frame
      //or the y axis if z and x are too close too each other
      tf::Vector3 x(1, 0, 0);
      if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = tf::Vector3(0, 1, 0);
      tf::Vector3 y = z.cross(x).normalized();
      x = y.cross(z).normalized();
    
      tf::Matrix3x3 rotation;
//      tf::Matrix3x3 rotation;
      rotation[0] = x;  // x
      rotation[1] = y;  // y
      rotation[2] = z;  // z
      rotation = rotation.transpose();
      tf::Quaternion orientation;
      rotation.getRotation(orientation);
      return tf::Transform(orientation, position);
    }
    
  bool PlaneTransform::transformPlanePoints ( const tf::Transform& plane_trans,
                                              sensor_msgs::PointCloud &plane_limits )
  {
    // Transform the data
    //    tf::TransformListener listener(ros::Duration(2.0) , false);
    //tf::TransformListener listener;
    tf::StampedTransform  plane_pose_frame(plane_trans, 
                                           plane_limits.header.stamp, 
                                           plane_limits.header.frame_id, 
                                           "plane_frame");
    listener.setTransform(plane_pose_frame);
    std::string error_msg;
    
    if (!listener.canTransform("plane_frame", plane_limits.header.frame_id, 
                               plane_limits.header.stamp, &error_msg))
      {
        ROS_ERROR("Cannot transform point cloud from frame %s to plane frame; error %s", 
                  plane_limits.header.frame_id.c_str(), error_msg.c_str());
        return false;
      }
    try
      {
        listener.transformPointCloud("plane_frame", plane_limits, plane_limits);
      }
    catch (tf::TransformException ex)
      {
        ROS_ERROR("Failed to transform point cloud from frame %s into plane_frame; error %s", 
                  plane_limits.header.frame_id.c_str(), ex.what());
        return false;
      }
    plane_limits.header.frame_id = "plane_frame";
    return true;
  }
  

  Plane 
  PlaneTransform::constructPlaneMsg(const sensor_msgs::PointCloud &plane_limits, 
                                    const std::vector<double> &coeffs,
                                    //const pcl::ModelCoefficients &coeffs, 
                                    const tf::Transform &plane_trans)
  {
    
    Plane plane;
    
    ROS_ASSERT(coeffs.size() > 3);
    double a = coeffs[0], b = coeffs[1], 
      c = coeffs[2];

    plane.normal.header = plane_limits.header;
    plane.normal.vector.x = a;
    plane.normal.vector.y = b;
    plane.normal.vector.z = c;
    

    geometry_msgs::Pose plane_pose;
    tf::poseTFToMsg(plane_trans, plane_pose);
      
    plane.top_left = plane_limits.points[0];
    plane.top_right = plane_limits.points[1];
    plane.bottom_left = plane_limits.points[3];
    plane.bottom_right = plane_limits.points[2];

    
    ROS_DEBUG("Plane Pose: [%f %f %f] [%f %f %f %f] ", 
              plane_pose.position.x, plane_pose.position.y, 
              plane_pose.position.z,
              plane_pose.orientation.x, plane_pose.orientation.y, 
              plane_pose.orientation.z, 
              plane_pose.orientation.w);
      
    plane.pose.pose = plane_pose;
    plane.pose.header = plane_limits.header;
    
    return plane;
  }

  void PlaneTransform::get3DTableLimits(sensor_msgs::PointCloud &plane_limits)
  {
    Point p;
    get3DPlanePoint(min_x_, min_y_, p);
    plane_limits.points[0] = p;

    get3DPlanePoint(max_x_, min_y_, p);
    plane_limits.points[1] = p;
    
    get3DPlanePoint(max_x_, max_y_, p);
    plane_limits.points[2] = p;
    
    get3DPlanePoint(min_x_, max_y_, p);
    plane_limits.points[3] = p;
  }

  void PlaneTransform::get3DPlanePoint( int u, int v, 
                                        Point &p)
  {
    float disp = alpha_*u + beta_*v + d_;
    /*
    float fx = camera_info_.P[0*4+0]; 
    float fy = camera_info_.P[1*4+1]; 
    float cx = camera_info_.P[0*4+2]; 
    float cy = camera_info_.P[1*4+2]; 
    */
    p.z = (fx_ * baseline_) / disp;
    p.x = (u - cx_) * p.z / fx_;
    p.y = (v - cy_) * p.z / fy_;
  }
  
} // fast_plane_detection