$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: parameterSpace.cpp 694 2012-04-20 10:24:24Z 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 00035 #include <srs_env_model_percp/but_plane_detector/parameter_space.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; 00043 // Constructor - creates and allocates a space (angle X angle X shift) 00044 // @param anglemin Minimal angle in space 00045 // @param anglemax Maximal angle in space 00046 // @param zmin Minimal shift in space 00047 // @param zmax Maximal shift in space 00048 // @param angleRes Angle resolution (step) 00049 // @param shiftRes Shift resolution (step) 00051 ParameterSpace::ParameterSpace(double anglemin, double anglemax, double zmin, double zmax, double angleRes, double shiftRes) 00052 { 00053 m_init = false; 00054 m_angleStep = (anglemax - anglemin) / (angleRes - 1); 00055 m_shiftStep = (zmax - zmin) / (shiftRes - 1); 00056 00057 m_angleSize = (anglemax - anglemin) / m_angleStep + 1; 00058 m_angleSize2 = m_angleSize * m_angleSize; 00059 m_shiftSize = (zmax - zmin) / m_shiftStep + 1; 00060 00061 m_shiftmin = zmin; 00062 m_shiftmax = zmax; 00063 m_anglemin = anglemin; 00064 m_anglemax = anglemax; 00065 m_size = m_angleSize2 * m_shiftSize; 00066 m_data = new double[m_size]; 00067 memset(m_data, 0, sizeof(double)*m_size); 00068 } 00069 00071 // Destructor 00073 ParameterSpace::~ParameterSpace() 00074 { 00075 delete m_data; 00076 } 00077 00079 // Conversion from Euclidian representation of normal (x, y, z) to parametrized (a1, a2) 00080 // @param x X vector coordinate 00081 // @param y Y vector coordinate 00082 // @param z Z vector coordinate 00083 // @param a1 First angle 00084 // @param a2 Second angle 00086 void ParameterSpace::toAngles(float x, float y, float z, float &a1, float &a2) 00087 { 00088 a1 = atan2(z, x); 00089 // align with X on XZ 00090 x = z * sin(a1) + x * cos(a1); 00091 a2 = atan2(y, x); 00092 } 00093 00095 // Conversion from parametrized representation of normal (a1, a2) to Euclidian (x, y, z) 00096 // @param x X vector coordinate 00097 // @param y Y vector coordinate 00098 // @param z Z vector coordinate 00099 // @param a1 First angle 00100 // @param a2 Second angle 00102 void ParameterSpace::toEuklid(float a1, float a2, float &x, float &y, float &z) 00103 { 00104 float auxx = cos(-a2); 00105 y = -sin(-a2); 00106 x = auxx * cos(-a1); 00107 z = -auxx * sin(-a1); 00108 } 00109 00111 // Returns a value saved at given coordinates (indices) 00112 // @param angle1 First angle coordinate 00113 // @param angle2 Second angle coordinate 00114 // @param z Shift (d param) coordinate 00116 double &ParameterSpace::operator() (int angle1, int angle2, int z) 00117 { 00118 assert( angle1 < m_angleSize && angle1 >= 0 && angle2 < m_angleSize && angle2 >= 0 && z < m_shiftSize && z >= 0); 00119 return m_data[z * m_angleSize2 + angle2 * m_angleSize + angle1]; 00120 } 00121 00123 // Returns a value saved at given index 00124 // @param index Given index 00126 double &ParameterSpace::operator[](int index) 00127 { 00128 return m_data[index]; 00129 } 00130 00132 // 00134 int ParameterSpace::getIndex(double angle1, double angle2, double z) 00135 { 00136 assert( angle1 <= m_anglemax && angle1 >= m_anglemin && 00137 angle2 <= m_anglemax && angle2 >= m_anglemin && 00138 z >= m_shiftmin && z <= m_shiftmax); 00139 int a1 = (angle1-m_anglemin) / m_angleStep; 00140 int a2 = (angle2-m_anglemin) / m_angleStep; 00141 int zz = (z-m_shiftmin) / m_shiftStep; 00142 return zz * m_angleSize2 + a2 * m_angleSize + a1; 00143 } 00144 00146 // Returns an index from given values 00147 // @param angle1 First angle value 00148 // @param angle2 Second angle value 00149 // @param z Shift (d param) value 00151 void ParameterSpace::getIndex(double angle1, double angle2, double z, int &angle1Index, int &angle2Index, int &shiftIndex) 00152 { 00153 assert( angle1 <= m_anglemax && angle1 >= m_anglemin && 00154 angle2 <= m_anglemax && angle2 >= m_anglemin && 00155 z >= m_shiftmin && z <= m_shiftmax); 00156 angle1Index = (angle1-m_anglemin) / m_angleStep; 00157 angle2Index = (angle2-m_anglemin) / m_angleStep; 00158 shiftIndex = (z-m_shiftmin) / m_shiftStep; 00159 } 00160 00162 // Adds a second volume to this with offset 00163 // @param second Second volume to be added 00164 // @param angle1 Angle 1 offset 00165 // @param angle2 Angle 2 offset 00166 // @param shift Shift offset 00168 void ParameterSpace::addVolume(ParameterSpace &second, int angle1, int angle2, int shift) 00169 { 00170 double secondAngleHalf = second.m_angleSize / 2; 00171 double secondShiftHalf = second.m_shiftSize / 2; 00172 int shiftThis, angle1This, angle2This; 00173 int shiftS, angle1S, angle2S; 00174 00175 for (shiftS = 0, shiftThis = shift - secondShiftHalf; 00176 shiftS < second.m_shiftSize; 00177 ++shiftS, ++shiftThis) 00178 for (angle2S=0, angle2This = angle2 - secondAngleHalf; 00179 angle2S < second.m_angleSize; 00180 ++angle2S, ++angle2This) 00181 for (angle1S=0, angle1This = angle1 - secondAngleHalf; 00182 angle1S < second.m_angleSize; 00183 ++angle1S, ++angle1This) 00184 { 00185 if (angle1This >= 0 && angle1This < m_angleSize && 00186 angle2This >= 0 && angle2This < m_angleSize && 00187 shiftThis >= 0 && shiftThis < m_shiftSize) 00188 this->operator ()(angle1This, angle2This, shiftThis) += second(angle1S, angle2S, shiftS); 00189 } 00190 } 00191 00193 // Generates a Gauss curve into this space (centered) 00194 // @param angleSigma sigma in angle directions 00195 // @param shiftSigma sigma in shift direction 00197 void ParameterSpace::generateGaussIn(double angleSigma, double shiftSigma) 00198 { 00199 double angleGaussMult = 1.0;// / (angleSigma * sqrt(2 * M_PI)); 00200 double angleGauss2o2 = 2 * angleSigma * angleSigma; 00201 double shiftGaussMult = 1.0;// / (shiftSigma * sqrt(2 * M_PI)); 00202 double shiftGauss2o2 = 2 * shiftSigma * shiftSigma; 00203 00204 for (int shift=0; shift < m_shiftSize; ++shift) 00205 { 00206 double shiftReal = getShift(shift); 00207 double shiftGauss = shiftGaussMult * exp(-(shiftReal * shiftReal) / shiftGauss2o2); 00208 for (int angle2=0; angle2 < m_angleSize; ++angle2) 00209 { 00210 double angle2Real = getAngle(angle2); 00211 double angle2Gauss = angleGaussMult * exp(-(angle2Real * angle2Real) / angleGauss2o2); 00212 for (int angle1=0; angle1 < m_angleSize; ++angle1) 00213 { 00214 double angle1Real = getAngle(angle1); 00215 double angle1Gauss = angleGaussMult * exp(-(angle1Real * angle1Real) / angleGauss2o2); 00216 this->operator ()(angle1, angle2, shift) = shiftGauss * angle2Gauss * angle1Gauss; 00217 } 00218 } 00219 } 00220 } 00221 00222 00224 // Finds maximas in this and saves them as planes to given vector 00225 // @param indices Found planes 00227 int ParameterSpace::findMaxima(tPlanes &indices) 00228 { 00229 int around = 2; 00230 float a, b, c; 00231 int maxind = -1; 00232 float max = -1; 00233 00234 for (int shift=around; shift < m_shiftSize-around; ++shift) 00235 { 00236 for (int angle2=around; angle2 < m_angleSize-around; ++angle2) 00237 { 00238 for (int angle1=around; angle1 < m_angleSize-around; ++angle1) 00239 { 00240 double val = this->operator ()(angle1, angle2, shift); 00241 if (val > 1500) 00242 { 00243 val += this->operator()(angle1-1, angle2, shift) + 00244 this->operator()(angle1+1, angle2, shift) + 00245 this->operator()(angle1, angle2-1, shift) + 00246 this->operator()(angle1, angle2+1, shift) + 00247 this->operator()(angle1, angle2, shift+1) + 00248 this->operator()(angle1, angle2, shift-1); 00249 00250 bool ok = true; 00251 double aux; 00252 int xx, yy, zz; 00253 for (int x = -1; x <= 1; ++x) 00254 for (int y = -1; y <= 1; ++y) 00255 for (int z = -1; z <= 1; ++z) 00256 { 00257 xx = angle1 + x; 00258 yy = angle2 + y; 00259 zz = shift + z; 00260 aux = this->operator()(xx, yy, zz) + 00261 this->operator()(xx-1, yy, zz) + 00262 this->operator()(xx+1, yy, zz) + 00263 this->operator()(xx, yy-1, zz) + 00264 this->operator()(xx, yy+1, zz) + 00265 this->operator()(xx, yy, zz+1) + 00266 this->operator()(xx, yy, zz-1); 00267 if (val < aux) 00268 { 00269 ok = false; 00270 break; 00271 } 00272 } 00273 00274 if (ok) 00275 { 00276 double aroundx = 0; 00277 double aroundy = 0; 00278 double aroundz = 0; 00279 double arounds = 0; 00280 for (int x = -1; x <= 1; ++x) 00281 for (int y = -1; y <= 1; ++y) 00282 for (int z = -1; z <= 1; ++z) 00283 { 00284 xx = angle1 + x; 00285 yy = angle2 + y; 00286 zz = shift + z; 00287 toEuklid(getAngle(xx), getAngle(yy), a, b, c); 00288 aroundx += a; 00289 aroundy += b; 00290 aroundz += c; 00291 arounds += getShift(zz); 00292 } 00293 aroundx /= 7.0; 00294 aroundy /= 7.0; 00295 aroundz /= 7.0; 00296 arounds /= 7.0; 00297 std::cerr << "Found plane size: " << val << " eq: " << aroundx <<" "<< aroundy <<" "<< aroundz <<" "<< arounds <<" "<< std::endl; 00298 indices.push_back(Plane<float>(aroundx, aroundy, aroundz, arounds)); 00299 if (val > max) 00300 { 00301 max = val; 00302 maxind = indices.size() - 1; 00303 } 00304 } 00305 } 00306 } 00307 } 00308 } 00309 return maxind; 00310 } 00311 00313 // Converts index in parameter space into angle value 00314 // @param index Angle axis index 00316 double ParameterSpace::getAngle(int index) 00317 { 00318 return (m_angleStep * ((double)index)) + m_anglemin; 00319 } 00320 00322 // Converts index in parameter space into shift value 00323 // @param index Shift axis index 00325 double ParameterSpace::getShift(int index) 00326 { 00327 return ((double)index) * m_shiftStep + m_shiftmin; 00328 } 00329 00331 // Returns a value saved at given values 00332 // @param angle1 First angle value 00333 // @param angle2 Second angle value 00334 // @param z Shift (d param) value 00336 double &ParameterSpace::operator() (double angle1, double angle2, double z) 00337 { 00338 return m_data[getIndex(angle1, angle2, z)]; 00339 } 00340 00342 // Returns a size of this space structure in Bytes 00344 int ParameterSpace::getSize() 00345 { 00346 return m_angleSize * m_angleSize * m_shiftSize; 00347 } 00348