Go to the documentation of this file.00001 #include <vector>
00002 #include <cmath>
00003 #include <algorithm>
00004 #include <list>
00005 #include <numeric>
00006 #include <iostream>
00007 #include <gsl/gsl_errno.h>
00008 #include <gsl/gsl_fft_real.h>
00009 #include <gsl/gsl_fft_halfcomplex.h>
00010 #include <gsl/gsl_fft_complex.h>
00011
00012 #include <pm_fourier/angle_shift.h>
00013 #include <lama_common/point.h>
00014
00015 using std::vector;
00016 using lama_common::Point2;
00017
00018 double getSimpleDissimilarityFft(const vector<double> &fft1, const vector<double> &fft2, const int size)
00019 {
00020
00021 if (fft1.size() != fft2.size() || fft1.size() == 0 || fft2.size() == 0)
00022 {
00023 std::cerr << __FUNCTION__ << ": desA.size()!=desB.size() (" << fft1.size();
00024 std::cerr << "!=" << fft2.size() << ")!\n";
00025 return 0;
00026 }
00027
00028 size_t size_ok = size;
00029 if (size_ok > fft1.size() - 1)
00030 size_ok = fft1.size() - 1;
00031
00032 vector<double> a(size_ok, 0.0);
00033 vector<double> b(size_ok, 0.0);
00034
00035 for(size_t i = 0; i < size_ok; i++)
00036 {
00037 a[i] = sqrt(fft1[2 * i] * fft1[2 * i] + fft1[2 * i + 1] * fft1[2 * i + 1]);
00038 b[i] = sqrt(fft2[2 * i] * fft2[2 * i] + fft2[2 * i + 1] * fft2[2 * i + 1]);
00039
00040
00041
00042 }
00043
00044 double r = 0;
00045 for(size_t i = 0; i < a.size(); i++)
00046 {
00047 r += sqrt((a[i] - b[i]) * (a[i] - b[i]));
00048 }
00049 r /= a.size();
00050 return r;
00051 }
00052
00053
00058 static vector<double> convertPolygonToRange(const vector<Point2> &polygon)
00059 {
00060 vector<double> r;
00061 r.reserve(polygon.size());
00062
00063 double sx = 0;
00064 double sy = 0;
00065
00066
00067 for (size_t i = 0; i < polygon.size(); i++)
00068 {
00069 sx += polygon[i].x;
00070 sy += polygon[i].y;
00071 }
00072 sx /= (double)polygon.size();
00073 sy /= (double)polygon.size();
00074
00075 for (size_t i = 0; i< polygon.size(); i++)
00076 {
00077 const double dx = sx - polygon[i].x;
00078 const double dy = sy - polygon[i].y;
00079 const double d = std::sqrt(dx * dx + dy * dy);
00080 r.push_back(d);
00081 }
00082 return r;
00083 }
00084
00085
00086 double getDissimilarityFourier(const vector<Point2> &polygon1, const vector<Point2> &polygon2, const int fftSize)
00087 {
00088 if (fftSize <= 0)
00089 {
00090 std::cerr << "fftSize must be >0 !\n";
00091 }
00092
00093 vector<double> range1(convertPolygonToRange(polygon1));
00094 vector<double> range2(convertPolygonToRange(polygon2));
00095
00096 vector<double> f1(fft2(range1));
00097 vector<double> f2(fft2(range2));
00098 vector<double> a1;
00099 vector<double> a2;
00100
00101 a1.reserve(f1.size());
00102 a2.reserve(f1.size());
00103
00104 for(int i=0;i<(int)f1.size()/2;i++) {
00105 a1.push_back(f1[2*i]);
00106 a1.push_back(f1[2*i+1]);
00107
00108 a2.push_back(f2[2*i]);
00109 a2.push_back(f2[2*i+1]);
00110 }
00111
00112
00113 const double distance = getSimpleDissimilarityFft(a1,a2,fftSize);
00114
00115 range1.clear();
00116 range2.clear();
00117 f1.clear();
00118 f2.clear();
00119 a1.clear();
00120 a2.clear();
00121
00122 return distance;
00123
00124 }
00125
00126
00127
00128 double getDissimilarityCorrelation(const vector<double> &range1, const vector<double> &range2)
00129 {
00130 vector<double> f1(fft2(range1));
00131 vector<double> f2(fft2(range2));
00132
00133 double val;
00134 getAngleShiftFFT(f1, f2, val);
00135
00136 return val;
00137 }
00138
00139
00140
00141 double getDissimilarityCorrelation(const int n1, const int n2, double *des1, double *des2)
00142 {
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153 return 0;
00154 }
00155
00156 double getDissimilarityNccFft(const vector<double> &fft1, const vector<double> &fft2, const int size)
00157 {
00158 if (fft1.size() != fft2.size() || fft1.size() == 0 || fft2.size() == 0)
00159 {
00160 std::cerr << __FUNCTION__ << ": desA.size()!=desB.size() (" << fft1.size();
00161 std::cerr << "!=" << fft2.size() << ")!\n";
00162 return 0;
00163 }
00164
00165 size_t size_ok = size;
00166 if (size_ok > fft1.size() - 1)
00167 size_ok = fft1.size() - 1;
00168
00169 vector<double> a(size_ok, 0.0);
00170 vector<double> b(size_ok, 0.0);
00171
00172 for(size_t i = 0; i < size_ok; i++)
00173 {
00174 a[i] = fft1[2 * i] * fft1[2 * i] + fft1[2 * i + 1] * fft1[2 * i + 1];
00175 b[i] = fft2[2 * i] * fft2[2 * i] + fft2[2 * i + 1] * fft2[2 * i + 1];
00176 }
00177
00178 double meanA = std::accumulate(a.begin(), a.end(), 0.0) / a.size();
00179 double meanB = std::accumulate(b.begin(), b.end(), 0.0) / b.size();
00180
00181 for (size_t i = 0; i < a.size(); i++)
00182 {
00183 a[i] = a[i] - meanA;
00184 b[i] = b[i] - meanB;
00185 }
00186
00187 double sa = 0;
00188 double sb = 0;
00189 for(size_t i = 0; i < a.size(); i++)
00190 {
00191 sa += a[i] * a[i];
00192 sb += b[i] * b[i];
00193 }
00194 sa = sqrt(sa / a.size());
00195 sb = sqrt(sb / b.size());
00196
00197 if (sa == 0 || sb == 0)
00198 {
00199 return 0;
00200 }
00201
00202 for (size_t i = 0; i < a.size(); i++)
00203 {
00204 a[i] = a[i] / sa;
00205 b[i] = b[i] / sb;
00206 }
00207
00208 double r = 0;
00209 for(int i = 0; i < a.size(); i++)
00210 {
00211 r += a[i] * b[i];
00212 }
00213 r /= a.size();
00214
00215 r = (r + 1) / 2.0;
00216
00217
00218 r = (10000.0 * r - 9900.0) / 100.0;
00219 if (r < 0)
00220 return 0;
00221
00222 return r;
00223 }
00224