HoughIndexCalculator.cpp
Go to the documentation of this file.
00001 /*******************************************************************************
00002  *  HoughIndexCalculator.cpp
00003  *
00004  *  (C) 2007 AG Aktives Sehen <agas@uni-koblenz.de>
00005  *           Universitaet Koblenz-Landau
00006  *
00007  *  Additional information:
00008  *  $Id: $
00009  *******************************************************************************/
00010 
00011 #include "HoughIndexCalculator.h"
00012 
00013 #include "Architecture/Config/Config.h"
00014 #include "Architecture/Singleton/Clock.h"
00015 
00016 #include "Workers/Math/Math.h"
00017 #include "Workers/Math/vec2.h"
00018 
00019 #include <algorithm>    // for max_element
00020 #include <assert.h>
00021 #include <map>
00022 #include <list>
00023 #include <math.h>
00024 #include <cstring>
00025 #include <fstream>
00026 #include <ros/ros.h>
00027 
00028 using namespace std;
00029 
00030 #define THIS HoughIndexCalculator
00031 
00032 void THIS::calculateScaleIndex(double scaleQuotient, int& scaleIndexFloor, int& scaleIndexCeil)
00033 {
00034   int scaleBins = Config::getInt( "ObjectRecognition.HoughClustering.iScaleBins" );
00035 
00036   //( log2( scaleQuotient ) -> anzahl der verdoppelungen(octaven) und halbierungen(octaven)  log2( ]0..inf] ) = [-inf..inf]
00037   //( log2( scaleQuotient ) / (maxOctaves-1) -> [-inf..inf] (-1 damit scalierung 1 und nicht nur octaven dazukommen)
00038   //( log2( scaleQuotient ) / (maxOctaves-1) / 2 + 0.5 ) -> wertebereich verschieben (bei scaleQuotient < 1 ist log2 negativ)
00039   //( log2( scaleQuotient ) / (maxOctaves-1) / 2 + 0.5 ) * scaleBins; -> auf anzahl der bins hochskalieren
00040 
00041   //Bsp: scaleQuotient 0.5 -> bin 3
00042   //Bsp: scaleQuotient 1 -> bin 5, also noch verdoppelungen und 4 halbierungen möglich
00043   //Bsp: scaleQuotient 2 -> bin 6
00044 
00045   /* Scale Index: a factor of 2 for scale (log2), 1/4 size to 4 times */
00046   int maxOctaves=5;
00047   float scaleIndex = ( log2( scaleQuotient ) / (maxOctaves-1) / 2 + 0.5 ) * scaleBins;
00048 
00049   //put all scales that are too large in last bin for ceil
00050   if ( scaleIndex >= scaleBins ) {
00051     scaleIndex = scaleBins-1;
00052   }
00053 
00054   if ( scaleIndex < 0 ) {
00055     scaleIndex = 0;
00056   }
00057 
00058   scaleIndexFloor = scaleIndex;
00059   scaleIndexCeil = scaleIndex+1;
00060 
00061   if(scaleIndexCeil >= scaleBins)
00062   {
00063     scaleIndexCeil = scaleBins-1;
00064   }
00065 }
00066 
00067 void THIS::calculateOrientationIndex(double turnAngle, int& orientationFloor, int& orientationCeil)
00068 {
00069   int orientationBins = Config::getInt( "ObjectRecognition.HoughClustering.iOrientationBins" );
00070 
00071   //turnAngle in radians (-PI to PI)
00072   
00073   if(turnAngle<-M_PI || turnAngle>M_PI)
00074   {
00075     ROS_ERROR_STREAM("Orientation "<< turnAngle);
00076   }
00077 
00078   //( turnAngle+M_PI ) -> [0..2*PI]
00079   //( turnAngle+M_PI ) /M_PI/2.0 -> [0..1]
00080   //( turnAngle+M_PI ) /M_PI/2.0 * orientationBins -> [0..orientationBins] float
00081 
00082   float orientationIndex = ( turnAngle+M_PI ) /M_PI/2.0 * orientationBins;
00083 
00084   assert( orientationIndex >= 0.0 );
00085 
00086   //int(orientationIndex) % orientationBins; -> [0..orientationBins] int
00087   orientationFloor = int(orientationIndex) % orientationBins;
00088   orientationCeil = ( orientationFloor+1 ) % orientationBins;
00089 }
00090 
00091 void THIS::calculatePositionIndex(KeyPoint sceneKeyPoint, KeyPoint objectKeyPoint, Point2D center, int w, int h, int& xDistanceFloor, int& xDistanceCeil, int& yDistanceFloor, int& yDistanceCeil)
00092 {
00093   int xLocationBins = Config::getInt( "ObjectRecognition.HoughClustering.iXLocationBins" );
00094   int yLocationBins = Config::getInt( "ObjectRecognition.HoughClustering.iYLocationBins" );
00095 
00096   //calculate offset between object and scene keypoints
00097   Point2D objectPoint = objectKeyPoint.position();
00098   Point2D scenePoint = sceneKeyPoint.position();
00099 
00100   //points from objectPoint to objectCenter
00101   Point2D v = center - objectPoint;
00102   v *= sceneKeyPoint.scale/ objectKeyPoint.scale;
00103   v.rotate ( Math::minTurnAngle(sceneKeyPoint.orientation, objectKeyPoint.orientation) );
00104 
00105   //position of object center in scene
00106   Point2D centerScene = v + scenePoint;
00107 
00108   //compute index depending on image dimension
00109   float xPositionIndex = (centerScene.x()/w)*xLocationBins;
00110   float yPositionIndex = (centerScene.y()/h)*yLocationBins;
00111 
00112   //values could be equal at border (don't just use ceil() and floor() )
00113   xDistanceFloor = xPositionIndex;
00114   xDistanceCeil = xDistanceFloor+1;
00115 
00116   yDistanceFloor = yPositionIndex;
00117   yDistanceCeil = yDistanceFloor+1;
00118 }
00119 
00120 #undef THIS


or_libs
Author(s): raphael
autogenerated on Mon Oct 6 2014 02:53:18