$search
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