scene_model.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: sceneModel.cpp 777 2012-05-11 11:23:17Z 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_plane_detector/scene_model.h>
00034 #include <srs_env_model_percp/but_segmentation/filtering.h>
00035 
00036 #include <pcl/segmentation/sac_segmentation.h>
00037 #include <pcl/segmentation/extract_clusters.h>
00038 #include <pcl/filters/statistical_outlier_removal.h>
00039 
00040 using namespace pcl;
00041 using namespace cv;
00042 using namespace but_plane_detector;
00043 namespace srs_env_model_percp
00044 {
00045 
00047         // Constructor - initializes a scene and allocates necessary space - a space of (angle, angle, d) where angles are angles of plane normal and d is d parameter of plane equation.
00048         // @param max_depth Maximum computed depth by each frame in meters (default 3.0)
00049         // @param min_shift Minimal d parameter value (ax + by + cz + d = 0) in Hough space (default -40.0)
00050         // @param max_shift Maximal d parameter value (ax + by + cz + d = 0) in Hough space (default 40.0)
00051         // @param angle_resolution Angle coordinates resolution (Hough space size in angle directions) (default 512)
00052         // @param shift_resolution d parameter coordinates resolution (Hough space size in angle directions) (default 4096)
00053         // @param gauss_angle_res Angle resolution of added Gauss function (default 11)
00054         // @param gauss_shift_res d parameter resolution of added Gauss function (default 11)
00055         // @param gauss_angle_sigma Sigma of added Gauss function in angle coordinates (default 11)
00056         // @param gauss_shift_sigma Sigma of added Gauss function in d parameter coordinates (default 11)
00058         SceneModel::SceneModel( double max_depth,
00059                                                         double min_shift,
00060                                                         double max_shift,
00061                                                         int angle_resolution,
00062                                                         int shift_resolution,
00063                                                         int gauss_angle_res,
00064                                                         int gauss_shift_res,
00065                                                         double gauss_angle_sigma,
00066                                                         double gauss_shift_sigma,
00067                                                         int lvl1_gauss_angle_res,
00068                                                         int lvl1_gauss_shift_res,
00069                                                         double lvl1_gauss_angle_sigma,
00070                                                         double lvl1_gauss_shift_sigma,
00071                                                         double plane_merge_angle,
00072                                                         double plane_merge_shift ) :    scene_cloud(new PointCloud<PointXYZRGB>),
00073                                                                                                                 space(-M_PI, M_PI, min_shift, max_shift, angle_resolution, shift_resolution),
00074                                                                                                                 current_space(-M_PI, M_PI, min_shift, max_shift, angle_resolution, shift_resolution),
00075                                                                                                                 gauss(-(gauss_angle_res/2) * space.m_angleStep, (gauss_angle_res/2) * space.m_angleStep, -(gauss_shift_res/2) * space.m_shiftStep, (gauss_shift_res/2) * space.m_shiftStep, gauss_angle_res, gauss_shift_res),
00076                                                                                                                 gaussPlane(-(lvl1_gauss_angle_res/2) * space.m_angleStep, (lvl1_gauss_angle_res/2) * space.m_angleStep, -(lvl1_gauss_shift_res/2) * space.m_shiftStep, (lvl1_gauss_shift_res/2) * space.m_shiftStep, lvl1_gauss_angle_sigma, lvl1_gauss_shift_sigma)
00077         {
00079                 // Using hierarchic array
00081 
00082                 m_angle_min = -M_PI;
00083                 m_angle_max = M_PI;
00084                 m_shift_min = min_shift;
00085                 m_shift_max = max_shift;
00086                 m_angle_res = angle_resolution;
00087                 m_shift_res = shift_resolution;
00088                 m_plane_merge_angle = plane_merge_angle;
00089                 m_plane_merge_shift = plane_merge_shift;
00090 
00091                 m_depth = max_depth;
00092 
00093                 // init of HS
00094                 std::cerr << "Parameter space size: " << space.getSize()*sizeof(double) / 1000000.0 << " MB" << std::endl;
00095                 std::cerr << "Parameter shift step: " << space.m_shiftStep << std::endl;
00096                 std::cerr << "Parameter angle step: " << space.m_angleStep << std::endl;
00097                 std::cerr << "Gauss space size: " << gauss.m_size*sizeof(double) / 1000000.0 << " MB" << std::endl;
00098                 std::cerr << "Gauss shift step: " << gauss.m_shiftStep << std::endl;
00099                 std::cerr << "Gauss angle step: " << gauss.m_angleStep << std::endl;
00100 
00101                 // generate Gauss function in gauss space
00102                 gauss.generateGaussIn(gauss_angle_sigma, gauss_shift_sigma);
00103                 gaussPlane.generateGaussIn(0.14, 0.14);
00104         }
00105 
00107         // Function recomputes a list of planes saved in this class (scene model)
00108         // @param min_current Minimal value for detected plane in current frame Hough space
00109         // @param min_global Minimal value for detected plane in global frame Hough space
00111         void SceneModel::recomputePlanes(double min_current, double min_global, int blur, int search_neighborhood, double substraction)
00112         {
00113                 planes.clear();
00114                 std::vector<int> counts;
00115                 tPlanes aux;
00116                 current_space.findMaxima(planes, min_current, blur, search_neighborhood);
00117                 int total_count = 0;
00118 
00119                 std::vector<bool> used(aux.size(), false);
00120 
00121 // possible merging close planes
00123 //              for (unsigned int i = 0; i < aux.size(); ++i)
00124 //              if (not used[i])
00125 //              {
00126 //                      used[i] = true;
00127 //                      Plane<float> final(aux[i].a, aux[i].b, aux[i].c, aux[i].d);
00128 //                      int count = 1;
00129 //                      for (unsigned int j = i+1; j < aux.size(); ++j)
00130 //                      if (not used[j] && aux[i].isSimilar(aux[j], 0.01, 0.01))
00131 //                      {
00132 //                              used[j] = true;
00133 //                              final.a += aux[j].a;
00134 //                              final.b += aux[j].b;
00135 //                              final.c += aux[j].c;
00136 //                              final.d += aux[j].d;
00137 //                              ++count;
00138 //                      }
00139 //
00140 //                      final.a /= count;
00141 //                      final.b /= count;
00142 //                      final.c /= count;
00143 //                      final.d /= count;
00144 //                      planes.push_back(final);
00145 //                      counts.push_back(count);
00146 //                      total_count += count;
00147 //              }
00149 
00150                 float a1, a2;
00151                 int ai1, ai2, zi;
00152                 // apply planes to the global model as gauss functions
00153                 for (unsigned int i = 0; i < planes.size(); ++i)
00154                 {
00155                         current_space.toAngles(planes[i].a, planes[i].b, planes[i].c, a1, a2);
00156                         current_space.getIndex(a1, a2, planes[i].d, ai1, ai2, zi);
00157 
00158                         space.addVolume(gauss, ai1, ai2, zi);
00159                 }
00160                 std::cerr << "Adding into global, size " << (double)space.getSize()*sizeof(double) / 1000000.0 << std::endl;
00161 
00162                 if (substraction > 0.0)
00163                 for (ParameterSpaceHierarchyFullIterator it(&space); !it.end; ++it)
00164                 {
00165                         double val = it.getVal();
00166                         if (val > 0.0)
00167                         {
00168                                 if (val > substraction)
00169                                         it.setVal(val - substraction);
00170                                 else
00171                                         it.setVal(0.0);
00172                         }
00173                 }
00174                 used.resize(planes.size(), false);
00175                 planes.clear();
00176                 used.clear();
00177 
00178                 aux.clear();
00179                 counts.clear();
00180                 space.findMaxima(aux, min_global, blur, search_neighborhood);
00181 
00182 // possible merging close planes
00184                 for (unsigned int i = 0; i < aux.size(); ++i)
00185 //
00186                 if (not used[i])
00187                 {
00188                         used[i] = true;
00189                         Plane<float> final(aux[i].a, aux[i].b, aux[i].c, aux[i].d);
00190                         int count = 1;
00191                         for (unsigned int j = i+1; j < aux.size(); ++j)
00192                         if (not used[j] && aux[i].isSimilar(aux[j], m_plane_merge_angle, m_plane_merge_shift))
00193                         {
00194                                 used[j] = true;
00195                                 final.a += aux[j].a;
00196                                 final.b += aux[j].b;
00197                                 final.c += aux[j].c;
00198                                 final.d += aux[j].d;
00199                                 ++count;
00200                                 //std::cerr << "merging " << aux[i].a << " " << aux[i].b << " " << aux[i].c << " " << aux[i].d << " ---> ";
00201                                 //std::cerr << "with " << aux[j].a << " " << aux[j].b << " " << aux[j].c << " " << aux[j].d << std::endl;
00202                         }
00203 
00204                         final.a /= count;
00205                         final.b /= count;
00206                         final.c /= count;
00207                         final.d /= count;
00208                         planes.push_back(final);
00209                         counts.push_back(count);
00210                         total_count += count;
00211                 }
00213 
00214                 std::cerr << "Found : " << planes.size() << " planes." << std::endl;
00215 //              //              //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00216 //              //                                      // Control visualisaation - uncoment to see HT space
00217 //              //                                      //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00218 //                                                                      {       double min = 99999999999.0;
00219 //                                                                      double max = -99999999999.0;
00220 //                                                                      ParameterSpaceHierarchyFullIterator it1(&space);
00221 //                                                                      while (not it1.end)
00222 //                                                                      {
00223 //                                                                              float value = it1.getVal();
00224 //                                                                              if (value < min) min = value;
00225 //                                                                              if (value > max) max = value;
00226 //                                                                              ++it1;
00227 //                                                                      }
00228 //
00229 //                                                                      ParameterSpaceHierarchyFullIterator it2(&space);
00230 //                                                                      while (not it2.end)
00231 //                                                                      {
00232 //                                                                              if (it2.getVal() != 0)
00233 //                                                                                      it2.setVal(((it2.getVal() - min) / (max - min)));
00234 //                                                                              ++it2;
00235 //                                                                      }
00236 //
00237 //                                                                      cv::Mat image = cv::Mat::zeros(cvSize(space.m_angleSize, space.m_angleSize), CV_32FC1);
00238 //                                                                      int shiftview = space.m_shiftSize/2;
00239 //                                                                      for (int angle1 = 0; angle1 < space.m_angleSize; angle1 += 1)
00240 //                                                                      for (int angle2 = 0; angle2 < space.m_angleSize; angle2 += 1)
00241 //                                                                      {
00242 //                                                                              image.at<float>(angle1, angle2) = space.get(angle1, angle2, shiftview);
00243 //                                                                      }
00244 //
00245 //
00246 //
00247 //                                                                      //create a new window & display the image
00248 //                                                                      cvNamedWindow("Smile", 1);
00249 //                                                                      cvShowImage("Smile", &IplImage(image));
00250 //                                                                      std::cerr << "Viewing shift = " << shiftview << std::endl;
00251 //                                                                      //wait for key to close the window
00252 //                                                                      int key = 0;
00253 //                                                                      while(1)
00254 //                                                                      {
00255 //                                                                          key = cvWaitKey();
00256 //                                                                          key &= 0x0000ffff;
00257 //                                                                          std::cerr << key << std::endl;
00258 //                                                                          if(key==27 || key == 0xffff) break;
00259 //
00260 //                                                                          switch(key)
00261 //                                                                          {
00262 //                                                                              case 'a':
00263 //                                                                                      if (shiftview < space.m_shiftSize-1)
00264 //                                                                                      {
00265 //                                                                                              ++shiftview;
00266 //                                                                                              for (int angle1 = 0; angle1 < space.m_angleSize; angle1 += 1)
00267 //                                                                                              for (int angle2 = 0; angle2 < space.m_angleSize; angle2 += 1)
00268 //                                                                                              {
00269 //                                                                                                      image.at<float>(angle1, angle2) = space.get(angle1, angle2, shiftview);
00270 //                                                                                              }
00271 //                                                                                      }
00272 //                                                                                      cvShowImage("Smile", &IplImage(image));
00273 //                                                                                      std::cerr << "Viewing shift = " << shiftview << "/" << space.m_shiftSize-1 << std::endl;
00274 //                                                                                  break;
00275 //                                                                              case 'z':
00276 //                                                                                      if (shiftview > 0)
00277 //                                                                                      {
00278 //                                                                                              --shiftview;
00279 //                                                                                              for (int angle1 = 0; angle1 < space.m_angleSize; angle1 += 1)
00280 //                                                                                              for (int angle2 = 0; angle2 < space.m_angleSize; angle2 += 1)
00281 //                                                                                              {
00282 //                                                                                                      image.at<float>(angle1, angle2) = space.get(angle1, angle2, shiftview);
00283 //                                                                                              }
00284 //                                                                                      }
00285 //                                                                                      cvShowImage("Smile", &IplImage(image));
00286 //                                                                                      std::cerr << "Viewing shift = " << shiftview << "/" << space.m_shiftSize-1 << std::endl;
00287 //                                                                                  break;
00288 //                                                                          }
00289 //                                                                      }
00290 //
00291 //
00292 //                                                                      cvDestroyWindow( "Smile" );}
00293 //              //              //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00294 //                                                      // Control visualisaation - uncoment to see HT space
00295 //                                                      //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00296 //                                                                      double min = 99999999999.0;
00297 //                                                                      double max = -99999999999.0;
00298 //                                                                      ParameterSpaceHierarchyFullIterator it1(&current_space);
00299 //                                                                      while (not it1.end)
00300 //                                                                      {
00301 //                                                                              float value = it1.getVal();
00302 //                                                                              if (value < min) min = value;
00303 //                                                                              if (value > max) max = value;
00304 //                                                                              ++it1;
00305 //                                                                      }
00306 //
00307 //                                                                      ParameterSpaceHierarchyFullIterator it2(&current_space);
00308 //                                                                      while (not it2.end)
00309 //                                                                      {
00310 //                                                                              if (it2.getVal() != 0)
00311 //                                                                                      it2.setVal(50*((it2.getVal() - min) / (max - min)));
00312 //                                                                              ++it2;
00313 //                                                                      }
00314 //
00315 //                                                                      cv::Mat image = cv::Mat::zeros(cvSize(current_space.m_angleSize, current_space.m_angleSize), CV_32FC1);
00316 //                                                                      int shiftview = current_space.m_shiftSize/2;
00317 //                                                                      for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00318 //                                                                      for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00319 //                                                                      {
00320 //                                                                              image.at<float>(angle1, angle2) = current_space.get(angle1, angle2, shiftview);
00321 //                                                                      }
00322 //
00323 //
00324 //
00325 //                                                                      //create a new window & display the image
00326 //                                                                      cvNamedWindow("Smile", 1);
00327 //                                                                      cvShowImage("Smile", &IplImage(image));
00328 //                                                                      std::cerr << "Viewing shift = " << shiftview << std::endl;
00329 //                                                                      //wait for key to close the window
00330 //                                                                      int key = 0;
00331 //                                                                      while(1)
00332 //                                                                      {
00333 //                                                                          key = cvWaitKey();
00334 //                                                                          key &= 0x0000ffff;
00335 //                                                                          std::cerr << key << std::endl;
00336 //                                                                          if(key==27 || key == 0xffff) break;
00337 //
00338 //                                                                          switch(key)
00339 //                                                                          {
00340 //                                                                              case 'a':
00341 //                                                                                      if (shiftview < current_space.m_shiftSize-1)
00342 //                                                                                      {
00343 //                                                                                              ++shiftview;
00344 //                                                                                              for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00345 //                                                                                              for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00346 //                                                                                              {
00347 //                                                                                                      image.at<float>(angle1, angle2) = current_space.get(angle1, angle2, shiftview);
00348 //                                                                                              }
00349 //                                                                                      }
00350 //                                                                                      cvShowImage("Smile", &IplImage(image));
00351 //                                                                                      std::cerr << "Viewing shift = " << shiftview << "/" << current_space.m_shiftSize-1 << std::endl;
00352 //                                                                                  break;
00353 //                                                                              case 'z':
00354 //                                                                                      if (shiftview > 0)
00355 //                                                                                      {
00356 //                                                                                              --shiftview;
00357 //                                                                                              for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00358 //                                                                                              for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00359 //                                                                                              {
00360 //                                                                                                      image.at<float>(angle1, angle2) = current_space.get(angle1, angle2, shiftview);
00361 //                                                                                              }
00362 //                                                                                      }
00363 //                                                                                      cvShowImage("Smile", &IplImage(image));
00364 //                                                                                      std::cerr << "Viewing shift = " << shiftview << "/" << current_space.m_shiftSize-1 << std::endl;
00365 //                                                                                  break;
00366 //                                                                          }
00367 //                                                                      }
00368 //
00369 //
00370 //                                                                      cvDestroyWindow( "Smile" );
00371 
00372 
00373         }
00374 
00376         // Clears all nodes with value lesser than parameter
00377         // @param minValue All nodes with value lesser than this parameter will be removed
00379         void SceneModel::clearNoise(double minValue)
00380         {
00381                 ParameterSpaceHierarchyFullIterator it(&space);
00382 
00383                 // pass all non null points
00384                 double val;
00385                 while (not it.end)
00386                 {
00387                         val = it.getVal();
00388                         if (val != 0.0 && val < minValue)
00389                         {
00390                                 it.setVal(0.0);
00391                         }
00392                         ++it;
00393                 }
00394         }
00395 
00397         // Function adds a depth map with computed normals into existing Hough space
00398         // @param normals Normals object (point cloud with precomputed normals)
00400         void SceneModel::AddNext(Normals &normals, double min_current)
00401         {
00402                 ParameterSpaceHierarchy cache_space(m_angle_min, m_angle_max, m_shift_min, m_shift_max, m_angle_res, m_shift_res);
00403                 current_space.clear();
00404 
00405                 int maxi = normals.m_points.rows;
00406                 int maxj = normals.m_points.cols;
00407 
00408                 Vec3f point;
00409                 Vec4f plane;
00410                 indexFactor = 1.0;
00411 
00412                 scene_cloud->clear();
00413                 // pass all points and write them into init space
00414                 for (int i = 0; i < maxi; ++i)
00415                 for (int j = 0; j < maxj; ++j)
00416                 {
00417                         point = normals.m_points.at<Vec3f>(i, j);
00418                         plane = normals.m_planes.at<Vec4f>(i, j);
00419 
00420                         if (plane[0] != 0.0 || plane[1] != 0.0 || plane[2] != 0.0)
00421                         {
00422                                 // signed angle atan2(b.y,b.x) - atan2(a.y,a.x)
00423                                 // angle on XZ plane with X
00424                                 float a1, a2;
00425                                 ParameterSpace::toAngles(plane[0], plane[1], plane[2], a1, a2);
00426 //
00427 
00428 //                              PointXYZRGB rgbpoint(255, 255, 255);
00429 //                              rgbpoint.x = point[0];
00430 //                              rgbpoint.y = point[1];
00431 //                              rgbpoint.z = point[2];
00432 //                              rgbpoint.r = (plane[0]+1)*128;
00433 //                              rgbpoint.g = (plane[1]+1)*128;
00434 //                              rgbpoint.b = (plane[2]+1)*128;
00435 //                              scene_cloud->push_back(rgbpoint);
00436 
00437                                 int i, j, k;
00438                                 cache_space.getIndex(a1, a2, plane[3], i, j, k);
00439 
00440                                 if (i < cache_space.m_angleSize && j < cache_space.m_angleSize && k < cache_space.m_shiftSize &&
00441                                         i >= 0 && j >= 0 && k >= 0)
00442                                 {
00443                                         cache_space.set(i, j, k, cache_space.get(i, j, k) + 1);
00444                                 }
00445                         }
00446                 }
00447 
00448                 // fire up the iterator on cache space
00449                 ParameterSpaceHierarchyFullIterator it(&cache_space);
00450                 int i, j, k;
00451                 double val;
00452 
00453                 // for each point in cache space which is not zero, write a multiplied gauss into the HT
00454                 while (not it.end)
00455                 {
00456                         val = it.getVal();
00457                         if (val >= min_current)
00458                         {
00459                                 current_space.fromIndex(it.bin_index, it.inside_index, i, j, k);
00460                                 current_space.addVolume(gauss, i, j, k, val);
00461                         }
00462                         ++it;
00463                 }
00464 
00465                 std::cerr << "New parameter space size: " << (double)current_space.getSize()*sizeof(double) / 1000000.0 << " MB" << std::endl;
00466 
00467         }
00468 
00470         // Function adds a depth map with computed normals into existing Hough space
00471         // @param depth Depth image
00472         // @param cam_info Camera info object
00473         // @param normals Computed normals of depth image
00475         void SceneModel::AddNext(Mat &depth, const sensor_msgs::CameraInfoConstPtr& cam_info, Normals &normals, double min_current)
00476         {
00477                 ParameterSpaceHierarchy cache_space(m_angle_min, m_angle_max, m_shift_min, m_shift_max, m_angle_res, m_shift_res);
00478                 current_space.clear();
00479 
00480                 int maxi = normals.m_points.rows;
00481                 int maxj = normals.m_points.cols;
00482 
00483                 Vec3f point;
00484                 Vec4f plane;
00485                 //Regions reg(&normals);
00486                 //reg.watershedRegions(depth, cam_info, WatershedType::DepthDiff, 1, 2, 20);
00487                 //double min, max;
00488                 //minMaxLoc(reg.m_regionMatrix, &min, &max);
00489                 indexFactor = 1.0;
00490 
00491                 //if (max !=0)
00492                 //      indexFactor /= (double)max;
00493 
00494                 scene_cloud->clear();
00495                 // pass all points and write them into init space
00496                 for (int i = 0; i < maxi; ++i)
00497                         for (int j = 0; j < maxj; ++j)
00498                         {
00499                                 point = normals.m_points.at<Vec3f>(i, j);
00500                                 plane = normals.m_planes.at<Vec4f>(i, j);
00501 
00502                                 // skip all which is farer than 3m
00503                                 //if (point[2] < m_depth)
00504                                 //{
00505                                         // signed angle atan2(b.y,b.x) - atan2(a.y,a.x)
00506                                         // angle on XZ plane with X
00507                                         float a1, a2;
00508                                         ParameterSpace::toAngles(plane[0], plane[1], plane[2], a1, a2);
00509 
00510 
00511                                         PointXYZRGB rgbpoint(255, 255, 255);
00512                                         //if (reg.m_regionMatrix.at<int>(i, j) > 0)
00513                                         //      rgbpoint.rgb = indexFactor *reg.m_regionMatrix.at<int>(i, j);
00514 
00515                                         rgbpoint.x = point[0];
00516                                         rgbpoint.y = point[1];
00517                                         rgbpoint.z = point[2];
00518                                         scene_cloud->push_back(rgbpoint);
00519 
00520                                         int i, j, k;
00521                                         cache_space.getIndex(a1, a2, plane[3], i, j, k);
00522                                         cache_space.set(i, j, k, cache_space.get(i, j, k) + 1);
00523                                 //}
00524                         }
00525 
00526                         // fire up the iterator on cache space
00527                         ParameterSpaceHierarchyFullIterator it(&cache_space);
00528                         int i, j, k;
00529                         double val;
00530                         // for each point in cache space which is not zero, write a multiplied gauss into the HT
00531                         while (not it.end)
00532                         {
00533                                 val = it.getVal();
00534                                 if (val > min_current)
00535                                 {
00536                                         current_space.fromIndex(it.bin_index, it.inside_index, i, j, k);
00537                                         current_space.addVolume(gauss, i, j, k, val);
00538                                 }
00539                                 ++it;
00540                         }
00541 
00542                         std::cerr << "New parameter space size: " << (double)current_space.getSize()*sizeof(double) / 1000000.0 << " MB" << std::endl;
00543 
00544 //                      //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00545 //                      // Control visualisaation - uncoment to see HT space
00546 //                      //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00547 //                                      double min = 99999999999.0;
00548 //                                      double max = -99999999999.0;
00549 //                                      for (int shift = 0; shift < current_space.m_shiftSize; shift += 1)
00550 //                                      for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00551 //                                      for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00552 //                                      {
00553 //                                              float value = current_space.get(angle1, angle2, shift);
00554 //                                              if (value < min) min = value;
00555 //                                              if (value > max) max = value;
00556 //
00557 //                      //                      if (space(angle1, angle2, shift) > 0.0)
00558 //                      //                      {
00559 //                      //                      PointXYZI pt;
00560 //                      //                      pt.x = (float)angle1;
00561 //                      //                      pt.y = (float)angle2;
00562 //                      //                      pt.z = (float)shift;
00563 //                      //                      pt.intensity = space(angle1, angle2, shift);
00564 //                      //                      current_hough_cloud->push_back(pt);
00565 //                      //                      }
00566 //                                      }
00567 //
00568 //                                      for (int shift = 0; shift < current_space.m_shiftSize; shift += 1)
00569 //                                      for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00570 //                                      for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00571 //                                      {
00572 //                                              if (current_space.get(angle1, angle2, shift) != 0)
00573 //                                                      current_space.set(angle1, angle2, shift, 255.0*((current_space.get(angle1, angle2, shift) - min) / (max - min)));
00574 //                                      }
00575 //
00576 //                                      cv::Mat image = cv::Mat::zeros(cvSize(current_space.m_angleSize, current_space.m_angleSize), CV_32FC1);
00577 //                                      int shiftview = current_space.m_shiftSize/2;
00578 //                                      for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00579 //                                      for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00580 //                                      {
00581 //                                              image.at<float>(angle1, angle2) = current_space.get(angle1, angle2, shiftview);
00582 //                                      }
00583 //
00584 //
00585 //
00586 //                                      //create a new window & display the image
00587 //                                      cvNamedWindow("Smile", 1);
00588 //                                      cvShowImage("Smile", &IplImage(image));
00589 //                                      std::cerr << "Viewing shift = " << shiftview << std::endl;
00590 //                                      //wait for key to close the window
00591 //                                      int key = 0;
00592 //                                      while(1)
00593 //                                      {
00594 //                                          key = cvWaitKey();
00595 //                                          key &= 0x0000ffff;
00596 //                                          std::cerr << key << std::endl;
00597 //                                          if(key==27 || key == 0xffff) break;
00598 //
00599 //                                          switch(key)
00600 //                                          {
00601 //                                              case 'a':
00602 //                                                      if (shiftview < current_space.m_shiftSize-1)
00603 //                                                      {
00604 //                                                              ++shiftview;
00605 //                                                              for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00606 //                                                              for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00607 //                                                              {
00608 //                                                                      image.at<float>(angle1, angle2) = current_space.get(angle1, angle2, shiftview);
00609 //                                                              }
00610 //                                                      }
00611 //                                                      cvShowImage("Smile", &IplImage(image));
00612 //                                                      std::cerr << "Viewing shift = " << shiftview << "/" << current_space.m_shiftSize-1 << std::endl;
00613 //                                                  break;
00614 //                                              case 'z':
00615 //                                                      if (shiftview > 0)
00616 //                                                      {
00617 //                                                              --shiftview;
00618 //                                                              for (int angle1 = 0; angle1 < current_space.m_angleSize; angle1 += 1)
00619 //                                                              for (int angle2 = 0; angle2 < current_space.m_angleSize; angle2 += 1)
00620 //                                                              {
00621 //                                                                      image.at<float>(angle1, angle2) = current_space.get(angle1, angle2, shiftview);
00622 //                                                              }
00623 //                                                      }
00624 //                                                      cvShowImage("Smile", &IplImage(image));
00625 //                                                      std::cerr << "Viewing shift = " << shiftview << "/" << current_space.m_shiftSize-1 << std::endl;
00626 //                                                  break;
00627 //                                          }
00628 //                                      }
00629 //
00630 //
00631 //                                      cvDestroyWindow( "Smile" );
00632         }
00633 } // but_plane_detector


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:51:57