parameter_space_hierarchy.cpp
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 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 
00035 #include <srs_env_model_percp/but_plane_detector/parameter_space_hierarchy.h>
00036 
00037 using namespace srs_env_model_percp;
00038 using namespace std;
00039 using namespace sensor_msgs;
00040 using namespace cv;
00041 using namespace but_plane_detector;
00042 
00044 // Constructor - creates and allocates a space (angle X angle X shift)
00045 // @param anglemin Minimal angle in space
00046 // @param anglemax Maximal angle in space
00047 // @param zmin Minimal shift in space
00048 // @param zmax Maximal shift in space
00049 // @param angleRes Angle resolution (step)
00050 // @param shiftRes Shift resolution (step)
00052 ParameterSpaceHierarchy::ParameterSpaceHierarchy(double anglemin, double anglemax, double zmin, double zmax, double angleRes, double shiftRes)
00053 {
00054         m_init = false;
00055         m_angleStep = (anglemax - anglemin) / (angleRes - 1);
00056         m_shiftStep = (zmax - zmin) / (shiftRes - 1);
00057         m_angleLoStep = m_angleStep * DEFAULT_BIN_SIZE;
00058         m_shiftLoStep = m_shiftStep * DEFAULT_BIN_SIZE;
00059 
00060         m_angleSize = (anglemax - anglemin) / m_angleStep + 1;
00061         m_angleSize2 = m_angleSize * m_angleSize;
00062         m_shiftSize = (zmax - zmin) / m_shiftStep + 1;
00063 
00064         assert(m_angleSize % DEFAULT_BIN_SIZE == 0 && m_shiftSize % DEFAULT_BIN_SIZE == 0);
00065 
00066         m_angleLoSize = m_angleSize / DEFAULT_BIN_SIZE;
00067         m_angleLoSize2 = m_angleSize2 / (DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE);
00068         m_shiftLoSize = m_shiftSize / DEFAULT_BIN_SIZE;
00069 
00070         m_shiftmin = zmin;
00071         m_shiftmax = zmax;
00072         m_anglemin = anglemin;
00073         m_anglemax = anglemax;
00074 
00075         m_size = m_angleSize2 * m_shiftSize;
00076         m_loSize = m_angleLoSize2 * m_shiftLoSize;
00077 
00078         m_hiSize = DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE;
00079         m_hiSize2 = DEFAULT_BIN_SIZE*DEFAULT_BIN_SIZE;
00080         m_dataLowRes = (double **)malloc(sizeof(double*)*m_loSize);
00081         for (int i = 0; i < m_loSize; ++i)
00082                 m_dataLowRes[i] = NULL;
00083 }
00084 
00086 // Destructor
00088 ParameterSpaceHierarchy::~ParameterSpaceHierarchy()
00089 {
00090         for (int i = 0; i < m_loSize; ++i)
00091                 if (m_dataLowRes[i] != NULL) free(m_dataLowRes[i]);
00092 
00093         free(m_dataLowRes);
00094 }
00095 
00096 void ParameterSpaceHierarchy::clear()
00097 {
00098         for (int i = 0; i < m_loSize; ++i)
00099         {
00100                 if (m_dataLowRes[i] != NULL) free(m_dataLowRes[i]);
00101                 m_dataLowRes[i] = NULL;
00102         }
00103 
00104 }
00105 
00107 // Conversion from Euclidian representation of normal (x, y, z) to parametrized (a1, a2)
00108 // @param x X vector coordinate
00109 // @param y Y vector coordinate
00110 // @param z Z vector coordinate
00111 // @param a1 First angle
00112 // @param a2 Second angle
00114 void ParameterSpaceHierarchy::toAngles(float x, float y, float z, float &a1, float &a2)
00115 {
00116         a1 = atan2(z, x);
00117         // align with X on XZ
00118         x = z * sin(a1) + x * cos(a1);
00119         a2 = atan2(y, x);
00120 }
00121 
00123 // Conversion from parametrized representation of normal (a1, a2) to Euclidian (x, y, z)
00124 // @param x X vector coordinate
00125 // @param y Y vector coordinate
00126 // @param z Z vector coordinate
00127 // @param a1 First angle
00128 // @param a2 Second angle
00130 void ParameterSpaceHierarchy::toEuklid(float a1, float a2, float &x, float &y, float &z)
00131 {
00132         float auxx = cos(-a2);
00133         y = -sin(-a2);
00134         x = auxx * cos(-a1);
00135         z = -auxx * sin(-a1);
00136 }
00137 
00139 // Returns a value saved at given coordinates (indices)
00140 // @param angle1 First angle coordinate
00141 // @param angle2 Second angle coordinate
00142 // @param z Shift (d param) coordinate
00144 double ParameterSpaceHierarchy::get(int angle1, int angle2, int z)
00145 {
00146         IndexStruct index = getIndex(angle1, angle2, z);
00147         //std::cout << m_loSize << std::endl;
00148         //std::cout << loIndex << " -> " << z%DEFAULT_BIN_SIZE * m_hiSize2 + angle2%DEFAULT_BIN_SIZE * DEFAULT_BIN_SIZE + angle1%DEFAULT_BIN_SIZE << std::endl;
00149         //std::cout << std::endl;
00150         if (m_dataLowRes[index.lowResolutionIndex] == NULL)
00151                 return 0.0;
00152         else
00153                 return m_dataLowRes[index.lowResolutionIndex][index.highResolutionIndex];
00154 
00155 }
00156 
00158 //* Saves a value at given coordinates (indices)
00159 //* @param angle1 First angle coordinate
00160 //* @param angle2 Second angle coordinate
00161 //* @param z Shift (d param) coordinate
00162 //* @param val Value to be saved
00164 void ParameterSpaceHierarchy::set(int angle1, int angle2, int z, double val)
00165 {
00166         IndexStruct index = getIndex(angle1, angle2, z);
00167         if (m_dataLowRes[index.lowResolutionIndex] == NULL)
00168         {
00169                 m_dataLowRes[index.lowResolutionIndex] = (double *)malloc(sizeof(double) *m_hiSize);
00170                 memset(m_dataLowRes[index.lowResolutionIndex], 0, m_hiSize * sizeof(double));
00171         }
00172         m_dataLowRes[index.lowResolutionIndex][index.highResolutionIndex] = val;
00173 }
00174 
00176 // Returns a value saved at given index
00177 // @param index Given index
00179 double ParameterSpaceHierarchy::get(int bin_index, int inside_index)
00180 {
00181         if (m_dataLowRes[bin_index] == NULL)
00182                 return 0.0;
00183         else
00184                 return m_dataLowRes[bin_index][inside_index];
00185 }
00186 
00188 // Saves a value at given index
00189 // @param index Given index
00190 // @param val Value to be saved
00192 void ParameterSpaceHierarchy::set(int bin_index, int inside_index, double val)
00193 {
00194         if (m_dataLowRes[bin_index] == NULL)
00195         {
00196                 m_dataLowRes[bin_index] = (double *)malloc(sizeof(double) *m_hiSize);
00197                 memset(m_dataLowRes[bin_index], 0, m_hiSize * sizeof(double));
00198         }
00199         m_dataLowRes[bin_index][inside_index] = val;
00200 }
00201 
00203 // Returns an index from given values
00204 // @param angle1 First angle value
00205 // @param angle2 Second angle value
00206 // @param z Shift (d param) value
00208 IndexStruct ParameterSpaceHierarchy::getIndex(double angle1, double angle2, double z)
00209 {
00210         IndexStruct index;
00211         int a1 = (angle1-m_anglemin) / m_angleStep;
00212         int a2 = (angle2-m_anglemin) / m_angleStep;
00213         int zz = (z-m_shiftmin) / m_shiftStep;
00214         index.highResolutionIndex = zz%DEFAULT_BIN_SIZE * m_hiSize2 + a2%DEFAULT_BIN_SIZE * DEFAULT_BIN_SIZE + a1%DEFAULT_BIN_SIZE;
00215         index.lowResolutionIndex = zz/DEFAULT_BIN_SIZE * m_angleLoSize2 + a2/DEFAULT_BIN_SIZE * m_angleLoSize + a1/DEFAULT_BIN_SIZE;
00216 
00217         return index;
00218 }
00219 
00221 // Converts an index into axis coordinates
00222 // @param i Given index
00223 // @param angle1 First angle coordinate
00224 // @param angle2 Second angle coordinate
00225 // @param z Shift (d param) coordinate
00227 void ParameterSpaceHierarchy::fromIndex(int bin_index, int inside_index, int& angle1, int& angle2, int& z)
00228 {
00229         //int iLo = i / m_hiSize;
00230         //int iHi = i % m_hiSize;
00231         int loZ, loA1, loA2;
00232 
00233         z = (bin_index / (m_angleLoSize2));
00234         loZ = inside_index / (m_hiSize2);
00235 
00236         angle2 = ((bin_index - z * m_angleLoSize2) / m_angleLoSize);
00237         loA2 = (inside_index - loZ * m_hiSize2) / DEFAULT_BIN_SIZE;
00238 
00239         angle1 = (bin_index - (z * m_angleLoSize2) - angle2 * m_angleLoSize);
00240         loA1 =   inside_index - (loZ * m_hiSize2) - loA2 * DEFAULT_BIN_SIZE;
00241 
00242         z = z*DEFAULT_BIN_SIZE + loZ;
00243         angle2 = angle2*DEFAULT_BIN_SIZE + loA2;
00244         angle1 = angle1*DEFAULT_BIN_SIZE + loA1;
00245 }
00246 
00248 // Returns an index from given coordinate indices
00249 // @param angle1 First angle coordinate
00250 // @param angle2 Second angle coordinate
00251 // @param z Shift (d param) coordinate
00253 IndexStruct ParameterSpaceHierarchy::getIndex(int angle1, int angle2, int z)
00254 {
00255         IndexStruct index;
00256 
00257         index.highResolutionIndex = z%DEFAULT_BIN_SIZE * m_hiSize2 + angle2%DEFAULT_BIN_SIZE * DEFAULT_BIN_SIZE + angle1%DEFAULT_BIN_SIZE;
00258         index.lowResolutionIndex = z/DEFAULT_BIN_SIZE * m_angleLoSize2 + angle2/DEFAULT_BIN_SIZE * m_angleLoSize + angle1/DEFAULT_BIN_SIZE;
00259 
00260         return index;
00261 }
00262 
00264 // Converts between values and coordinates
00265 // @param angle1 First angle value
00266 // @param angle2 Second angle value
00267 // @param z Shift (d param) value
00268 // @param angle1Index First angle coordinate
00269 // @param angle2Index Second angle coordinate
00270 // @param shiftIndex (d param) coordinate
00272 void ParameterSpaceHierarchy::getIndex(double angle1, double angle2, double z, int &angle1Index, int &angle2Index, int &shiftIndex)
00273 {
00274         angle1Index = (angle1-m_anglemin) / m_angleStep;
00275         angle2Index = (angle2-m_anglemin) / m_angleStep;
00276     shiftIndex = (z-m_shiftmin) / m_shiftStep;
00277 }
00278 
00280 // Adds a second volume to this with offset
00281 // @param second Second volume to be added
00282 // @param angle1 Angle 1 offset
00283 // @param angle2 Angle 2 offset
00284 // @param shift Shift offset
00286 void ParameterSpaceHierarchy::addVolume(ParameterSpace &second, int angle1, int angle2, int shift)
00287 {
00288         //std::cout << angle1 << " " << angle2 << " " << shift << " " << std::endl;
00289         double secondAngleHalf = second.m_angleSize / 2;
00290         double secondShiftHalf = second.m_shiftSize / 2;
00291         int shiftThis, angle1This, angle2This;
00292         int shiftS, angle1S, angle2S;
00293 
00294         for (shiftS = 0, shiftThis = shift - secondShiftHalf; shiftS < second.m_shiftSize; ++shiftS, ++shiftThis)
00295         for (angle2S=0, angle2This = angle2 - secondAngleHalf; angle2S < second.m_angleSize; ++angle2S, ++angle2This)
00296         for (angle1S=0, angle1This = angle1 - secondAngleHalf; angle1S < second.m_angleSize; ++angle1S, ++angle1This)
00297         {
00298                 if (angle1This >= 0 && angle1This < m_angleSize &&
00299                         angle2This >= 0 && angle2This < m_angleSize &&
00300                         shiftThis  >= 0 && shiftThis < m_shiftSize)
00301                 {
00302                         set(angle1This, angle2This, shiftThis, get(angle1This, angle2This, shiftThis) + second(angle1S, angle2S, shiftS));
00303                 }
00304 
00305         }
00306 }
00307 
00309 // Adds a second volume to this with offset and multiplied by given factor
00310 // @param second Second volume to be added
00311 // @param angle1 Angle 1 offset
00312 // @param angle2 Angle 2 offset
00313 // @param shift Shift offset
00314 // @param factor number by which each gauss function will be multiplied
00316 void ParameterSpaceHierarchy::addVolume(ParameterSpace &second, int angle1, int angle2, int shift, float factor)
00317 {
00318         double secondAngleHalf = second.m_angleSize / 2;
00319         double secondShiftHalf = second.m_shiftSize / 2;
00320         int shiftThis, angle1This, angle2This;
00321         int shiftS, angle1S, angle2S;
00322 
00323         for (shiftS = 0, shiftThis = shift - secondShiftHalf; shiftS < second.m_shiftSize; ++shiftS, ++shiftThis)
00324         for (angle2S=0, angle2This = angle2 - secondAngleHalf; angle2S < second.m_angleSize; ++angle2S, ++angle2This)
00325         for (angle1S=0, angle1This = angle1 - secondAngleHalf; angle1S < second.m_angleSize; ++angle1S, ++angle1This)
00326         {
00327                 if (angle1This >= 0 && angle1This < m_angleSize &&
00328                         angle2This >= 0 && angle2This < m_angleSize &&
00329                         shiftThis  >= 0 && shiftThis < m_shiftSize)
00330                 {
00331                         set(angle1This, angle2This, shiftThis, get(angle1This, angle2This, shiftThis) + factor * second(angle1S, angle2S, shiftS));
00332                 }
00333 
00334         }
00335 }
00336 
00338 // Finds maximas in this and saves them as planes to given vector
00339 // @param indices Found planes
00340 // @returns index of maximal plane
00342 int ParameterSpaceHierarchy::findMaxima(tPlanes &indices, double min_value, int neighborhood, int around)
00343 {
00344         float a, b, c;
00345         int maxind = -1;
00346         float max = -1;
00347         int angle1, angle2, shift;
00348 
00349         int around3 = around + around + 1;
00350         around3 = around3 * around3 * around3;
00351         ParameterSpaceHierarchyFullIterator it(this);
00352 
00353         // pass all non null points
00354         while (not it.end)
00355         {
00356                 double val = it.getVal();
00357                 // if value is greater than min_value
00358                 if (val > min_value)
00359                 {
00360                         this->fromIndex(it.bin_index, it.inside_index, angle1, angle2, shift);
00361 
00362                         // if we are in bounds due to search neighborhood
00363                         if (angle1 >= neighborhood+around && angle1 < m_angleSize-(neighborhood+around) &&
00364                                 angle2 >= neighborhood+around && angle2 < m_angleSize-(neighborhood+around) &&
00365                                 shift >= neighborhood+around && shift < m_shiftSize-(neighborhood+around))
00366                         {
00367                                 val = 0.0;
00368                                 // compute value from neighborhood
00369                                 for (int a1 = -neighborhood; a1 <= neighborhood; ++a1)
00370                                 for (int a2 = -neighborhood; a2 <= neighborhood; ++a2)
00371                                 for (int s = -neighborhood; s <= neighborhood; ++s)
00372                                         val +=  get(angle1+a1, angle2+a2, shift+s);
00373 
00374 
00375                                 bool ok = true;
00376                                 double aux;
00377                                 int xx, yy, zz;
00378 
00379                                 // search around meaned values
00380                                 for (int x = -around; x <= around; ++x)
00381                                 for (int y = -around; y <= around; ++y)
00382                                 for (int z = -around; z <= around; ++z)
00383                                 {
00384                                         xx = angle1 + x;
00385                                         yy = angle2 + y;
00386                                         zz = shift + z;
00387                                         aux = 0.0;
00388                                         for (int a1 = -neighborhood; a1 <= neighborhood; ++a1)
00389                                         for (int a2 = -neighborhood; a2 <= neighborhood; ++a2)
00390                                         for (int s = -neighborhood; s <= neighborhood; ++s)
00391                                                 aux += get(xx+a1, yy+a2, zz+s);
00392                                         if (val < aux)
00393                                         {
00394                                                 ok = false;
00395                                                 break;
00396                                         }
00397                                 }
00398 
00399                                 // if we found local maximum, we save plane
00400                                 if (ok)
00401                                 {
00402 //                                      toEuklid(getAngle(angle1), getAngle(angle2), a, b, c);
00403 //                                      indices.push_back(Plane<float>(a, b, c, getShift(shift)));
00404 //                                      if (val > max)
00405 //                                      {
00406 //                                              max = val;
00407 //                                              maxind = indices.size() - 1;
00408 //                                      }
00409 
00410                                         double aroundx = 0;
00411                                         double aroundy = 0;
00412                                         double aroundz = 0;
00413                                         double arounds = 0;
00414                                         for (int x = -around; x <= around; ++x)
00415                                         for (int y = -around; y <= around; ++y)
00416                                         for (int z = -around; z <= around; ++z)
00417                                         {
00418                                                 xx = angle1 + x;
00419                                                 yy = angle2 + y;
00420                                                 zz = shift + z;
00421                                                 toEuklid(getAngle(xx), getAngle(yy), a, b, c);
00422                                                 aroundx += a;
00423                                                 aroundy += b;
00424                                                 aroundz += c;
00425                                                 arounds += getShift(zz);
00426                                         }
00427 
00428                                         aroundx /= around3;
00429                                         aroundy /= around3;
00430                                         aroundz /= around3;
00431                                         arounds /= around3;
00432                                         //std::cout << "Found plane size: " << val << " eq: " << aroundx <<" "<< aroundy <<" "<< aroundz <<" "<< arounds <<" "<< std::endl;
00433                                         indices.push_back(Plane<float>(aroundx, aroundy, aroundz, arounds));
00434                                         if (val > max)
00435                                                 {
00436                                                         max = val;
00437                                                         maxind = indices.size() - 1;
00438                                                 }
00439                                 }
00440                         }
00441                 }
00442                 ++it;
00443         }
00444         return maxind;
00445 
00446 }
00447 
00449 // Converts index in parameter space into angle value
00450 // @param index Angle axis index
00452 double ParameterSpaceHierarchy::getAngle(int index)
00453 {
00454         return (m_angleStep * ((double)index)) + m_anglemin;
00455 }
00456 
00458 // Converts index in parameter space into shift value
00459 // @param index Shift axis index
00461 double ParameterSpaceHierarchy::getShift(int index)
00462 {
00463         return ((double)index) * m_shiftStep + m_shiftmin;
00464 }
00465 
00467 // Returns a size of this space structure in Bytes
00469 int ParameterSpaceHierarchy::getSize()
00470 {
00471         int size = 0;
00472         for (int i = 0; i < m_loSize; ++i)
00473                 if (m_dataLowRes[i] != NULL) size += m_hiSize;
00474         return size;
00475 }
00476 
00478 // Returns a value saved at given values
00479 // @param angle1 First angle value
00480 // @param angle2 Second angle value
00481 // @param z Shift (d param) value
00483 double ParameterSpaceHierarchy::get(double angle1, double angle2, double z)
00484 {
00485         IndexStruct index = getIndex(angle1, angle2, z);
00486 
00487         if (m_dataLowRes[index.lowResolutionIndex] == NULL)
00488                         return 0.0;
00489                 else
00490                         return m_dataLowRes[index.lowResolutionIndex][index.highResolutionIndex];
00491 }
00492 
00494 // Saves a value at given values
00495 // @param angle1 First angle value
00496 // @param angle2 Second angle value
00497 // @param z Shift (d param) value
00498 // @param val Value to be saved
00500 void ParameterSpaceHierarchy::set(double angle1, double angle2, double z, double val)
00501 {
00502         IndexStruct index = getIndex(angle1, angle2, z);
00503 
00504         if (m_dataLowRes[index.lowResolutionIndex] == NULL)
00505                 {
00506                         m_dataLowRes[index.lowResolutionIndex] = (double *)malloc(sizeof(double) *m_hiSize);
00507                         memset(m_dataLowRes[index.lowResolutionIndex], 0, m_hiSize * sizeof(double));
00508                 }
00509                 m_dataLowRes[index.lowResolutionIndex][index.highResolutionIndex] = val;
00510 }
00511 
00512 


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Mon Oct 6 2014 08:07:21