plane_detection.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 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 #include <ros/console.h>
00036 
00037 namespace enc = sensor_msgs::image_encodings;
00038 
00039 namespace fast_plane_detection {
00040   
00041   PlaneDetection::PlaneDetection( int drange, 
00042                                   int w, int h, 
00043                                   bool find_table,
00044                                   std_msgs::Header img_header,
00045                                   bool search_y)
00046     : drange_(drange)
00047     , width_(w)
00048     , height_(h)
00049     , min_x_(width_)
00050     , max_x_(0)
00051     , min_y_(height_)
00052     , max_y_(0)
00053     , find_table_(find_table)
00054     , search_y_(search_y)
00055     , spread_d_y_(1.0f)
00056     , spread_d_x_(1.0f)
00057   {
00058     if(find_table_)
00059       min_beta_ = 0.08f;
00060 
00061     labels_.header = img_header;
00062     labels_.width  = w;
00063     labels_.height = h;
00064     labels_.encoding = enc::MONO8;
00065     labels_.is_bigendian = false;
00066     labels_.step = labels_.width;
00067     size_t size = labels_.step * labels_.height;
00068     labels_.data.resize(size, 0);
00069 
00070     hist_y_.header = img_header;
00071     hist_y_.width  = drange_;
00072     hist_y_.height = h;
00073     hist_y_.encoding = enc::MONO8;
00074     hist_y_.is_bigendian = false;
00075     hist_y_.step = hist_y_.width;
00076     size = hist_y_.step * hist_y_.height;
00077     hist_y_.data.resize(size, 0);
00078 
00079     
00080     hist_x_.header = img_header;
00081     hist_x_.width  = drange_;
00082     hist_x_.height = w;
00083     hist_x_.encoding = enc::MONO8;
00084     hist_x_.is_bigendian = false;
00085     hist_x_.step = hist_x_.width;
00086     size = hist_x_.step * hist_x_.height;
00087     hist_x_.data.resize(size, 0);
00088 
00089   }
00090 
00091   PlaneDetection::~PlaneDetection()
00092   {}
00093   
00094   void PlaneDetection::Update(const stereo_msgs::DisparityImage::ConstPtr&
00095                               disparity_image)
00096   {
00097     if(search_y_)
00098       Update_Y( disparity_image );
00099     else 
00100       Update_X( disparity_image );
00101   }
00102 
00103   void PlaneDetection::Update_Y(const stereo_msgs::DisparityImage::ConstPtr&
00104                                 disparity_image)
00105   {
00106     // reset plane limits
00107     min_x_ = width_;
00108     max_x_ = 0;
00109     min_y_ = height_;
00110     max_y_ = 0;
00111 
00112     float *dimd = (float*)&(disparity_image->image.data[0]);
00113     uint8_t *labelsd = (uint8_t*)&(labels_.data[0]);
00114     uint8_t *histd = (uint8_t*)&(hist_y_.data[0]);
00115 
00116     int w = disparity_image->image.width;
00117     int h = disparity_image->image.height;
00118     
00119     for (int y=0;y<h;y++) {
00120       for (int x=0;x<w;x++) {
00121         int i = y*w + x;
00122         labelsd[i]=0;
00123       }
00124     }
00125 
00126     // Find dominating disparity for each y-value
00127     int *hist = new int[h*drange_];
00128     int *totals = new int[h];
00129     for (int y=0;y<h;y++) {
00130       int *histy = &hist[y*drange_];
00131       uint8_t *histdy = &histd[y*drange_];
00132       for (int i=0;i<drange_;i++){ 
00133         histy[i] = 0;
00134         histdy[i] = 0;
00135       }
00136       for (int x=0;x<w;x++) {
00137         int d = (int)dimd[y*w + x];
00138         if (d>=0 && d<drange_){
00139           histy[d] ++;
00140           histdy[d] ++;
00141         }
00142       }
00143       for (int i=0;i<drange_-2;i++){
00144         histy[i] = histy[i] + 2*histy[i+1] + histy[i+2];
00145         histdy[i] = histdy[i] + 2*histdy[i+1] + histdy[i+2];
00146       }
00147       for (int i=drange_-1;i>1;i--){
00148         histy[i] = histy[i] + 2*histy[i-1] + histy[i-2];
00149         histdy[i] = histdy[i] + 2*histdy[i-1] + histdy[i-2];
00150       }
00151       totals[y] = 0;
00152       for (int i=0;i<drange_;i++)
00153         totals[y] += histy[i];
00154     }
00155 
00156     
00157 
00158     // Find best line using random sampling
00159     float maxwei = 0.0f; 
00160     alpha_ = 0.0f; 
00161     beta_ = 0.0f; 
00162     disp_ = drange_/2;
00163     for (int l=0;l<1000;l++) {
00164       int idx1 = rand()%h;
00165       int idx2 = rand()%h;
00166       while (idx1==idx2)
00167         idx2 = rand()%h;
00168       if (!totals[idx1] || !totals[idx2])
00169         continue;
00170       int cnt1 = rand()%totals[idx1];
00171       int cnt2 = rand()%totals[idx2];
00172       int disp1 = 0, disp2 = 0;
00173       for (int sum1=0;sum1<cnt1;disp1++) 
00174         sum1 += hist[idx1*drange_+disp1];
00175       for (int sum2=0;sum2<cnt2;disp2++) 
00176         sum2 += hist[idx2*drange_+disp2];
00177       disp1--;
00178       disp2--;
00179       float dgra = (float)(disp2 - disp1) / (idx2 - idx1);
00180       float dzer = disp2 - dgra*idx2;
00181       float sumwei = 0.0f;
00182       for (int y=0;y<h;y++) {
00183         for (int dd=-3;dd<=3;dd++) {
00184           int d = (int)(dgra*y + dzer + 0.5f) + dd;
00185           if (d<0 || d>=drange_) 
00186             continue;
00187           float er = d - (dgra*y + dzer);
00188           sumwei += hist[y*drange_ + d] / (4.0f + er*er);
00189         }
00190       }
00191       if (sumwei>maxwei) 
00192         if( !find_table_ || dgra>min_beta_) {
00193           maxwei = sumwei;
00194           beta_ = dgra;
00195           disp_ = dzer;
00196         }
00197     }
00198     
00199     // tmp_beta_ = beta_;
00200     //tmp_disp_ = disp_;
00201 
00202     //    ROS_WARN("beta_ %f, disp_ %f", beta_, disp_);
00203 
00204     // Improve line (depends only on y) using m-estimator
00205     for (int l=0;l<3;l++) {
00206       float syy = 0.0, sy1 = 0.0f, s11 = 0.0f;
00207       float sdy = 0.0, sd1 = 0.0f;
00208       for (int y=0;y<h;y++) {
00209         for (int dd=-3;dd<=3;dd++) {
00210           int d = (int)(beta_*y + disp_ + 0.5f) + dd;
00211           if (d<0 || d>=drange_) 
00212             continue;
00213           float er = d - (beta_*y + disp_);
00214           float w = hist[y*drange_ + d] / (4.0f + er*er);
00215           syy += w*y*y;
00216           sy1 += w*y;
00217           s11 += w;
00218           sdy += w*d*y;
00219           sd1 += w*d;
00220         }
00221       }
00222       float det = syy*s11 - sy1*sy1;
00223       beta_ = s11*sdy - sy1*sd1;
00224       disp_ = syy*sd1 - sy1*sdy;
00225       if (det!=0.0f) {
00226         beta_ /= det;
00227         disp_ /= det;
00228       }
00229     }
00230     disp_ += 0.5f;
00231 
00232     tmp_beta_ = beta_;
00233     tmp_disp_ = disp_;
00234 
00235     // Improve plane (depends on both x and y) using m-estimator
00236     for (int l=0;l<3;l++) {
00237       float sxx = 0.0, sx1 = 0.0f, s11 = 0.0f;
00238       float sdx = 0.0, sd1 = 0.0f;
00239       for (int y=0;y<h;y++) {
00240         for (int x=0;x<w;x++) {
00241           if (dimd[y*w+x]>0.0f) {
00242             float d = dimd[y*w+x] - beta_*y;
00243             float er = d - (alpha_*x + disp_);
00244             float w = 1.0f / (1.0f + er*er);
00245             sxx += w*x*x;
00246             sx1 += w*x;
00247             s11 += w;
00248             sdx += w*d*x;
00249             sd1 += w*d;
00250           }
00251         }
00252       }
00253       float det = sxx*s11 - sx1*sx1;
00254       alpha_ = s11*sdx - sx1*sd1;
00255       disp_ = sxx*sd1 - sx1*sdx;
00256       if (det!=0.0f) {
00257         alpha_ /= det;
00258         disp_ /= det;
00259       }
00260     }
00261 
00262     int num=0;
00263     int numDisp=0;
00264     float vard = 0.0f;
00265     for (int y=0;y<h;y++) {
00266       for (int x=0;x<w;x++) {
00267         int i = y*w + x;
00268         float d = dimd[i];
00269         if (d>=0.0f && d<drange_) {
00270           numDisp++;
00271           float er = alpha_*x + beta_*y + disp_ - d;
00272           if (er*er<4*spread_d_y_) {
00273             vard += er*er;
00274             num++;
00275             labelsd[i] = 255;
00276 
00277             if(y>max_y_)
00278               max_y_ = y;;
00279             if(y<min_y_)
00280               min_y_ = y;
00281 
00282             if(x>max_x_)
00283               max_x_ = x;
00284             if(x<min_x_)
00285               min_x_ = x;
00286           } 
00287         }        
00288       }
00289     }
00290     spread_d_y_ = (num ? vard/num : 1e-3);
00291     // sum of squared errors
00292     err_ = vard;
00293     // number of inliers
00294     n_inliers_ = num;
00295 
00296     // number of valid disparities
00297     n_disps_ = numDisp;
00298  
00299     delete [] hist;
00300     delete [] totals;
00301     ROS_DEBUG("YYY ### Plane: %f %f %f %f", 
00302               alpha_, beta_, disp_, spread_d_y_);
00303     ROS_DEBUG("YYY ### Pixel limits: %d %d %d %d", 
00304              min_x_, max_x_, min_y_, max_y_);
00305 
00306     labels_.header.stamp = ros::Time::now();
00307     hist_y_.header.stamp = ros::Time::now();
00308     
00309   }
00310 
00311 
00312   void PlaneDetection::Update_X(const stereo_msgs::DisparityImage::ConstPtr&
00313                                 disparity_image)
00314   {
00315     // reset plane limits
00316     min_x_ = width_;
00317     max_x_ = 0;
00318     min_y_ = height_;
00319     max_y_ = 0;
00320 
00321     float *dimd = (float*)&(disparity_image->image.data[0]);
00322     uint8_t *labelsd = (uint8_t*)&(labels_.data[0]);
00323     uint8_t *histd = (uint8_t*)&(hist_x_.data[0]);
00324 
00325     int w = disparity_image->image.width;
00326     int h = disparity_image->image.height;
00327 
00328     for (int y=0;y<h;y++) {
00329       for (int x=0;x<w;x++) {
00330         int i = y*w + x;
00331         labelsd[i]=0;
00332       }
00333     }
00334 
00335     // Find dominating disparity for each x-value
00336     int *hist = new int[w*drange_];
00337     int *totals = new int[w];
00338     for (int x=0;x<w;x++) {
00339       int *histx = &hist[x*drange_];
00340       uint8_t *histdx = &histd[x*drange_];
00341       for (int i=0;i<drange_;i++){ 
00342         histx[i] = 0;
00343         histdx[i] = 0;
00344       }
00345       for (int y=0;y<h;y++) {
00346         int d = (int)dimd[y*w + x];
00347         if (d>=0 && d<drange_){
00348           histx[d] ++;
00349           histdx[d] ++;
00350         }
00351       }
00352       for (int i=0;i<drange_-2;i++){
00353         histx[i] = histx[i] + 2*histx[i+1] + histx[i+2];
00354         histdx[i] = histdx[i] + 2*histdx[i+1] + histdx[i+2];
00355       }
00356       for (int i=drange_-1;i>1;i--){
00357         histx[i] = histx[i] + 2*histx[i-1] + histx[i-2];
00358         histdx[i] = histdx[i] + 2*histdx[i-1] + histdx[i-2];
00359       }
00360       totals[x] = 0;
00361       for (int i=0;i<drange_;i++)
00362         totals[x] += histx[i];
00363     }
00364 
00365     
00366 
00367     // Find best line using random sampling
00368     float maxwei = 0.0f; 
00369     alpha_ = 0.0f; 
00370     beta_ = 0.0f; 
00371     disp_ = drange_/2;
00372     for (int l=0;l<1000;l++) {
00373       int idx1 = rand()%h;
00374       int idx2 = rand()%h;
00375       while (idx1==idx2)
00376         idx2 = rand()%h;
00377       if (!totals[idx1] || !totals[idx2])
00378         continue;
00379       int cnt1 = rand()%totals[idx1];
00380       int cnt2 = rand()%totals[idx2];
00381       int disp1 = 0, disp2 = 0;
00382       for (int sum1=0;sum1<cnt1;disp1++) 
00383         sum1 += hist[idx1*drange_+disp1];
00384       for (int sum2=0;sum2<cnt2;disp2++) 
00385         sum2 += hist[idx2*drange_+disp2];
00386       disp1--;
00387       disp2--;
00388       float dgra = (float)(disp2 - disp1) / (idx2 - idx1);
00389       float dzer = disp2 - dgra*idx2;
00390       float sumwei = 0.0f;
00391       for (int y=0;y<h;y++) {
00392         for (int dd=-3;dd<=3;dd++) {
00393           int d = (int)(dgra*y + dzer + 0.5f) + dd;
00394           if (d<0 || d>=drange_) 
00395             continue;
00396           float er = d - (dgra*y + dzer);
00397           sumwei += hist[y*drange_ + d] / (4.0f + er*er);
00398         }
00399       }
00400       if (sumwei>maxwei) 
00401         if( !find_table_ || dgra>min_beta_) {
00402           maxwei = sumwei;
00403           beta_ = dgra;
00404           disp_ = dzer;
00405         }
00406     }
00407     
00408     //tmp_beta_x_ = beta_;
00409     //tmp_disp_x_ = disp_;
00410 
00411     
00412     //    ROS_WARN("beta_ %f, disp_ %f", beta_, disp_);
00413 
00414     // Improve line (depends only on y) using m-estimator
00415     for (int l=0;l<3;l++) {
00416       float sxx = 0.0, sx1 = 0.0f, s11 = 0.0f;
00417       float sdx = 0.0, sd1 = 0.0f;
00418       for (int x=0;x<w;x++) {
00419         for (int dd=-3;dd<=3;dd++) {
00420           int d = (int)(beta_*x + disp_ + 0.5f) + dd;
00421           if (d<0 || d>=drange_) 
00422             continue;
00423           float er = d - (beta_*x + disp_);
00424           float w = hist[x*drange_ + d] / (4.0f + er*er);
00425           sxx += w*x*x;
00426           sx1 += w*x;
00427           s11 += w;
00428           sdx += w*d*x;
00429           sd1 += w*d;
00430         }
00431       }
00432       float det = sxx*s11 - sx1*sx1;
00433       beta_ = s11*sdx - sx1*sd1;
00434       disp_ = sxx*sd1 - sx1*sdx;
00435       if (det!=0.0f) {
00436         beta_ /= det;
00437         disp_ /= det;
00438       }
00439     }
00440     disp_ += 0.5f;
00441 
00442     tmp_beta_x_ = beta_;
00443     tmp_disp_x_ = disp_;
00444     
00445     // Improve plane (depends on both x and y) using m-estimator
00446     for (int l=0;l<3;l++) {
00447       float syy = 0.0, sy1 = 0.0f, s11 = 0.0f;
00448       float sdy = 0.0, sd1 = 0.0f;
00449       for (int y=0;y<h;y++) {
00450         for (int x=0;x<w;x++) {
00451           if (dimd[y*w+x]>0.0f) {
00452             float d = dimd[y*w+x] - beta_*x;
00453             float er = d - (alpha_*y + disp_);
00454             float w = 1.0f / (1.0f + er*er);
00455             syy += w*y*y;
00456             sy1 += w*y;
00457             s11 += w;
00458             sdy += w*d*y;
00459             sd1 += w*d;
00460           }
00461         }
00462       }
00463       float det = syy*s11 - sy1*sy1;
00464       alpha_ = s11*sdy - sy1*sd1;
00465       disp_ = syy*sd1 - sy1*sdy;
00466       if (det!=0.0f) {
00467         alpha_ /= det;
00468         disp_ /= det;
00469       }
00470     }
00471 
00472     int num=0;
00473     int numDisp=0;
00474     float vard = 0.0f;
00475     for (int y=0;y<h;y++) {
00476       for (int x=0;x<w;x++) {
00477         int i = y*w + x;
00478         float d = dimd[i];
00479         if (d>=0.0f && d<drange_) {
00480           numDisp++;
00481           float er = alpha_*y + beta_*x + disp_ - d;
00482           if (er*er<4*spread_d_x_) {
00483             vard += er*er;
00484             num++;
00485             labelsd[i] = 255;
00486 
00487             if(y>max_y_)
00488               max_y_ = y;
00489             if(y<min_y_)
00490               min_y_ = y;
00491 
00492             if(x>max_x_)
00493               max_x_ = x;
00494             if(x<min_x_)
00495               min_x_ = x;
00496           } 
00497         }        
00498       }
00499     }
00500     spread_d_x_ = (num ? vard/num : 1e-3);
00501     // sum of squared errors
00502     err_ = vard;
00503     // number of inliers
00504     n_inliers_ = num;
00505 
00506     // number of valid disparities
00507     n_disps_ = numDisp;
00508 
00509     // switch alpha and beta a for consistency
00510     float tmp = alpha_;
00511     alpha_ = beta_;
00512     beta_ = tmp;
00513 
00514     delete [] hist;
00515     delete [] totals;
00516     ROS_INFO("XXX ### Plane: %f %f %f %f", alpha_, beta_, disp_, spread_d_x_);
00517     ROS_INFO("XXX ### Pixel limits: %d %d %d %d", 
00518              min_x_, max_x_, min_y_, max_y_);
00519 
00520     labels_.header.stamp = ros::Time::now();
00521     hist_x_.header.stamp = ros::Time::now();
00522   }
00523     
00524   void PlaneDetection::GetPlaneParameters( float &alpha, 
00525                                            float &beta, 
00526                                            float &d,
00527                                            int &min_x, int &max_x, 
00528                                            int &min_y, int &max_y)
00529   {
00530     alpha = alpha_;
00531     beta = beta_;
00532     d = disp_;
00533         
00534     min_x = min_x_;
00535     max_x = max_x_;
00536     
00537     min_y = min_y_;
00538     max_y = max_y_;
00539   }
00540 
00541   float PlaneDetection::GetSpreadX()
00542   {
00543     return spread_d_x_;
00544   }
00545 
00546   float PlaneDetection::GetSpreadY()
00547   {
00548     return spread_d_y_;
00549   }
00550 
00551   void PlaneDetection::GetLabels(sensor_msgs::Image &image)
00552   {
00553     image = labels_;
00554   }
00555 
00556   void PlaneDetection::GetHistY(sensor_msgs::Image &image,
00557                                 float &beta, float &disp)
00558   {
00559     image = hist_y_;
00560     beta = tmp_beta_;
00561     disp = tmp_disp_;
00562   }
00563 
00564   void PlaneDetection::GetHistX(sensor_msgs::Image &image,
00565                                 float &beta, float &disp)
00566   {
00567     image = hist_x_;
00568     beta = tmp_beta_x_;
00569     disp = tmp_disp_x_;
00570   }
00571 
00572   void PlaneDetection::GetSumOfSquaredError(float &error)
00573   {
00574     error = err_;
00575   }
00576 
00577   void PlaneDetection::GetNInliers(int &n_inliers)
00578   {
00579     n_inliers = n_inliers_;
00580   }
00581 
00582   void PlaneDetection::GetNDisp(int &n_disp)
00583   {
00584     n_disp = n_disps_;
00585   }
00586 
00587   void PlaneDetection::GetMeanSquaredError(float &mean_error)
00588   {
00589     // same as getSpread - but independent of update method (X or Y)
00590     mean_error = err_/(float)n_inliers_;
00591   }
00592 
00593 } // fast_plane_detection
00594 


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