plane_detection.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_detection.h"
#include <sensor_msgs/image_encodings.h>
#include <ros/console.h>

namespace enc = sensor_msgs::image_encodings;

namespace fast_plane_detection {
  
  PlaneDetection::PlaneDetection( int drange, 
                                  int w, int h, 
                                  bool find_table,
                                  std_msgs::Header img_header,
                                  bool search_y)
    : drange_(drange)
    , width_(w)
    , height_(h)
    , min_x_(width_)
    , max_x_(0)
    , min_y_(height_)
    , max_y_(0)
    , find_table_(find_table)
    , search_y_(search_y)
    , spread_d_y_(1.0f)
    , spread_d_x_(1.0f)
  {
    if(find_table_)
      min_beta_ = 0.08f;

    labels_.header = img_header;
    labels_.width  = w;
    labels_.height = h;
    labels_.encoding = enc::MONO8;
    labels_.is_bigendian = false;
    labels_.step = labels_.width;
    size_t size = labels_.step * labels_.height;
    labels_.data.resize(size, 0);

    hist_y_.header = img_header;
    hist_y_.width  = drange_;
    hist_y_.height = h;
    hist_y_.encoding = enc::MONO8;
    hist_y_.is_bigendian = false;
    hist_y_.step = hist_y_.width;
    size = hist_y_.step * hist_y_.height;
    hist_y_.data.resize(size, 0);

    
    hist_x_.header = img_header;
    hist_x_.width  = drange_;
    hist_x_.height = w;
    hist_x_.encoding = enc::MONO8;
    hist_x_.is_bigendian = false;
    hist_x_.step = hist_x_.width;
    size = hist_x_.step * hist_x_.height;
    hist_x_.data.resize(size, 0);

  }

  PlaneDetection::~PlaneDetection()
  {}
  
  void PlaneDetection::Update(const stereo_msgs::DisparityImage::ConstPtr&
                              disparity_image)
  {
    if(search_y_)
      Update_Y( disparity_image );
    else 
      Update_X( disparity_image );
  }

  void PlaneDetection::Update_Y(const stereo_msgs::DisparityImage::ConstPtr&
                                disparity_image)
  {
    // reset plane limits
    min_x_ = width_;
    max_x_ = 0;
    min_y_ = height_;
    max_y_ = 0;

    float *dimd = (float*)&(disparity_image->image.data[0]);
    uint8_t *labelsd = (uint8_t*)&(labels_.data[0]);
    uint8_t *histd = (uint8_t*)&(hist_y_.data[0]);

    int w = disparity_image->image.width;
    int h = disparity_image->image.height;
    
    for (int y=0;y<h;y++) {
      for (int x=0;x<w;x++) {
        int i = y*w + x;
        labelsd[i]=0;
      }
    }

    // Find dominating disparity for each y-value
    int *hist = new int[h*drange_];
    int *totals = new int[h];
    for (int y=0;y<h;y++) {
      int *histy = &hist[y*drange_];
      uint8_t *histdy = &histd[y*drange_];
      for (int i=0;i<drange_;i++){ 
        histy[i] = 0;
        histdy[i] = 0;
      }
      for (int x=0;x<w;x++) {
        int d = (int)dimd[y*w + x];
        if (d>=0 && d<drange_){
          histy[d] ++;
          histdy[d] ++;
        }
      }
      for (int i=0;i<drange_-2;i++){
        histy[i] = histy[i] + 2*histy[i+1] + histy[i+2];
        histdy[i] = histdy[i] + 2*histdy[i+1] + histdy[i+2];
      }
      for (int i=drange_-1;i>1;i--){
        histy[i] = histy[i] + 2*histy[i-1] + histy[i-2];
        histdy[i] = histdy[i] + 2*histdy[i-1] + histdy[i-2];
      }
      totals[y] = 0;
      for (int i=0;i<drange_;i++)
        totals[y] += histy[i];
    }

    

    // Find best line using random sampling
    float maxwei = 0.0f; 
    alpha_ = 0.0f; 
    beta_ = 0.0f; 
    disp_ = drange_/2;
    for (int l=0;l<1000;l++) {
      int idx1 = rand()%h;
      int idx2 = rand()%h;
      while (idx1==idx2)
        idx2 = rand()%h;
      if (!totals[idx1] || !totals[idx2])
        continue;
      int cnt1 = rand()%totals[idx1];
      int cnt2 = rand()%totals[idx2];
      int disp1 = 0, disp2 = 0;
      for (int sum1=0;sum1<cnt1;disp1++) 
        sum1 += hist[idx1*drange_+disp1];
      for (int sum2=0;sum2<cnt2;disp2++) 
        sum2 += hist[idx2*drange_+disp2];
      disp1--;
      disp2--;
      float dgra = (float)(disp2 - disp1) / (idx2 - idx1);
      float dzer = disp2 - dgra*idx2;
      float sumwei = 0.0f;
      for (int y=0;y<h;y++) {
        for (int dd=-3;dd<=3;dd++) {
          int d = (int)(dgra*y + dzer + 0.5f) + dd;
          if (d<0 || d>=drange_) 
            continue;
          float er = d - (dgra*y + dzer);
          sumwei += hist[y*drange_ + d] / (4.0f + er*er);
        }
      }
      if (sumwei>maxwei) 
        if( !find_table_ || dgra>min_beta_) {
          maxwei = sumwei;
          beta_ = dgra;
          disp_ = dzer;
        }
    }
    
    // tmp_beta_ = beta_;
    //tmp_disp_ = disp_;

    //    ROS_WARN("beta_ %f, disp_ %f", beta_, disp_);

    // Improve line (depends only on y) using m-estimator
    for (int l=0;l<3;l++) {
      float syy = 0.0, sy1 = 0.0f, s11 = 0.0f;
      float sdy = 0.0, sd1 = 0.0f;
      for (int y=0;y<h;y++) {
        for (int dd=-3;dd<=3;dd++) {
          int d = (int)(beta_*y + disp_ + 0.5f) + dd;
          if (d<0 || d>=drange_) 
            continue;
          float er = d - (beta_*y + disp_);
          float w = hist[y*drange_ + d] / (4.0f + er*er);
          syy += w*y*y;
          sy1 += w*y;
          s11 += w;
          sdy += w*d*y;
          sd1 += w*d;
        }
      }
      float det = syy*s11 - sy1*sy1;
      beta_ = s11*sdy - sy1*sd1;
      disp_ = syy*sd1 - sy1*sdy;
      if (det!=0.0f) {
        beta_ /= det;
        disp_ /= det;
      }
    }
    disp_ += 0.5f;

    tmp_beta_ = beta_;
    tmp_disp_ = disp_;

    // Improve plane (depends on both x and y) using m-estimator
    for (int l=0;l<3;l++) {
      float sxx = 0.0, sx1 = 0.0f, s11 = 0.0f;
      float sdx = 0.0, sd1 = 0.0f;
      for (int y=0;y<h;y++) {
        for (int x=0;x<w;x++) {
          if (dimd[y*w+x]>0.0f) {
            float d = dimd[y*w+x] - beta_*y;
            float er = d - (alpha_*x + disp_);
            float w = 1.0f / (1.0f + er*er);
            sxx += w*x*x;
            sx1 += w*x;
            s11 += w;
            sdx += w*d*x;
            sd1 += w*d;
          }
        }
      }
      float det = sxx*s11 - sx1*sx1;
      alpha_ = s11*sdx - sx1*sd1;
      disp_ = sxx*sd1 - sx1*sdx;
      if (det!=0.0f) {
        alpha_ /= det;
        disp_ /= det;
      }
    }

    int num=0;
    int numDisp=0;
    float vard = 0.0f;
    for (int y=0;y<h;y++) {
      for (int x=0;x<w;x++) {
        int i = y*w + x;
        float d = dimd[i];
        if (d>=0.0f && d<drange_) {
          numDisp++;
          float er = alpha_*x + beta_*y + disp_ - d;
          if (er*er<4*spread_d_y_) {
            vard += er*er;
            num++;
            labelsd[i] = 255;

            if(y>max_y_)
              max_y_ = y;;
            if(y<min_y_)
              min_y_ = y;

            if(x>max_x_)
              max_x_ = x;
            if(x<min_x_)
              min_x_ = x;
          } 
        }        
      }
    }
    spread_d_y_ = (num ? vard/num : 1e-3);
    // sum of squared errors
    err_ = vard;
    // number of inliers
    n_inliers_ = num;

    // number of valid disparities
    n_disps_ = numDisp;
 
    delete [] hist;
    delete [] totals;
    ROS_DEBUG("YYY ### Plane: %f %f %f %f", 
              alpha_, beta_, disp_, spread_d_y_);
    ROS_DEBUG("YYY ### Pixel limits: %d %d %d %d", 
             min_x_, max_x_, min_y_, max_y_);

    labels_.header.stamp = ros::Time::now();
    hist_y_.header.stamp = ros::Time::now();
    
  }


  void PlaneDetection::Update_X(const stereo_msgs::DisparityImage::ConstPtr&
                                disparity_image)
  {
    // reset plane limits
    min_x_ = width_;
    max_x_ = 0;
    min_y_ = height_;
    max_y_ = 0;

    float *dimd = (float*)&(disparity_image->image.data[0]);
    uint8_t *labelsd = (uint8_t*)&(labels_.data[0]);
    uint8_t *histd = (uint8_t*)&(hist_x_.data[0]);

    int w = disparity_image->image.width;
    int h = disparity_image->image.height;

    for (int y=0;y<h;y++) {
      for (int x=0;x<w;x++) {
        int i = y*w + x;
        labelsd[i]=0;
      }
    }

    // Find dominating disparity for each x-value
    int *hist = new int[w*drange_];
    int *totals = new int[w];
    for (int x=0;x<w;x++) {
      int *histx = &hist[x*drange_];
      uint8_t *histdx = &histd[x*drange_];
      for (int i=0;i<drange_;i++){ 
        histx[i] = 0;
        histdx[i] = 0;
      }
      for (int y=0;y<h;y++) {
        int d = (int)dimd[y*w + x];
        if (d>=0 && d<drange_){
          histx[d] ++;
          histdx[d] ++;
        }
      }
      for (int i=0;i<drange_-2;i++){
        histx[i] = histx[i] + 2*histx[i+1] + histx[i+2];
        histdx[i] = histdx[i] + 2*histdx[i+1] + histdx[i+2];
      }
      for (int i=drange_-1;i>1;i--){
        histx[i] = histx[i] + 2*histx[i-1] + histx[i-2];
        histdx[i] = histdx[i] + 2*histdx[i-1] + histdx[i-2];
      }
      totals[x] = 0;
      for (int i=0;i<drange_;i++)
        totals[x] += histx[i];
    }

    

    // Find best line using random sampling
    float maxwei = 0.0f; 
    alpha_ = 0.0f; 
    beta_ = 0.0f; 
    disp_ = drange_/2;
    for (int l=0;l<1000;l++) {
      int idx1 = rand()%h;
      int idx2 = rand()%h;
      while (idx1==idx2)
        idx2 = rand()%h;
      if (!totals[idx1] || !totals[idx2])
        continue;
      int cnt1 = rand()%totals[idx1];
      int cnt2 = rand()%totals[idx2];
      int disp1 = 0, disp2 = 0;
      for (int sum1=0;sum1<cnt1;disp1++) 
        sum1 += hist[idx1*drange_+disp1];
      for (int sum2=0;sum2<cnt2;disp2++) 
        sum2 += hist[idx2*drange_+disp2];
      disp1--;
      disp2--;
      float dgra = (float)(disp2 - disp1) / (idx2 - idx1);
      float dzer = disp2 - dgra*idx2;
      float sumwei = 0.0f;
      for (int y=0;y<h;y++) {
        for (int dd=-3;dd<=3;dd++) {
          int d = (int)(dgra*y + dzer + 0.5f) + dd;
          if (d<0 || d>=drange_) 
            continue;
          float er = d - (dgra*y + dzer);
          sumwei += hist[y*drange_ + d] / (4.0f + er*er);
        }
      }
      if (sumwei>maxwei) 
        if( !find_table_ || dgra>min_beta_) {
          maxwei = sumwei;
          beta_ = dgra;
          disp_ = dzer;
        }
    }
    
    //tmp_beta_x_ = beta_;
    //tmp_disp_x_ = disp_;

    
    //    ROS_WARN("beta_ %f, disp_ %f", beta_, disp_);

    // Improve line (depends only on y) using m-estimator
    for (int l=0;l<3;l++) {
      float sxx = 0.0, sx1 = 0.0f, s11 = 0.0f;
      float sdx = 0.0, sd1 = 0.0f;
      for (int x=0;x<w;x++) {
        for (int dd=-3;dd<=3;dd++) {
          int d = (int)(beta_*x + disp_ + 0.5f) + dd;
          if (d<0 || d>=drange_) 
            continue;
          float er = d - (beta_*x + disp_);
          float w = hist[x*drange_ + d] / (4.0f + er*er);
          sxx += w*x*x;
          sx1 += w*x;
          s11 += w;
          sdx += w*d*x;
          sd1 += w*d;
        }
      }
      float det = sxx*s11 - sx1*sx1;
      beta_ = s11*sdx - sx1*sd1;
      disp_ = sxx*sd1 - sx1*sdx;
      if (det!=0.0f) {
        beta_ /= det;
        disp_ /= det;
      }
    }
    disp_ += 0.5f;

    tmp_beta_x_ = beta_;
    tmp_disp_x_ = disp_;
    
    // Improve plane (depends on both x and y) using m-estimator
    for (int l=0;l<3;l++) {
      float syy = 0.0, sy1 = 0.0f, s11 = 0.0f;
      float sdy = 0.0, sd1 = 0.0f;
      for (int y=0;y<h;y++) {
        for (int x=0;x<w;x++) {
          if (dimd[y*w+x]>0.0f) {
            float d = dimd[y*w+x] - beta_*x;
            float er = d - (alpha_*y + disp_);
            float w = 1.0f / (1.0f + er*er);
            syy += w*y*y;
            sy1 += w*y;
            s11 += w;
            sdy += w*d*y;
            sd1 += w*d;
          }
        }
      }
      float det = syy*s11 - sy1*sy1;
      alpha_ = s11*sdy - sy1*sd1;
      disp_ = syy*sd1 - sy1*sdy;
      if (det!=0.0f) {
        alpha_ /= det;
        disp_ /= det;
      }
    }

    int num=0;
    int numDisp=0;
    float vard = 0.0f;
    for (int y=0;y<h;y++) {
      for (int x=0;x<w;x++) {
        int i = y*w + x;
        float d = dimd[i];
        if (d>=0.0f && d<drange_) {
          numDisp++;
          float er = alpha_*y + beta_*x + disp_ - d;
          if (er*er<4*spread_d_x_) {
            vard += er*er;
            num++;
            labelsd[i] = 255;

            if(y>max_y_)
              max_y_ = y;
            if(y<min_y_)
              min_y_ = y;

            if(x>max_x_)
              max_x_ = x;
            if(x<min_x_)
              min_x_ = x;
          } 
        }        
      }
    }
    spread_d_x_ = (num ? vard/num : 1e-3);
    // sum of squared errors
    err_ = vard;
    // number of inliers
    n_inliers_ = num;

    // number of valid disparities
    n_disps_ = numDisp;

    // switch alpha and beta a for consistency
    float tmp = alpha_;
    alpha_ = beta_;
    beta_ = tmp;

    delete [] hist;
    delete [] totals;
    ROS_INFO("XXX ### Plane: %f %f %f %f", alpha_, beta_, disp_, spread_d_x_);
    ROS_INFO("XXX ### Pixel limits: %d %d %d %d", 
             min_x_, max_x_, min_y_, max_y_);

    labels_.header.stamp = ros::Time::now();
    hist_x_.header.stamp = ros::Time::now();
  }
    
  void PlaneDetection::GetPlaneParameters( float &alpha, 
                                           float &beta, 
                                           float &d,
                                           int &min_x, int &max_x, 
                                           int &min_y, int &max_y)
  {
    alpha = alpha_;
    beta = beta_;
    d = disp_;
        
    min_x = min_x_;
    max_x = max_x_;
    
    min_y = min_y_;
    max_y = max_y_;
  }

  float PlaneDetection::GetSpreadX()
  {
    return spread_d_x_;
  }

  float PlaneDetection::GetSpreadY()
  {
    return spread_d_y_;
  }

  void PlaneDetection::GetLabels(sensor_msgs::Image &image)
  {
    image = labels_;
  }

  void PlaneDetection::GetHistY(sensor_msgs::Image &image,
                                float &beta, float &disp)
  {
    image = hist_y_;
    beta = tmp_beta_;
    disp = tmp_disp_;
  }

  void PlaneDetection::GetHistX(sensor_msgs::Image &image,
                                float &beta, float &disp)
  {
    image = hist_x_;
    beta = tmp_beta_x_;
    disp = tmp_disp_x_;
  }

  void PlaneDetection::GetSumOfSquaredError(float &error)
  {
    error = err_;
  }

  void PlaneDetection::GetNInliers(int &n_inliers)
  {
    n_inliers = n_inliers_;
  }

  void PlaneDetection::GetNDisp(int &n_disp)
  {
    n_disp = n_disps_;
  }

  void PlaneDetection::GetMeanSquaredError(float &mean_error)
  {
    // same as getSpread - but independent of update method (X or Y)
    mean_error = err_/(float)n_inliers_;
  }

} // fast_plane_detection