parameter_space_hierarchy.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Rostislav Hulik (ihulik@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: 11.01.2012 (version 1.0)
 * 
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef BUT_PLANE_DET_PARAMSPACEH_H
#define BUT_PLANE_DET_PARAMSPACEH_H

// Opencv 2
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc_c.h>

// ROS
#include <sensor_msgs/CameraInfo.h>

// but_scenemodel
#include <srs_env_model_percp/but_plane_detector/parameter_space.h>
#include <srs_env_model_percp/but_segmentation/normals.h>


namespace srs_env_model_percp
{

        #define DEFAULT_ANGLE_STEP 5
        #define DEFAULT_SHIFT_STEP 5
        #define DEFAULT_BIN_SIZE 16
        #define DEFAULT_BIN_SIZE3 DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE

        class IndexStruct
        {
                public:
                        int lowResolutionIndex;
                        
                        int highResolutionIndex;
        };
        
        class ParameterSpaceHierarchy
        {
                public:
                        typedef but_plane_detector::Plane<float> tPlane;
                        typedef std::vector<tPlane, Eigen::aligned_allocator<tPlane> > tPlanes;


                public:
                        ParameterSpaceHierarchy(double anglemin, double anglemax, double zmin, double zmax, double angleRes = DEFAULT_ANGLE_STEP, double shiftRes = DEFAULT_SHIFT_STEP);

                        ~ParameterSpaceHierarchy();

                        int findMaxima(tPlanes &indices, double min_value, int neighborhood, int around);

                        void addVolume(ParameterSpace &second, int angle1, int angle2, int shift);
                        
                        void addVolume(ParameterSpace &second, int angle1, int angle2, int shift, float factor);

                        double getAngle(int index);

                        double getShift(int irndex);

                        double get(int angle1, int angle2, int z);
                        
                        double get(int bin_index, int inside_index);
                        
                        double get(double angle1, double angle2, double z);
                        
                        void set(int angle1, int angle2, int z, double val);
                        
                        void set(int bin_index, int inside_index, double val);

                        void set(double angle1, double angle2, double z, double val);

                        int getSize();

                        IndexStruct getIndex(double angle1, double angle2, double z);
                        
                        IndexStruct getIndex(int angle1, int angle2, int z);
                        
                        void getIndex(double angle1, double angle2, double z, int &angle1Index, int &angle2Index, int &shiftIndex);
                        
                        void fromIndex(int bin_index, int inside_index, int& angle1, int& angle2, int& z);
                
                        static void toAngles(float x, float y, float z, float &a1, float &a2);

                        static void toEuklid(float a1, float a2, float &x, float &y, float &z);

                        void clear();
                        bool m_init;
                        
                        double m_angleStep;
                        
                        double m_shiftStep;
                        
                        double m_angleLoStep;
                        
                        double m_shiftLoStep;
                        
                        double m_shiftmin;
                        
                        double m_shiftmax;
                        
                        double m_anglemin;
                        
                        double m_anglemax;
                        
                        int m_size;
                        
                        int m_loSize;
                        
                        int m_angleSize;
                        
                        int m_angleSize2;
                        
                        int m_shiftSize;
                        
                        int m_angleLoSize;
                        
                        int m_angleLoSize2;
                        
                        int m_shiftLoSize;
                        
                        int m_hiSize;
                        
                        int m_hiSize2;
                        
                        cv::Mat m_paramSpace;
                        
                        double **m_dataLowRes;
        };

        class ParameterSpaceHierarchyFullIterator
        {
                public:
                        ParameterSpaceHierarchyFullIterator(ParameterSpaceHierarchy *space)
                        {
                                index = space->m_angleSize*space->m_angleSize*space->m_shiftSize;

                                bin_index = 0;
                                inside_index = 0;

                                end = false;
                                m_space = space;
                                max_size = space->m_loSize;
                        }

                        double getVal()
                        {
                                return m_space->get(bin_index, inside_index);
                        }

                        void setVal(double val)
                        {
                                m_space->set(bin_index, inside_index, val);
                        }

                        ParameterSpaceHierarchyFullIterator &operator ++()
                        {
                                if (m_space->m_dataLowRes[bin_index] == NULL)
                                {
                                        inside_index = 0;
                                        ++bin_index;
                                }
                                else
                                {
                                        ++inside_index;
                                        if (inside_index >= DEFAULT_BIN_SIZE3)
                                        {
                                                inside_index = 0;
                                                ++bin_index;
                                        }
                                }

                                if (bin_index>=max_size) end = true;
                                return *this;
                        }

                        int bin_index;
                        
                        int inside_index;

                        bool end;
                private:
                
                        int index;
                        
                        int max_size;

                        ParameterSpaceHierarchy *m_space;
        };
//      class ParameterSpaceHierarchyFullIterator
//      {
//              public:
//                      /**
//                       * Constructor - initializes iterator and sets an index to the first.
//                       */
//                      ParameterSpaceHierarchyFullIterator(ParameterSpaceHierarchy *space)
//                      {
//                              index = space->m_angleSize*space->m_angleSize*space->m_shiftSize;
//                              currentI = 0;
//
//                              end = false;
//                              m_space = space;
//                              max_size = space->m_size;
//                      }
//
//                      /**
//                       * Returns a value at the current index
//                       */
//                      double getVal()
//                      {
//                              return m_space->get(currentI);
//                      }
//
//                      /**
//                       * Sets a value at the current index
//                       * @param val Value to be saved
//                       */
//                      void setVal(double val)
//                      {
//                              m_space->set(currentI, val);
//                      }
//
//                      /**
//                       * Increases an index to the next non null value
//                       */
//                      ParameterSpaceHierarchyFullIterator &operator ++()
//                      {
//                              if (m_space->m_dataLowRes[currentI / m_space->m_hiSize] == NULL)
//                                      currentI += m_space->m_hiSize;
//                              else
//                              {
//                                      ++currentI;
//                              }
//
//                              if (currentI>=max_size) end = true;
//                              return *this;
//                      }
//
//                      /**
//                       * Current index
//                       */
//                      int currentI;
//
//                      /**
//                       * End flag (true if we are out of given array)
//                       */
//                      bool end;
//              private:
//
//                      /**
//                       * TODO
//                       */
//                      int index;
//
//                      /**
//                       * Max size of given structure
//                       */
//                      int max_size;
//
//                      /**
//                       * Structure pointer
//                       */
//                      ParameterSpaceHierarchy *m_space;
//      };
} // but_plane_detector

#endif