00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "../hough_transform.h"
00027 #include <utils/time/tracker.h>
00028 #include <utils/math/angle.h>
00029 #include <utils/math/coord.h>
00030
00031 #include <cstdio>
00032 #include <unistd.h>
00033
00034 using namespace fawkes;
00035
00036 int
00037 main(int argc, char **argv)
00038 {
00039 HoughTransform *ht = new HoughTransform(2);
00040
00041 unsigned int num_vals = 24;
00042 int angle_step = 360 / num_vals;
00043 float r_scale = 100.;
00044
00045 int **values = new int*[num_vals];
00046 for (unsigned int i = 0; i < num_vals; ++i) {
00047 values[i] = new int[2];
00048 }
00049
00050 float samples[][2] =
00051 { { 0, 1}, { 1, 0},
00052 { 0, 1}, {-1, 0},
00053 {-1, 0}, { 0, -1},
00054 { 1, 0}, { 0, -1},
00055 { 0, 1}, { 1, 1},
00056 { 1, 0}, { 1, 1},
00057 { 0, -1}, { 1, -1},
00058 {-1, 0}, {-1, 1}
00059 };
00060
00061 printf("Num samples: %zu\n", (sizeof(samples)/sizeof(float *))/2);
00062
00063 for (size_t S = 0; S < (sizeof(samples)/sizeof(float *))/2; ++S) {
00064 float x[2], y[2];
00065 x[0] = samples[2 * S ][0]; y[0] = samples[2 * S ][1];
00066 x[1] = samples[2 * S + 1][0]; y[1] = samples[2 * S + 1][1];
00067
00068 ht->reset();
00069
00070 for (unsigned int i = 0; i < 2; ++i) {
00071 for (unsigned int j = 0; j < num_vals; ++j) {
00072 float theta = deg2rad(j * angle_step);
00073 float r = x[i] * cos(theta) + y[i] * sin(theta);
00074 r *= r_scale;
00075 values[j][0] = (int)roundf(r);
00076 values[j][1] = j * angle_step;
00077
00078
00079 }
00080 ht->process(values, num_vals);
00081 }
00082
00083 int max_values[2];
00084 unsigned int max_count = ht->max(max_values);
00085 printf("Max count: %u (%i, %i)\n", max_count, max_values[0],
00086 max_values[1]);
00087
00088 float phi = deg2rad(max_values[1]);
00089 float r = max_values[0] / r_scale;
00090 float x1, y1, x2, y2;
00091 polar2cart2d(phi, r, &x1, &y1);
00092
00093 float y_factor = 1;
00094 float alpha;
00095 if ( ((max_values[1] >= 0) && (max_values[1] < 90)) ||
00096 (max_values[1] >= 270) ) {
00097 y_factor = -1;
00098 alpha = deg2rad(90 - (max_values[1] % 90));
00099 } else {
00100 alpha = deg2rad((max_values[1] % 90));
00101 }
00102 float dx = 1 * cos(alpha);
00103 float dy = 1 * y_factor * sin(alpha);
00104 x2 = x1 + dx;
00105 y2 = y1 + dy;
00106
00107 printf("p1=(%f,%f) p2=(%f, %f)\n", x[0], y[0], x[1], y[1]);
00108 printf("r=%f phi=%f alpha=%f dx=%f dy=%f p1=(%f,%f) p2=(%f,%f)\n\n",
00109 r, phi, alpha, dx, dy, x1, y1, x2, y2);
00110
00111 }
00112
00113 delete ht;
00114 for (unsigned int i = 0; i < num_vals; ++i) {
00115 delete[] values[i];
00116 }
00117 delete[] values;
00118
00119 return 0;
00120 }
00121
00122