fast_plane_detection: plane_detection.cpp Source 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 endorse or
00017  *    promote products derived from this software without specific
00018  *    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_detection.h"
00034 #include <sensor_msgs/image_encodings.h>
00035 
00036 namespace enc = sensor_msgs::image_encodings;
00037 
00038 namespace fast_plane_detection {
00039   
00040   PlaneDetection::PlaneDetection( int drange, 
00041                                   int w, int h, 
00042                                   bool find_table,
00043                                   std_msgs::Header img_header)
00044     : drange_(drange)
00045     , width_(w)
00046     , height_(h)
00047     , min_x_(width_)
00048     , max_x_(0)
00049     , min_y_(height_)
00050     , max_y_(0)
00051     , find_table_(find_table)
00052     , spread_d_(1.0f)
00053   {
00054     if(find_table_)
00055       min_beta_ = 0.08f;
00056 
00057     labels_.header = img_header;
00058     labels_.width  = w;
00059     labels_.height = h;
00060     labels_.encoding = enc::MONO8;
00061     labels_.is_bigendian = false;
00062     labels_.step = labels_.width;
00063     size_t size = labels_.step * labels_.height;
00064     labels_.data.resize(size, 0);
00065   }
00066 
00067   PlaneDetection::~PlaneDetection()
00068   {}
00069   
00070   void PlaneDetection::Update(const stereo_msgs::DisparityImage::ConstPtr&
00071                               disparity_image)
00072   {
00073     // reset plane limits
00074     min_x_ = width_;
00075     max_x_ = 0;
00076     min_y_ = height_;
00077     max_y_ = 0;
00078 
00079     float *dimd = (float*)&(disparity_image->image.data[0]);
00080     uint8_t *labelsd = (uint8_t*)&(labels_.data[0]);
00081 
00082     int w = disparity_image->image.width;
00083     int h = disparity_image->image.height;
00084 
00085     // Find dominating disparity for each y-value
00086     int *hist = new int[h*drange_];
00087     int *totals = new int[h];
00088     for (int y=0;y<h;y++) {
00089       int *histy = &hist[y*drange_];
00090       for (int i=0;i<drange_;i++) 
00091         histy[i] = 0;
00092       for (int x=0;x<w;x++) {
00093         int d = (int)dimd[y*w + x];
00094         if (d>=0 && d<drange_)
00095           histy[d] ++;
00096       }
00097       for (int i=0;i<drange_-2;i++)
00098         histy[i] = histy[i] + 2*histy[i+1] + histy[i+2];
00099       for (int i=drange_-1;i>1;i--)
00100         histy[i] = histy[i] + 2*histy[i-1] + histy[i-2];
00101       totals[y] = 0;
00102       for (int i=0;i<drange_;i++)
00103         totals[y] += histy[i];
00104     }
00105     // Find best line using random sampling
00106     float maxwei = 0.0f; 
00107     alpha_ = 0.0f; 
00108     beta_ = 0.0f; 
00109     disp_ = drange_/2;
00110     for (int l=0;l<1000;l++) {
00111       int idx1 = rand()%h;
00112       int idx2 = rand()%h;
00113       while (idx1==idx2)
00114         idx2 = rand()%h;
00115       if (!totals[idx1] || !totals[idx2])
00116         continue;
00117       int cnt1 = rand()%totals[idx1];
00118       int cnt2 = rand()%totals[idx2];
00119       int disp1 = 0, disp2 = 0;
00120       for (int sum1=0;sum1<cnt1;disp1++) 
00121         sum1 += hist[idx1*drange_+disp1];
00122       for (int sum2=0;sum2<cnt2;disp2++) 
00123         sum2 += hist[idx2*drange_+disp2];
00124       disp1--;
00125       disp2--;
00126       float dgra = (float)(disp2 - disp1) / (idx2 - idx1);
00127       float dzer = disp2 - dgra*idx2;
00128       float sumwei = 0.0f;
00129       for (int y=0;y<h;y++) {
00130         for (int dd=-3;dd<=3;dd++) {
00131           int d = (int)(dgra*y + dzer + 0.5f) + dd;
00132           if (d<0 || d>=drange_) 
00133             continue;
00134           float er = d - (dgra*y + dzer);
00135           sumwei += hist[y*drange_ + d] / (4.0f + er*er);
00136         }
00137       }
00138       if (sumwei>maxwei) 
00139         if( !find_table_ || dgra>min_beta_) {
00140           maxwei = sumwei;
00141           beta_ = dgra;
00142           disp_ = dzer;
00143         }
00144     }
00145     // Improve line (depends only on y) using m-estimator
00146     for (int l=0;l<3;l++) {
00147       float syy = 0.0, sy1 = 0.0f, s11 = 0.0f;
00148       float sdy = 0.0, sd1 = 0.0f;
00149       for (int y=0;y<h;y++) {
00150         for (int dd=-3;dd<=3;dd++) {
00151           int d = (int)(beta_*y + disp_ + 0.5f) + dd;
00152           if (d<0 || d>=drange_) 
00153             continue;
00154           float er = d - (beta_*y + disp_);
00155           float w = hist[y*drange_ + d] / (4.0f + er*er);
00156           syy += w*y*y;
00157           sy1 += w*y;
00158           s11 += w;
00159           sdy += w*d*y;
00160           sd1 += w*d;
00161         }
00162       }
00163       float det = syy*s11 - sy1*sy1;
00164       beta_ = s11*sdy - sy1*sd1;
00165       disp_ = syy*sd1 - sy1*sdy;
00166       if (det!=0.0f) {
00167         beta_ /= det;
00168         disp_ /= det;
00169       }
00170     }
00171     disp_ += 0.5f;
00172     // Improve plane (depends on both x and y) using m-estimator
00173     for (int l=0;l<3;l++) {
00174       float sxx = 0.0, sx1 = 0.0f, s11 = 0.0f;
00175       float sdx = 0.0, sd1 = 0.0f;
00176       for (int y=0;y<h;y++) {
00177         for (int x=0;x<w;x++) {
00178           if (dimd[y*w+x]>0.0f) {
00179             float d = dimd[y*w+x] - beta_*y;
00180             float er = d - (alpha_*x + disp_);
00181             float w = 1.0f / (1.0f + er*er);
00182             sxx += w*x*x;
00183             sx1 += w*x;
00184             s11 += w;
00185             sdx += w*d*x;
00186             sd1 += w*d;
00187           }
00188         }
00189       }
00190       float det = sxx*s11 - sx1*sx1;
00191       alpha_ = s11*sdx - sx1*sd1;
00192       disp_ = sxx*sd1 - sx1*sdx;
00193       if (det!=0.0f) {
00194         alpha_ /= det;
00195         disp_ /= det;
00196       }
00197     }
00198 
00199     int num=0;
00200     float vard = 0.0f;
00201     for (int y=0;y<h;y++) {
00202       for (int x=0;x<w;x++) {
00203         int i = y*w + x;
00204         float d = dimd[i];
00205         if (d>=0.0f && d<drange_) {
00206           float er = alpha_*x + beta_*y + disp_ - d;
00207           if (er*er<4*spread_d_) {
00208             vard += er*er;
00209             num++;
00210             labelsd[i] = 255;
00211 
00212             if(y>max_y_)
00213               max_y_ = y;;
00214             if(y<min_y_)
00215               min_y_ = y;
00216 
00217             if(x>max_x_)
00218               max_x_ = x;
00219             if(x<min_x_)
00220               min_x_ = x;
00221           } 
00222         } else {
00223           labelsd[i] = 0;
00224         }
00225       }
00226     }
00227     spread_d_ = (num ? vard/num : 1e-3);
00228     // sum of squared errors
00229     err_ = vard;
00230     // number of inliers
00231     n_inliers_ = num;
00232       
00233     delete [] hist;
00234     delete [] totals;
00235     ROS_DEBUG("### Plane: %f %f %f %f", alpha_, beta_, disp_, spread_d_);
00236     ROS_DEBUG("### Pixel limits: %d %d %d %d", 
00237              min_x_, max_x_, min_y_, max_y_);
00238 
00239     labels_.header.stamp = ros::Time::now();
00240     
00241   }
00242 
00243   void PlaneDetection::GetPlaneParameters( float &alpha, 
00244                                            float &beta, 
00245                                            float &d,
00246                                            int &min_x, int &max_x, 
00247                                            int &min_y, int &max_y)
00248   {
00249     alpha = alpha_;
00250     beta = beta_;
00251     d = disp_;
00252     
00253     min_x = min_x_;
00254     max_x = max_x_;
00255     
00256     min_y = min_y_;
00257     max_y = max_y_;
00258   }
00259 
00260   void PlaneDetection::GetLabels(sensor_msgs::Image &image)
00261   {
00262     image = labels_;
00263   }
00264 
00265   void PlaneDetection::GetSumOfSquaredError(float &error)
00266   {
00267     error = err_;
00268   }
00269 
00270   void PlaneDetection::GetNInliers(int &n_inliers)
00271   {
00272     n_inliers = n_inliers_;
00273   }
00274 
00275   void PlaneDetection::GetMeanSquaredError(float &mean_error)
00276   {
00277     mean_error = err_/(float)n_inliers_;
00278   }
00279   
00280 
00281 } // fast_plane_detection
00282