parameter_space_hierarchy.h
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id:$
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 1.0)
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 
00035 #pragma once
00036 #ifndef BUT_PLANE_DET_PARAMSPACEH_H
00037 #define BUT_PLANE_DET_PARAMSPACEH_H
00038 
00039 // Opencv 2
00040 #include <opencv2/highgui/highgui.hpp>
00041 #include <opencv2/imgproc/imgproc_c.h>
00042 
00043 // ROS
00044 #include <sensor_msgs/CameraInfo.h>
00045 
00046 // but_scenemodel
00047 #include <srs_env_model_percp/but_plane_detector/parameter_space.h>
00048 #include <srs_env_model_percp/but_segmentation/normals.h>
00049 
00050 
00051 namespace srs_env_model_percp
00052 {
00053 
00054         #define DEFAULT_ANGLE_STEP 5
00055         #define DEFAULT_SHIFT_STEP 5
00056         #define DEFAULT_BIN_SIZE 16
00057         #define DEFAULT_BIN_SIZE3 DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE
00058 
00062         class IndexStruct
00063         {
00064                 public:
00068                         int lowResolutionIndex;
00069                         
00073                         int highResolutionIndex;
00074         };
00075         
00080         class ParameterSpaceHierarchy
00081         {
00082                 public:
00083                         typedef but_plane_detector::Plane<float> tPlane;
00084                         typedef std::vector<tPlane, Eigen::aligned_allocator<tPlane> > tPlanes;
00085 
00086 
00087                 public:
00097                         ParameterSpaceHierarchy(double anglemin, double anglemax, double zmin, double zmax, double angleRes = DEFAULT_ANGLE_STEP, double shiftRes = DEFAULT_SHIFT_STEP);
00098 
00102                         ~ParameterSpaceHierarchy();
00103 
00109                         int findMaxima(tPlanes &indices, double min_value, int neighborhood, int around);
00110 
00118                         void addVolume(ParameterSpace &second, int angle1, int angle2, int shift);
00119                         
00128                         void addVolume(ParameterSpace &second, int angle1, int angle2, int shift, float factor);
00129 
00134                         double getAngle(int index);
00135 
00140                         double getShift(int irndex);
00141 
00148                         double get(int angle1, int angle2, int z);
00149                         
00154                         double get(int bin_index, int inside_index);
00155                         
00162                         double get(double angle1, double angle2, double z);
00163                         
00171                         void set(int angle1, int angle2, int z, double val);
00172                         
00178                         void set(int bin_index, int inside_index, double val);
00179 
00187                         void set(double angle1, double angle2, double z, double val);
00188 
00192                         int getSize();
00193 
00200                         IndexStruct getIndex(double angle1, double angle2, double z);
00201                         
00208                         IndexStruct getIndex(int angle1, int angle2, int z);
00209                         
00219                         void getIndex(double angle1, double angle2, double z, int &angle1Index, int &angle2Index, int &shiftIndex);
00220                         
00228                         void fromIndex(int bin_index, int inside_index, int& angle1, int& angle2, int& z);
00229                 
00238                         static void toAngles(float x, float y, float z, float &a1, float &a2);
00239 
00248                         static void toEuklid(float a1, float a2, float &x, float &y, float &z);
00249 
00250                         void clear();
00254                         bool m_init;
00255                         
00259                         double m_angleStep;
00260                         
00264                         double m_shiftStep;
00265                         
00269                         double m_angleLoStep;
00270                         
00274                         double m_shiftLoStep;
00275                         
00279                         double m_shiftmin;
00280                         
00284                         double m_shiftmax;
00285                         
00289                         double m_anglemin;
00290                         
00294                         double m_anglemax;
00295                         
00299                         int m_size;
00300                         
00304                         int m_loSize;
00305                         
00309                         int m_angleSize;
00310                         
00314                         int m_angleSize2;
00315                         
00319                         int m_shiftSize;
00320                         
00324                         int m_angleLoSize;
00325                         
00329                         int m_angleLoSize2;
00330                         
00334                         int m_shiftLoSize;
00335                         
00339                         int m_hiSize;
00340                         
00344                         int m_hiSize2;
00345                         
00349                         cv::Mat m_paramSpace;
00350                         
00354                         double **m_dataLowRes;
00355         };
00356 
00360         class ParameterSpaceHierarchyFullIterator
00361         {
00362                 public:
00366                         ParameterSpaceHierarchyFullIterator(ParameterSpaceHierarchy *space)
00367                         {
00368                                 index = space->m_angleSize*space->m_angleSize*space->m_shiftSize;
00369 
00370                                 bin_index = 0;
00371                                 inside_index = 0;
00372 
00373                                 end = false;
00374                                 m_space = space;
00375                                 max_size = space->m_loSize;
00376                         }
00377 
00381                         double getVal()
00382                         {
00383                                 return m_space->get(bin_index, inside_index);
00384                         }
00385 
00390                         void setVal(double val)
00391                         {
00392                                 m_space->set(bin_index, inside_index, val);
00393                         }
00394 
00398                         ParameterSpaceHierarchyFullIterator &operator ++()
00399                         {
00400                                 if (m_space->m_dataLowRes[bin_index] == NULL)
00401                                 {
00402                                         inside_index = 0;
00403                                         ++bin_index;
00404                                 }
00405                                 else
00406                                 {
00407                                         ++inside_index;
00408                                         if (inside_index >= DEFAULT_BIN_SIZE3)
00409                                         {
00410                                                 inside_index = 0;
00411                                                 ++bin_index;
00412                                         }
00413                                 }
00414 
00415                                 if (bin_index>=max_size) end = true;
00416                                 return *this;
00417                         }
00418 
00422                         int bin_index;
00423                         
00427                         int inside_index;
00428 
00432                         bool end;
00433                 private:
00434                 
00438                         int index;
00439                         
00443                         int max_size;
00444 
00448                         ParameterSpaceHierarchy *m_space;
00449         };
00450 //      class ParameterSpaceHierarchyFullIterator
00451 //      {
00452 //              public:
00453 //                      /**
00454 //                       * Constructor - initializes iterator and sets an index to the first.
00455 //                       */
00456 //                      ParameterSpaceHierarchyFullIterator(ParameterSpaceHierarchy *space)
00457 //                      {
00458 //                              index = space->m_angleSize*space->m_angleSize*space->m_shiftSize;
00459 //                              currentI = 0;
00460 //
00461 //                              end = false;
00462 //                              m_space = space;
00463 //                              max_size = space->m_size;
00464 //                      }
00465 //
00466 //                      /**
00467 //                       * Returns a value at the current index
00468 //                       */
00469 //                      double getVal()
00470 //                      {
00471 //                              return m_space->get(currentI);
00472 //                      }
00473 //
00474 //                      /**
00475 //                       * Sets a value at the current index
00476 //                       * @param val Value to be saved
00477 //                       */
00478 //                      void setVal(double val)
00479 //                      {
00480 //                              m_space->set(currentI, val);
00481 //                      }
00482 //
00483 //                      /**
00484 //                       * Increases an index to the next non null value
00485 //                       */
00486 //                      ParameterSpaceHierarchyFullIterator &operator ++()
00487 //                      {
00488 //                              if (m_space->m_dataLowRes[currentI / m_space->m_hiSize] == NULL)
00489 //                                      currentI += m_space->m_hiSize;
00490 //                              else
00491 //                              {
00492 //                                      ++currentI;
00493 //                              }
00494 //
00495 //                              if (currentI>=max_size) end = true;
00496 //                              return *this;
00497 //                      }
00498 //
00499 //                      /**
00500 //                       * Current index
00501 //                       */
00502 //                      int currentI;
00503 //
00504 //                      /**
00505 //                       * End flag (true if we are out of given array)
00506 //                       */
00507 //                      bool end;
00508 //              private:
00509 //
00510 //                      /**
00511 //                       * TODO
00512 //                       */
00513 //                      int index;
00514 //
00515 //                      /**
00516 //                       * Max size of given structure
00517 //                       */
00518 //                      int max_size;
00519 //
00520 //                      /**
00521 //                       * Structure pointer
00522 //                       */
00523 //                      ParameterSpaceHierarchy *m_space;
00524 //      };
00525 } // but_plane_detector
00526 
00527 #endif


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:56