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