plane_transform.cpp
Go to the documentation of this file.
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 "fast_plane_detector/plane_transform.h"
00034 #include <geometry_msgs/Pose.h>
00035 
00036 namespace fast_plane_detection {
00037   
00038   PlaneTransform::PlaneTransform(const sensor_msgs::CameraInfo &camera_info, 
00039                                  float baseline, float up_direction)    
00040     : baseline_(baseline)
00041     , up_direction_(up_direction)
00042   {
00043       fx_ = camera_info.P[0*4+0]; 
00044       fy_ = camera_info.P[1*4+1]; 
00045       cx_ = camera_info.P[0*4+2]; 
00046       cy_ = camera_info.P[1*4+2]; 
00047   }
00048 
00049     PlaneTransform::PlaneTransform(float fx, float fy, 
00050                                    float cx, float cy,  
00051                                    float baseline, float up_direction)  
00052         : fx_(fx)
00053         , fy_(fy)
00054         , cx_(cx)
00055         , cy_(cy)
00056         , baseline_(baseline)
00057         , up_direction_(up_direction)
00058   {}
00059   
00060   PlaneTransform::~PlaneTransform()
00061   {}
00062   
00063   void PlaneTransform::setParameters( float alpha, 
00064                                       float beta, 
00065                                       float d,
00066                                       int min_x, int max_x, 
00067                                       int min_y, int max_y, 
00068                                       std_msgs::Header header)
00069   {
00070     alpha_ = alpha;
00071     beta_  = beta;
00072     d_     = d;
00073     
00074     min_x_ = min_x;
00075     max_x_ = max_x;
00076     
00077     min_y_ = min_y;
00078     max_y_ = max_y;
00079 
00080     header_ = header;  
00081   }
00082 
00083   bool PlaneTransform::getPlane( Plane &plane ){
00084     
00085     //pcl::ModelCoefficients plane_coefficients 
00086     std::vector<double> plane_coefficients
00087       = convertTo3DPlane( alpha_, beta_, d_);
00088     
00089     tf::Transform plane_trans 
00090       = getPlaneTransform (plane_coefficients, up_direction_);
00091 
00092     sensor_msgs::PointCloud plane_limits;
00093     plane_limits.header.frame_id = header_.frame_id;
00094     plane_limits.header.stamp = ros::Time::now();
00095     plane_limits.points.resize(4);
00096 
00097     get3DTableLimits(plane_limits);
00098     
00099     if(!transformPlanePoints(plane_trans, plane_limits))
00100       return false;
00101     
00102     plane = constructPlaneMsg(plane_limits, plane_coefficients, plane_trans);
00103     
00104     return true;
00105   }
00106 
00107   
00108     std::vector<double> PlaneTransform::convertTo3DPlane( float alpha, 
00109                                                           float beta, 
00110                                                           float d )
00111   {
00112       /*
00113     float fx = camera_info.P[0*4+0]; 
00114     float cx = camera_info.P[0*4+2]; 
00115     float cy = camera_info.P[1*4+2]; 
00116       */
00117     float A = alpha/baseline_;
00118     float B = beta/baseline_;
00119     float C = d/(baseline_ * fx_) + (A*cx_ + B*cy_)/fx_;
00120     
00121     ROS_DEBUG("Camera Parameters:");
00122     ROS_DEBUG("Focal length: %f", fx_);
00123     ROS_DEBUG("Principal point: %f %f", cx_, cy_);
00124     ROS_DEBUG("Baseline %f", baseline_);
00125     ROS_DEBUG("alpha, beta and d %f %f %f", alpha, beta, d);
00126 
00127     // calc disparity for principal point
00128     float disp = alpha*cx_ + beta*cy_ + d;
00129     // convert point to 3D
00130     float Z = baseline_*fx_/disp;
00131     float D = C * Z;
00132     ROS_DEBUG("Apart from rounding error D should be 1 and is %f", D);
00133 
00134     
00135     float NMod = sqrt(A*A+B*B+C*C);
00136     
00137     A/=NMod;
00138     B/=NMod;
00139     C/=NMod;
00140     D/=NMod;
00141     
00142     ROS_DEBUG("3D Table plane parameters: %f %f %f %f", A, B, C, D);
00143  
00144     //    pcl::ModelCoefficients table_coefficients;
00145     std::vector<double> table_coefficients;
00146     table_coefficients.push_back(A);
00147     table_coefficients.push_back(B);
00148     table_coefficients.push_back(C); 
00149     table_coefficients.push_back(D);
00150 
00151     return table_coefficients;
00152   }
00153     
00154   tf::Transform 
00155   PlaneTransform::getPlaneTransform ( const std::vector<double> &coeffs,
00156                                      //const pcl::ModelCoefficients &coeffs, 
00157                                      double up_direction)
00158   {
00159       ROS_ASSERT(coeffs.size() > 3);
00160       double a = coeffs[0], b = coeffs[1], 
00161         c = coeffs[2], d = coeffs[3];
00162       //asume plane coefficients are normalized
00163       tf::Vector3 position(a*d, b*d, c*d);
00164       tf::Vector3 z(a, b, c);
00165       //make sure z points "up"
00166       ROS_DEBUG("z.dot: %0.3f", z.dot(tf::Vector3(0,0,1)));
00167       ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", 
00168                 z[0], z[1], z[2]);
00169       if ( z.dot( tf::Vector3(0, 0, up_direction) ) < 0)
00170         {
00171           z = -1.0 * z;
00172           ROS_DEBUG("flipped z");
00173         }
00174       ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", 
00175                 z[0], z[1], z[2]);
00176     
00177       //try to align the x axis with the x axis of the original frame
00178       //or the y axis if z and x are too close too each other
00179       tf::Vector3 x(1, 0, 0);
00180       if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = tf::Vector3(0, 1, 0);
00181       tf::Vector3 y = z.cross(x).normalized();
00182       x = y.cross(z).normalized();
00183     
00184       tf::Matrix3x3 rotation;
00185 //      tf::Matrix3x3 rotation;
00186       rotation[0] = x;  // x
00187       rotation[1] = y;  // y
00188       rotation[2] = z;  // z
00189       rotation = rotation.transpose();
00190       tf::Quaternion orientation;
00191       rotation.getRotation(orientation);
00192       return tf::Transform(orientation, position);
00193     }
00194     
00195   bool PlaneTransform::transformPlanePoints ( const tf::Transform& plane_trans,
00196                                               sensor_msgs::PointCloud &plane_limits )
00197   {
00198     // Transform the data
00199     //    tf::TransformListener listener(ros::Duration(2.0) , false);
00200     //tf::TransformListener listener;
00201     tf::StampedTransform  plane_pose_frame(plane_trans, 
00202                                            plane_limits.header.stamp, 
00203                                            plane_limits.header.frame_id, 
00204                                            "plane_frame");
00205     listener.setTransform(plane_pose_frame);
00206     std::string error_msg;
00207     
00208     if (!listener.canTransform("plane_frame", plane_limits.header.frame_id, 
00209                                plane_limits.header.stamp, &error_msg))
00210       {
00211         ROS_ERROR("Cannot transform point cloud from frame %s to plane frame; error %s", 
00212                   plane_limits.header.frame_id.c_str(), error_msg.c_str());
00213         return false;
00214       }
00215     try
00216       {
00217         listener.transformPointCloud("plane_frame", plane_limits, plane_limits);
00218       }
00219     catch (tf::TransformException ex)
00220       {
00221         ROS_ERROR("Failed to transform point cloud from frame %s into plane_frame; error %s", 
00222                   plane_limits.header.frame_id.c_str(), ex.what());
00223         return false;
00224       }
00225     plane_limits.header.frame_id = "plane_frame";
00226     return true;
00227   }
00228   
00229 
00230   Plane 
00231   PlaneTransform::constructPlaneMsg(const sensor_msgs::PointCloud &plane_limits, 
00232                                     const std::vector<double> &coeffs,
00233                                     //const pcl::ModelCoefficients &coeffs, 
00234                                     const tf::Transform &plane_trans)
00235   {
00236     
00237     Plane plane;
00238     
00239     ROS_ASSERT(coeffs.size() > 3);
00240     double a = coeffs[0], b = coeffs[1], 
00241       c = coeffs[2];
00242 
00243     plane.normal.header = plane_limits.header;
00244     plane.normal.vector.x = a;
00245     plane.normal.vector.y = b;
00246     plane.normal.vector.z = c;
00247     
00248 
00249     geometry_msgs::Pose plane_pose;
00250     tf::poseTFToMsg(plane_trans, plane_pose);
00251       
00252     plane.top_left = plane_limits.points[0];
00253     plane.top_right = plane_limits.points[1];
00254     plane.bottom_left = plane_limits.points[3];
00255     plane.bottom_right = plane_limits.points[2];
00256 
00257     
00258     ROS_DEBUG("Plane Pose: [%f %f %f] [%f %f %f %f] ", 
00259               plane_pose.position.x, plane_pose.position.y, 
00260               plane_pose.position.z,
00261               plane_pose.orientation.x, plane_pose.orientation.y, 
00262               plane_pose.orientation.z, 
00263               plane_pose.orientation.w);
00264       
00265     plane.pose.pose = plane_pose;
00266     plane.pose.header = plane_limits.header;
00267     
00268     return plane;
00269   }
00270 
00271   void PlaneTransform::get3DTableLimits(sensor_msgs::PointCloud &plane_limits)
00272   {
00273     Point p;
00274     get3DPlanePoint(min_x_, min_y_, p);
00275     plane_limits.points[0] = p;
00276 
00277     get3DPlanePoint(max_x_, min_y_, p);
00278     plane_limits.points[1] = p;
00279     
00280     get3DPlanePoint(max_x_, max_y_, p);
00281     plane_limits.points[2] = p;
00282     
00283     get3DPlanePoint(min_x_, max_y_, p);
00284     plane_limits.points[3] = p;
00285   }
00286 
00287   void PlaneTransform::get3DPlanePoint( int u, int v, 
00288                                         Point &p)
00289   {
00290     float disp = alpha_*u + beta_*v + d_;
00291     /*
00292     float fx = camera_info_.P[0*4+0]; 
00293     float fy = camera_info_.P[1*4+1]; 
00294     float cx = camera_info_.P[0*4+2]; 
00295     float cy = camera_info_.P[1*4+2]; 
00296     */
00297     p.z = (fx_ * baseline_) / disp;
00298     p.x = (u - cx_) * p.z / fx_;
00299     p.y = (v - cy_) * p.z / fy_;
00300   }
00301   
00302 } // fast_plane_detection
00303 


fast_plane_detection
Author(s): Jeannette Bohg
autogenerated on Fri Jan 3 2014 12:11:20