plane_transform.h

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.
 */

#ifndef PLANE_TRANSFORM
#define PLANE_TRANSFORM

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud.h>

//#include <pcl/ModelCoefficients.h>
#include "fast_plane_detection/Plane.h"

#include <tf/transform_listener.h>

namespace fast_plane_detection {
  
  typedef geometry_msgs::Point32    Point;

  class PlaneTransform {
    
  public:
    PlaneTransform(const sensor_msgs::CameraInfo &camera_info, 
                   float baseline, float up_direction);
    
    PlaneTransform(float fx, float fy, 
                   float cx, float cy,  
                   float baseline, float up_direction);
    ~PlaneTransform();

    void setParameters( float alpha, 
                        float beta, 
                        float d,
                        int min_x, int max_x, 
                        int min_y, int max_y, 
                        std_msgs::Header header);

    bool getPlane( fast_plane_detection::Plane &plane );

  private:
    
    std::vector<double> convertTo3DPlane(float alpha, 
                                         float beta, 
                                         float d );
    
    tf::Transform getPlaneTransform (const std::vector<double> &coeffs,
                                     //const pcl::ModelCoefficients &coeffs, 
                                     double up_direction);
    
    bool transformPlanePoints ( const tf::Transform& plane_trans,
                                sensor_msgs::PointCloud &plane_limits );

    fast_plane_detection::Plane 
      constructPlaneMsg(const sensor_msgs::PointCloud &plane_limits,
                        const std::vector<double> &coeffs,//const pcl::ModelCoefficients &coeffs, 
                        const tf::Transform &plane_trans);
    
    void get3DTableLimits(sensor_msgs::PointCloud &plane_limits);

    void get3DPlanePoint( int u, int v, Point &p);

    //sensor_msgs::CameraInfo camera_info_;
    
    float fx_, fy_;
    float cx_, cy_;

    float baseline_;
    float up_direction_;
    
    float alpha_, beta_, d_;

    int min_x_, max_x_;
    int min_y_, max_y_;
    
    std_msgs::Header header_;
    
    tf::TransformListener listener;

  };
  
} // fast_plane_detection

#endif