$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: filtering.cpp 694 2012-04-20 10:24:24Z ihulik $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Rostislav Hulik (ihulik@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 11.01.2012 (version 0.8) 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00033 #include <srs_env_model_percp/but_segmentation/filtering.h> 00034 00035 // Open CV 00036 #include <cv.h> 00037 00038 // ROS 00039 #include <ros/ros.h> 00040 #include <omp.h> 00041 // Eigen 00042 #include <Eigen/Core> 00043 #include <Eigen/Eigenvalues> 00044 00045 using namespace sensor_msgs; 00046 using namespace cv; 00047 00048 namespace but_plane_detector { 00049 00051 // A class providing an interface to depth image segmenter 00052 // @param normals Normals object with precomputed real points and normals - not necessary - only if you do not want to compute it in this module (for speed reasons) 00053 // @see Normals() 00055 Regions::Regions(Normals *normals) 00056 { 00057 m_normals = normals; 00058 } 00059 00061 // Destructor - obviously, it destructs this class 00063 Regions::~Regions() 00064 { 00065 //if (m_normals != NULL) 00066 // free(m_normals); 00067 } 00068 00070 // Method computes from m_regionMatrix statistical information (fills m_planes and m_stddeviation matrices) 00071 // @param threshold Not used now (TODO) 00073 bool Regions::computeStatistics(float threshold) 00074 { 00075 if (m_normals == NULL) return false; 00076 00077 std::vector<Vec4f> normals; 00078 std::vector<int> sizes; 00079 Vec4f nullVector = Vec4f(0.0, 0.0, 0.0, 0.0); 00080 Vec4f auxVector; 00081 int aux; 00082 00083 for (int i = 0; i < m_regionMatrix.rows; ++i) 00084 for (int j = 0; j < m_regionMatrix.cols; ++j) 00085 { 00086 auxVector = m_normals->m_planes.at<Vec4f>(i, j); 00087 if (auxVector != nullVector) 00088 { 00089 aux = m_regionMatrix.at<int>(i, j); 00090 if (aux >= 0) 00091 { 00092 if (aux >= (int)normals.size()) 00093 { 00094 sizes.resize(aux+1, 0); 00095 normals.resize(aux+1, Vec4f(0.0, 0.0, 0.0, 0.0)); 00096 } 00097 sizes[aux] += 1; 00098 normals[aux] += auxVector; 00099 } 00100 00101 } 00102 else 00103 { 00104 m_regionMatrix.at<int>(i, j) = -1; 00105 } 00106 } 00107 00108 for (unsigned int i = 0; i < normals.size(); ++i) 00109 { 00110 normals[i][0] /= sizes[i]; 00111 normals[i][1] /= sizes[i]; 00112 normals[i][2] /= sizes[i]; 00113 normals[i][3] /= sizes[i]; 00114 } 00115 00116 std::vector<float> deviations(normals.size()); 00117 std::vector<float> means(normals.size()); 00118 00119 for (int i = 0; i < m_regionMatrix.rows; ++i) 00120 for (int j = 0; j < m_regionMatrix.cols; ++j) 00121 { 00122 aux = m_regionMatrix.at<int>(i, j); 00123 if (aux >= 0) 00124 { 00125 auxVector = m_normals->m_planes.at<Vec4f>(i, j); 00126 float angle = acos(normals[aux][0]*auxVector[0]+normals[aux][1]*auxVector[1]+normals[aux][2]*auxVector[2]); 00127 means[aux] += angle*angle; 00128 } 00129 } 00130 00131 for (unsigned int i = 0; i < normals.size(); ++i) 00132 { 00133 means[i] /= sizes[i]; 00134 means[i] = sqrt(means[i]); 00135 } 00136 00137 m_planes = Mat::zeros(m_regionMatrix.size(), CV_32FC4); 00138 m_stddeviation = Mat::zeros(m_regionMatrix.size(), CV_32FC1); 00139 00140 for (int i = 0; i < m_regionMatrix.rows; ++i) 00141 for (int j = 0; j < m_regionMatrix.cols; ++j) 00142 { 00143 if (m_regionMatrix.at<int>(i, j) >= 0) 00144 { 00145 m_stddeviation.at<float>(i, j) = means[m_regionMatrix.at<int>(i, j)]; 00146 m_planes.at<Vec4f>(i, j) = normals[m_regionMatrix.at<int>(i, j)]; 00147 } 00148 } 00149 return true; 00150 } 00151 00153 // Method which segments a depth image with watershed algorithm. There is several approaches, so please be aware of input parameters 00154 // @param src Input CV_16UC depth matrix (raw input from kinect) 00155 // @param cam_info Camera info message (ROS) 00156 // @param type Type of watershed input - one of WatershedType constants 00157 // @see WatershedType 00158 // @param seedThreshold Threshold which specifies number of anomal neighbors for selecting this pixel as seed for watershed 00159 // @param alpha It is always size of neigborhood which is considered for searching the image in watershed preprocessing. If 0.0, default is selected. 00160 // @param beta When simple preprocessing is selected (depth, normal, predictor), it is always a threshold for selecting anomal neighbors (in depth is in milimeters, normal is in radians ). If combined type is selected, it represents a depth difference. 00161 // @param gamma It is used only in combined and predictor. In combined, it specifiesd normal difference threshold, in predictor a sobel size. 00162 // @see Normals() 00164 void Regions::watershedRegions(Mat &src, const CameraInfoConstPtr& cam_info, 00165 int type, int seedThreshold, 00166 float alpha, float beta, float gamma 00167 ) 00168 { 00169 ros::Time begin = ros::Time::now(); 00170 Mat output1 = src;; 00171 Mat output2; 00172 00173 Mat markers = Mat::zeros(src.size(), CV_32FC1); 00174 Mat inputWatershed = Mat::zeros(src.size(), CV_8UC3); 00175 00176 00177 unsigned short val; 00178 if (type & WatershedType::DepthDiff) 00179 { 00180 gradientDepthDifference(src, output1, alpha, beta); 00181 for (int i = 0; i < output1.rows; ++i) 00182 for (int j = 0; j < output1.cols; ++j) 00183 { 00184 val = output1.at<unsigned short>(i, j); 00185 if (val > 255) val = 255; 00186 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[0] = (unsigned char)val; 00187 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[1] = (unsigned char)val; 00188 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[2] = (unsigned char)val; 00189 if (val >= seedThreshold) 00190 markers.at<float>(i, j) = 1; 00191 } 00192 } 00193 else if (type & WatershedType::NormalDiff) 00194 { 00195 if (m_normals == NULL) 00196 m_normals = new Normals(src, cam_info, NORMAL_COMPUTATION_TYPE, NORMAL_COMPUTATION_SIZE); 00197 //std::cout << "Normal computation time: " << (ros::Time::now() - begin).nsec / 1000000 << std::endl; 00198 //begin = ros::Time::now(); 00199 gradientNormalDifference(m_normals->m_planes, output1, alpha, beta); 00200 //std::cout << "Gradient computation time: " << (ros::Time::now() - begin).nsec / 1000000 << std::endl; 00201 //begin = ros::Time::now(); 00202 #pragma omp parallel for 00203 for (int i = 0; i < output1.rows; ++i) 00204 for (int j = 0; j < output1.cols; ++j) 00205 { 00206 val = output1.at<unsigned short>(i, j); 00207 if (val > 255) val = 255; 00208 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[0] = (unsigned char)val; 00209 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[1] = (unsigned char)val; 00210 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[2] = (unsigned char)val; 00211 if (val >= seedThreshold) 00212 markers.at<float>(i, j) = 1; 00213 } 00214 //std::cout << "Watershed computation time: " << (ros::Time::now() - begin).nsec / 1000000 << std::endl; 00215 } 00216 else if (type & WatershedType::Combined) 00217 { 00218 if (m_normals == NULL) 00219 m_normals = new Normals(src, cam_info, NORMAL_COMPUTATION_TYPE, NORMAL_COMPUTATION_SIZE); 00220 gradientNormalDifference(m_normals->m_planes, output1, alpha, gamma); 00221 gradientDepthDifference(src, output2, alpha, beta); 00222 #pragma omp parallel for 00223 for (int i = 0; i < output1.rows; ++i) 00224 for (int j = 0; j < output1.cols; ++j) 00225 { 00226 val = output1.at<unsigned short>(i, j) + output2.at<unsigned short>(i, j); 00227 if (val > 255) val = 255; 00228 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[0] = (unsigned char)val; 00229 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[1] = (unsigned char)val; 00230 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[2] = (unsigned char)val; 00231 if (val >= seedThreshold) 00232 markers.at<float>(i, j) = 1; 00233 } 00234 } 00235 else if (type & WatershedType::PredictorDiff) 00236 { 00237 gradientPlanePredictor(src, output1, beta, alpha, gamma); 00238 #pragma omp parallel for 00239 for (int i = 0; i < output1.rows; ++i) 00240 for (int j = 0; j < output1.cols; ++j) 00241 { 00242 val = output1.at<unsigned short>(i, j); 00243 if (val > 255) val = 255; 00244 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[0] = (unsigned char)val; 00245 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[1] = (unsigned char)val; 00246 inputWatershed.at<Vec<unsigned char, 3> >(i, j)[2] = (unsigned char)val; 00247 if (val >= seedThreshold) 00248 markers.at<float>(i, j) = 1; 00249 } 00250 } 00251 00252 float aux; 00253 unsigned int index = 2; 00254 for (int i = 0; i < markers.rows; ++i) 00255 for (int j = 0; j < markers.cols; ++j) 00256 { 00257 aux = markers.at<float>(i, j); 00258 if (aux < 1) 00259 { 00260 if (floodFill(markers, Point(j, i), Scalar(index)) < 100) 00261 floodFill(markers, Point(j, i), Scalar(1)); 00262 else 00263 ++index; 00264 } 00265 } 00266 00267 00268 for (int i = 0; i < markers.rows; ++i) 00269 for (int j = 0; j < markers.cols; ++j) 00270 { 00271 if (markers.at<float>(i, j) == 1) 00272 markers.at<float>(i, j) = 0; 00273 } 00274 00275 markers.convertTo(output1, CV_32SC1); 00276 watershed(inputWatershed, output1); 00277 00278 output1.convertTo(m_regionMatrix, CV_32SC1); 00279 } 00280 00282 // Method converts an input image into gradient image using comparison of depth data 00283 // @param src Input CV_16UC depth matrix (raw input from kinect) 00284 // @param dst Final gradient image 00285 // @param size Size of neigborhood (number of maximal pixel distance) 00286 // @param differenceThreshold Difference from center depth to mark a pixel as anomal 00288 void Regions::gradientDepthDifference(Mat &src, Mat& dst, int size, float differenceThreshold) 00289 { 00290 Mat input(src.size(), CV_32FC1); 00291 Mat output = Mat::ones(src.size(), CV_32FC1); 00292 00293 src.convertTo(input, CV_32F); 00294 00295 #pragma omp parallel for 00296 for (int i = size; i < src.rows-size; ++i) 00297 for (int j = size; j < src.cols-size; ++j) 00298 { 00299 int count = 0; 00300 float center = input.at<float>(i, j); 00301 for (int x = i-size; x < i+size; ++x) 00302 for (int y = j-size; y < j+size; ++y) 00303 { 00304 if (abs(input.at<float>(x, y) - center) > differenceThreshold) 00305 count++; 00306 } 00307 output.at<float>(i, j) = count; 00308 } 00309 output.convertTo(dst, CV_16U); 00310 } 00311 00313 // Method converts an input image into gradient image using comparison of normals 00314 // @param src Input CV_16UC depth matrix (raw input from kinect) 00315 // @param dst Final gradient image 00316 // @param size Size of neigborhood (number of maximal pixel distance) 00317 // @param differenceThreshold Difference from center normal to mark a pixel as anomal 00319 void Regions::gradientNormalDifference(Mat &src, Mat& dst, int size, float differenceThreshold) 00320 { 00321 Mat output = Mat::zeros(src.size(), CV_32FC1); 00322 00323 float minangle = differenceThreshold; 00324 float maxangle = M_PI - differenceThreshold; 00325 Vec4f aux; 00326 00327 #pragma omp parallel for 00328 for (int i = size; i < src.rows-size; ++i) 00329 for (int j = size; j < src.cols-size; ++j) 00330 { 00331 Vec4f center = src.at<Vec4f>(i, j); 00332 int count = 0; 00333 00334 if (center == Vec4f(0.0, 0.0, 0.0, 0.0)) 00335 { 00336 output.at<float>(i, j) = 0; 00337 continue; 00338 } 00339 00340 for (int x = i-size; x < i+size; ++x) 00341 for (int y = j-size; y < j+size; ++y) 00342 { 00343 aux = src.at<Vec4f>(x, y); 00344 00345 float angle = acos(aux[0]*center[0]+aux[1]*center[1]+aux[2]*center[2]); 00346 if (angle > minangle && angle < maxangle) 00347 count++; 00348 } 00349 output.at<float>(i, j) = count; 00350 } 00351 output.convertTo(dst, CV_16U); 00352 } 00353 00355 // Method converts an input image into gradient image using expected plane prediction. Pixel value is number of pixels around which are not in difference threshold from expected plane. 00356 // @param src Input CV_16UC depth matrix (raw input from kinect) 00357 // @param dst Final gradient image 00358 // @param differenceThreshold Difference from expected plane threshold to mark a pixel as anomal 00359 // @param size Size of neigborhood (number of maximal pixel distance) 00360 // @param sobelSize Size of sobel kernel used for gradient image computation 00362 void Regions::gradientPlanePredictor(Mat &src, Mat& dst, float differenceThreshold, int size, int sobelSize)//, Mat& normals, Mat& coefs, Mat& points) 00363 { 00364 Mat input(src.size(), CV_32FC1); 00365 Mat output(src.size(), CV_32FC1); 00366 Mat output2(src.size(), CV_32FC1); 00367 00368 //GaussianBlur(normals, normals, cvSize(3,3), 3); 00369 src.convertTo(input, CV_32F); 00370 00371 int size2 = size+size+1; 00372 int sizesq = size2 * size2; 00373 00374 input.copyTo(output); 00375 00377 // normalized sobel 00378 // get dx, dy 00379 Mat dx(src.size(), CV_32FC1); 00380 Mat dy(src.size(), CV_32FC1); 00381 00382 Mat kx(cvSize(sobelSize,sobelSize), CV_32FC1); 00383 Mat ky(cvSize(sobelSize,sobelSize), CV_32FC1); 00384 00385 getDerivKernels(kx, ky, 1, 1, sobelSize, true, CV_32F); 00386 filter2D(output, dx, CV_32F, kx); 00387 filter2D(output, dy, CV_32F, ky); 00388 00389 Vec3f centerNormal; 00390 Vec3f nowNormal; 00391 00392 // null outputs 00393 output = Mat::zeros(output.size(), CV_32FC1); 00394 output2 = Mat::zeros(output.size(), CV_32FC1); 00395 00396 float xNow, yNow, xCenter, yCenter, dist, theoreticalDepth, realDepth, centerDepth, difference, magVec, d; 00397 00398 // for each 00399 Vec3f z(0,0,1); 00400 00401 #pragma omp parallel for 00402 for (int i = size; i < src.rows-size; ++i) 00403 for (int j = size; j < src.cols-size; ++j) 00404 { 00405 centerDepth = input.at<float>(i, j); 00406 if (centerDepth != 0.0) 00407 { 00408 // get center 00409 xCenter = dx.at<float>(i, j); 00410 yCenter = dy.at<float>(i, j); 00411 00412 dist = 0.0; 00413 00414 00415 // compute gradient magnitude 00416 if (xCenter == 0 && yCenter == 0) 00417 magVec = 0; 00418 else 00419 magVec = sqrt(xCenter*xCenter + yCenter*yCenter); 00420 00421 int ones = 0; 00422 int last = 0; 00423 int changes = 0; 00424 00425 // neighbourhood 00426 int x = -size; 00427 int y = -size; 00428 int plusX = 1; 00429 int plusY = 0; 00430 Mat diffs(cvSize(size+size+1, size+size+1), CV_8UC1); 00431 00432 float vals[sizesq]; 00433 int index = 0; 00434 changes = 0; 00435 00436 // For each in points neighbourhood 00437 for (int cnt = 0; cnt <= size*8; ++cnt) 00438 { 00439 xNow = dx.at<float>(i+x, j+y); 00440 yNow = dy.at<float>(i+x, j+y); 00441 00442 // compute how much of points in neighbourhood are in predicted depth 00443 realDepth = input.at<float>(i+x, j+y); 00444 vals[index] = acos(z.dot(nowNormal)); 00445 ++index; 00446 00447 d = (xCenter*x + yCenter*y); 00448 theoreticalDepth = d + centerDepth; 00449 difference = abs(theoreticalDepth - realDepth) / SHRT_MAX; 00450 00451 if (cnt != 0) 00452 { 00453 // threshold difference - first trait 00454 if (difference > differenceThreshold) 00455 { 00456 ones+=1; 00457 if (last == 0) 00458 { 00459 last = 1; 00460 changes++; 00461 } 00462 } 00463 else 00464 if (last == 1) 00465 { 00466 last = 0; 00467 changes++; 00468 } 00469 } 00470 else 00471 { 00472 if (difference > differenceThreshold) 00473 last = 1; 00474 else 00475 last = 0; 00476 } 00477 00478 dist += abs(realDepth-centerDepth); 00479 00480 // move next pixel around 00481 if (x == -size && y == -size) 00482 { 00483 plusX = 1; 00484 plusY = 0; 00485 } 00486 if (x == size && y == -size) 00487 { 00488 plusX = 0; 00489 plusY = 1; 00490 } 00491 if (x == size && y == size) 00492 { 00493 plusX = -1; 00494 plusY = 0; 00495 } 00496 if (x == -size && y == size) 00497 { 00498 plusX = 0; 00499 plusY = -1; 00500 } 00501 x += plusX; 00502 y += plusY; 00503 } 00504 dist /= size * 8; 00505 dist /= SHRT_MAX; 00506 00507 float ch = ((cos((float)changes * ((2.0*M_PI)/(size*8))) +1.0) /2.0) * ones; 00508 00509 output2.at<float>(i,j) = ch; 00510 output.at<float>(i, j) = dist; 00511 } 00512 } 00513 00514 // normalize the output 00515 double min, max; 00516 Point minLoc, maxLoc; 00517 00518 // medianBlur(output2, input, 3); 00519 // medianBlur(output, output2, 5); 00520 00521 input = output + output2; 00522 00523 minMaxLoc(input, &min, &max, &minLoc, &maxLoc); 00524 input.convertTo(dst, CV_16U, SHRT_MAX/(max-min), -min); 00525 } 00526 00528 // Auxiliary method which flood fills a tile untill min_planeDistance threshold difference is met 00529 // @param tile Tile subimage of depth data 00530 // @param tileMask Mask subimage 00531 // @param pointMask Point mask subimage 00532 // @param points Vector of tile points 00533 // @param index Index of filling region 00534 // @param plane Equation of filled plane 00535 // @param ioffset Row offset of tile 00536 // @param joffset Column offset of tile 00537 // @param min_planeDistance Minimal distance from plane to fill 00539 int Regions::floodFillTile(Mat &tile, Mat&tileMask, Mat&pointMask, Vec3f *points, int index, Plane<float> &plane, int ioffset, int joffset, float min_planeDistance /*0.02*/) 00540 { 00541 std::vector<Vec2i> unprocessed; 00542 Mat processedMask = Mat::zeros(tileMask.size(), CV_8UC1); 00543 unprocessed.push_back(Vec2i(points[0][0] - ioffset, points[0][1] - joffset)); 00544 00545 // if some of points is non zero (was filled before), bail 00546 if (tileMask.at<int>(points[0][0] - ioffset, points[0][1] - joffset) != 0 || 00547 tileMask.at<int>(points[1][0] - ioffset, points[1][1] - joffset) != 0|| 00548 tileMask.at<int>(points[2][0] - ioffset, points[2][1] - joffset) != 0) 00549 return 0; 00550 00551 // mark vertices 00552 tileMask.at<int>(points[0][0] - ioffset, points[0][1] - joffset) = -1; 00553 tileMask.at<int>(points[1][0] - ioffset, points[1][1] - joffset) = -1; 00554 tileMask.at<int>(points[2][0] - ioffset, points[2][1] - joffset) = -1; 00555 Vec2i current; 00556 Vec2i newPoint; 00557 int neighbour; 00558 int found = 0; 00559 int size = 0; 00560 int tileSize = tile.rows; 00561 00562 // while loop of seed fill 00563 while (!unprocessed.empty()) 00564 { 00565 current = unprocessed.back(); 00566 unprocessed.pop_back(); 00567 00568 if (tileMask.at<int>(current[0], current[1]) == -1) 00569 ++found; 00570 00571 // set current index 00572 tileMask.at<int>(current[0], current[1]) = index; 00573 ++size; 00574 00575 // search neighborhood 00576 for (int i = -1; i <= 1; ++i) 00577 for (int j = -1; j <= 1; ++j) 00578 if (i != 0 || j != 0) 00579 { 00580 newPoint[0] = current[0] + i; 00581 newPoint[1] = current[1] + j; 00582 if (newPoint[0] >= 0 && 00583 newPoint[1] >= 0 && 00584 newPoint[0] < tileSize && 00585 newPoint[1] < tileSize) 00586 { 00587 neighbour = tileMask.at<int>(newPoint[0], newPoint[1]); 00588 00589 if (processedMask.at<unsigned char>(newPoint[0], newPoint[1]) == 0 && (neighbour == 0 || neighbour == -1) && plane.distance(pointMask.at<Vec3f>(newPoint[0], newPoint[1])) < min_planeDistance) 00590 unprocessed.push_back(newPoint); 00591 00592 processedMask.at<unsigned char>(newPoint[0], newPoint[1]) = 1; 00593 } 00594 } 00595 } 00596 00597 // if we touched all three vertices, return size else bail 00598 if (found < 3) 00599 return 0; 00600 else return size; 00601 } 00602 00604 // Auxiliary function extracts current region's plane using LSQ 00605 // @param plane Extracted plane 00606 // @param tile Tile subimage of depth data 00607 // @param tileMask Mask subimage 00608 // @param ioffset Row offset of tile 00609 // @param joffset Column offset of tile 00610 // @param index Index of filling region 00612 void Regions::getLeastSquaresAndBorder(Plane<float> &plane, Mat &tile, Mat&tileMask, int ioffset, int joffset, int index) 00613 { 00614 int tileSize = tile.rows; 00615 std::vector<Vec3f> points; 00616 for (int i = 0; i < tileSize; ++i) 00617 for (int j = 0; j < tileSize; ++j) 00618 { 00619 std::cout << tileMask.at<int>(i,j) << " " << index << std::endl; 00620 if (tileMask.at<int>(i,j) == index) 00621 points.push_back(Vec3f(i+ioffset, j+joffset, tile.at<float>(i,j))); 00622 } 00623 00624 plane = Normals::LeastSquaresPlane(points); 00625 } 00626 00628 // Auxiliary function for filling entire image from mask seed (until threshold from plane is met) 00629 // @param plane Plane equation 00630 // @param depth Depth image 00631 // @param mask Region mask 00632 // @param points Vector of seed points 00633 // @param i_tile Row tile offset 00634 // @param j_tile Column tile offset 00635 // @param tileSize Size of tile 00636 // @param index Index of filled region 00637 // @param min_planeDistance Maximal distance from plane to be filled 00639 void Regions::fillEverything(Plane<float> &plane, Mat & depth, Mat & mask, Mat & points, int i_tile, int j_tile, int tileSize, int index, float min_planeDistance /*0.05*/) 00640 { 00641 int maxi = tileSize+i_tile; 00642 int maxj = tileSize+j_tile; 00643 std::vector<Vec2i> unprocessed; 00644 Mat processedMask = Mat::zeros(depth.size(), CV_8UC1); 00645 00646 // loop in tile and mark all border pixels as seeds 00647 //#pragma omp parallel for 00648 for (int i = i_tile; i < maxi; ++i) 00649 for (int j = j_tile; j < maxj; ++j) 00650 { 00651 if (mask.at<int>(i,j) == index) 00652 { 00653 if (mask.at<int>(i+1,j+1) != index) 00654 { 00655 unprocessed.push_back(Vec2i(i+1, j+1)); 00656 break; 00657 } 00658 if (mask.at<int>(i ,j+1) != index) 00659 { 00660 unprocessed.push_back(Vec2i(i, j+1)); 00661 break; 00662 } 00663 if (mask.at<int>(i-1,j+1) != index) 00664 { 00665 unprocessed.push_back(Vec2i(i-1, j+1)); 00666 break; 00667 } 00668 if (mask.at<int>(i+1,j) != index) 00669 { 00670 unprocessed.push_back(Vec2i(i+1, j)); 00671 break; 00672 } 00673 if (mask.at<int>(i-1,j) != index) 00674 { 00675 unprocessed.push_back(Vec2i(i-1, j)); 00676 break; 00677 } 00678 if (mask.at<int>(i+1,j-1) != index) 00679 { 00680 unprocessed.push_back(Vec2i(i+1, j-1)); 00681 break; 00682 } 00683 if (mask.at<int>(i ,j-1) != index) 00684 { 00685 unprocessed.push_back(Vec2i(i, j-1)); 00686 break; 00687 } 00688 if (mask.at<int>(i-1,j-1) != index) 00689 { 00690 unprocessed.push_back(Vec2i(i-1, j-1)); 00691 break; 00692 } 00693 } 00694 } 00695 00696 Vec2i current, newPoint; 00697 Vec3f planenormal; 00698 planenormal[0] = plane.a; 00699 planenormal[1] = plane.b; 00700 planenormal[2] = plane.c; 00701 00702 int neighbour; 00703 00704 // seed fill loop 00705 while (!unprocessed.empty()) 00706 { 00707 current = unprocessed.back(); 00708 00709 unprocessed.pop_back(); 00710 00711 if (current[0] < 0 || current[1] < 0 || current[0] >= depth.rows || current[1] >= depth.cols) 00712 continue; 00713 00714 00715 if (mask.at<int>(current[0], current[1]) != 0) 00716 continue; 00717 00718 Vec4f planeVec = m_normals->m_planes.at<Vec4f>(current[0], current[1]); 00719 Vec3f normal(planeVec[0], planeVec[1], planeVec[2]); 00720 if (normal[2] < 0) 00721 { 00722 normal[0] *= -1; 00723 normal[1] *= -1; 00724 normal[2] *= -1; 00725 } 00726 // set current index 00727 mask.at<int>(current[0], current[1]) = index; 00728 // search neighbourhood 00729 00730 for (int i = -1; i <= 1; ++i) 00731 for (int j = -1; j <= 1; ++j) 00732 if (i != 0 || j != 0) 00733 { 00734 newPoint[0] = current[0] + i; 00735 newPoint[1] = current[1] + j; 00736 if (newPoint[0] >= 0 && newPoint[1] >= 0 && newPoint[0] < depth.rows && newPoint[1] < depth.cols) 00737 { 00738 neighbour = mask.at<int>(newPoint[0], newPoint[1]); 00739 if (processedMask.at<unsigned char>(newPoint[0], newPoint[1]) == 0 && (neighbour == 0 || neighbour == -1) && plane.distance(points.at<Vec3f>(newPoint[0], newPoint[1])) < min_planeDistance) 00740 unprocessed.push_back(newPoint); 00741 00742 processedMask.at<unsigned char>(newPoint[0], newPoint[1]) = 1; 00743 } 00744 } 00745 } 00746 } 00747 00749 // Method segments a depth image using own tile plane searching RANSAC method. 00750 // @param src Input CV_16UC depth matrix (raw input from kinect) 00751 // @param cam_info Camera info message (ROS) 00752 // @param minimumPlaneDistance Minimal distance threshold from triangle plane in whole tile (in meters) 00753 // @param minimumTriDistance Minimal distance threshold from triangle plane in triangle flood fill (in meters) 00754 // @param minimumFloodDistance Minimal distance threshold from triangle plane in whole image (in meters) 00755 // @param tileSize Tile size 00756 // @param smallestTriangleArea Smalest randomly found triangle to be considered for plane computation (in pixels^2) 00757 // @param minRegionSize Minimal region size to be considered 00758 // @param minCandidates Minimal number of pixels in tile to start a RANSAC 00760 void Regions::independentTileRegions(Mat &src, const CameraInfoConstPtr& cam_info, float minimumPlaneDistance, float minimumTriDistance, float minimumFloodDistance, int tileSize, int smallestTriangleArea, int minRegionSize, int minCandidates) 00761 { 00762 Mat input(src.size(), CV_32FC1); 00763 Mat output(src.size(), CV_32FC1); 00764 00765 Mat mask = Mat::zeros(src.size(), CV_32SC1); 00766 src.convertTo(input, CV_32F); 00767 00768 00769 if (m_normals == NULL) 00770 m_normals = new Normals(src, cam_info, NORMAL_COMPUTATION_TYPE, NORMAL_COMPUTATION_SIZE); 00771 00772 00773 int tileCenteri, tileCenterj; 00774 Vec3f points[3]; 00775 Vec3f pointsInt[3]; 00776 int random; 00777 Vec3f normal; 00778 float normalnorm; 00779 int currentIndex = 1; 00780 cv::Mat currentWindow, currentWindowMask, currentPoints; 00781 int maxrow = input.rows - tileSize; 00782 int maxcol = input.cols - tileSize; 00783 std::vector<Plane<float> > planes; 00784 std::vector<int > indices; 00785 00786 // for each of tiles... 00787 for (int tilei = 0; tilei < maxrow; tilei+=tileSize) 00788 for (int tilej = 0; tilej < maxcol; tilej+=tileSize) 00789 { 00790 tileCenteri = tilei + tileSize/2; 00791 tileCenterj = tilej + tileSize/2; 00792 std::vector<Vec3f> candidates; 00793 std::vector<Vec3f> candidatesInt; 00794 00795 // get all points in tile (used in RANSAC) 00796 for (int i = tilei; i < tilei+tileSize; ++i) 00797 for (int j = tilej; j < tilej+tileSize; ++j) 00798 if (mask.at<int>(i,j) == 0) 00799 { 00800 candidates.push_back(m_normals->m_points.at<Vec3f>(i,j)); 00801 candidatesInt.push_back(Vec3f(i,j,0)); 00802 } 00803 00804 int xsize = candidates.size(); 00805 00806 // if there is less than 10 candidate points, skip the tile 00807 if (xsize < minCandidates) 00808 continue; 00809 00810 // GO ransac 00811 for (unsigned int formax = 0; formax < RANSACMAX; ++formax) 00812 { 00813 // get three triangle points 00814 random = ((float)xsize*rand())/RAND_MAX; 00815 points[0] = candidates[random]; 00816 pointsInt[0] = candidatesInt[random]; 00817 random = ((float)xsize*rand())/RAND_MAX; 00818 points[1] = candidates[random]; 00819 pointsInt[1] = candidatesInt[random]; 00820 random = ((float)xsize*rand())/RAND_MAX; 00821 points[2] = candidates[random]; 00822 pointsInt[2] = candidatesInt[random]; 00823 00824 00825 if (points[0][2] < MIN_DISTANCE || 00826 points[1][2] < MIN_DISTANCE || 00827 points[2][2] < MIN_DISTANCE || 00828 points[0][2] > MAX_DISTANCE || 00829 points[1][2] > MAX_DISTANCE || 00830 points[2][2] > MAX_DISTANCE || 00831 norm((pointsInt[1] - pointsInt[0]).cross(pointsInt[2] - pointsInt[0])) < smallestTriangleArea ) 00832 continue; 00833 00834 00835 // compute normal of random triangle 00836 normal = (points[1]-points[0]).cross((points[2]-points[0])); 00837 normalnorm = norm(normal); 00838 normal[0] /= normalnorm; 00839 normal[1] /= normalnorm; 00840 normal[2] /= normalnorm; 00841 Plane<float> plane( normal[0], normal[1], normal[2], 00842 -(normal[0]*points[0][0] + normal[1]*points[0][1] + normal[2]*points[0][2])); 00843 00844 // get current tile windows 00845 currentPoints = m_normals->m_points(Rect(tilej, tilei, tileSize, tileSize)); 00846 currentWindow = input(Rect(tilej, tilei, tileSize, tileSize)); 00847 mask(Rect(tilej, tilei, tileSize, tileSize)).copyTo(currentWindowMask); 00848 00849 // flood fill tile with current region 00850 int size = floodFillTile(currentWindow, currentWindowMask, currentPoints, pointsInt, currentIndex, plane, tilei, tilej, minimumTriDistance); 00851 00852 // if we found sufficient big region 00853 if (size > minRegionSize) 00854 { 00855 // get border and least squares normal 00856 std::vector<Vec2i> border; 00857 std::vector<Vec3f> points; 00858 for (int i = 0; i < tileSize; ++i) 00859 for (int j = 0; j < tileSize; ++j) 00860 { 00861 if (currentWindowMask.at<int>(i,j) == currentIndex) 00862 points.push_back(currentPoints.at<Vec3f>(i, j)); 00863 } 00864 00865 // fill the rest of tile 00866 for (int i = 0; i < tileSize; ++i) 00867 for (int j = 0; j < tileSize; ++j) 00868 { 00869 if (currentWindowMask.at<int>(i,j) == 0 && plane.distance(currentPoints.at<Vec3f>(i, j)) < minimumPlaneDistance) 00870 currentWindowMask.at<int>(i,j) = currentIndex; 00871 } 00872 00873 points.clear(); 00874 std::vector<Vec3f> points2; 00875 for (int i = 0; i < tileSize; ++i) 00876 for (int j = 0; j < tileSize; ++j) 00877 { 00878 if (currentWindowMask.at<int>(i,j) == currentIndex) 00879 points2.push_back(currentPoints.at<Vec3f>(i, j)); 00880 } 00881 00882 plane = Normals::LeastSquaresPlane(points2); 00883 indices.push_back(currentIndex); 00884 planes.push_back(plane); 00885 00886 // save mask into whole image mask 00887 currentWindow = mask(Rect(tilej, tilei, tileSize, tileSize)); 00888 currentWindowMask.copyTo(currentWindow); 00889 00890 // floodFillRest() 00891 fillEverything(plane, input, mask, m_normals->m_points, tilei, tilej, tileSize, currentIndex, minimumFloodDistance); 00892 ++currentIndex; 00893 // we do not need to search anymore here 00894 break; 00895 } // if sufficient big region 00896 } // RANSAC loop 00897 } // tile loop 00898 00899 m_regionMatrix = Mat(src.size(), CV_32SC1); 00900 mask.convertTo(m_regionMatrix, CV_32SC1); 00901 } // function 00902 00903 } // but_seg_utils