plane_detection.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_DETECTION
#define PLANE_DETECTION

#include <stereo_msgs/DisparityImage.h>
#include <sensor_msgs/Image.h>

namespace fast_plane_detection {

  class PlaneDetection {
    
  public:
    PlaneDetection( int drange, int w, int h, 
                    bool find_table, std_msgs::Header img_header,
                    bool search_y = true);
    ~PlaneDetection();
    
    void Update(const stereo_msgs::DisparityImage::ConstPtr&
                disparity_image);
    
    void GetPlaneParameters( float &alpha, float &beta, float &d,
                             int &min_x, int &max_x, 
                             int &min_y, int &max_y);
    float GetSpreadX();
    float GetSpreadY();
    void GetLabels(sensor_msgs::Image &image);
    void GetHistY(sensor_msgs::Image &image,
                  float &beta, float &disp);
    void GetHistX(sensor_msgs::Image &image,
                  float &beta, float &disp);
    void GetSumOfSquaredError(float &error);
    void GetNInliers(int &n_inliers);
    void GetNDisp(int &n_disp);
    void GetMeanSquaredError(float &mean_error);

  private:

    void Update_Y( const stereo_msgs::DisparityImage::ConstPtr&
                   disparity_image );
    void Update_X( const stereo_msgs::DisparityImage::ConstPtr&
                   disparity_image );
    
    sensor_msgs::Image labels_;

    sensor_msgs::Image hist_y_;
    float tmp_beta_;
    float tmp_disp_;

    sensor_msgs::Image hist_x_;
    float tmp_beta_x_;
    float tmp_disp_x_;

    int drange_;

    int width_, height_;

    int min_x_; int max_x_;
    int min_y_; int max_y_;

    bool find_table_;
    float min_beta_;

    bool search_y_;

    float alpha_;
    float beta_;
    float disp_;
 
    float spread_d_y_;
    float spread_d_x_;

    float err_;
    
    int n_inliers_;

    int n_disps_;

  };
  
} // fast_plane_detection

#endif //PLANE_DETECTION