26 #include "../hough_transform.h"
27 #include <utils/time/tracker.h>
28 #include <utils/math/angle.h>
29 #include <utils/math/coord.h>
37 main(
int argc,
char **argv)
41 unsigned int num_vals = 24;
42 int angle_step = 360 / num_vals;
45 int **values =
new int*[num_vals];
46 for (
unsigned int i = 0; i < num_vals; ++i) {
47 values[i] =
new int[2];
61 printf(
"Num samples: %zu\n", (
sizeof(samples)/
sizeof(
float *))/2);
63 for (
size_t S = 0; S < (
sizeof(samples)/
sizeof(
float *))/2; ++S) {
65 x[0] = samples[2 * S ][0]; y[0] = samples[2 * S ][1];
66 x[1] = samples[2 * S + 1][0]; y[1] = samples[2 * S + 1][1];
70 for (
unsigned int i = 0; i < 2; ++i) {
71 for (
unsigned int j = 0; j < num_vals; ++j) {
72 float theta =
deg2rad(j * angle_step);
73 float r = x[i] * cos(theta) + y[i] * sin(theta);
75 values[j][0] = (int)roundf(r);
76 values[j][1] = j * angle_step;
84 unsigned int max_count = ht->
max(max_values);
85 printf(
"Max count: %u (%i, %i)\n", max_count, max_values[0],
88 float phi =
deg2rad(max_values[1]);
89 float r = max_values[0] / r_scale;
95 if ( ((max_values[1] >= 0) && (max_values[1] < 90)) ||
96 (max_values[1] >= 270) ) {
98 alpha =
deg2rad(90 - (max_values[1] % 90));
100 alpha =
deg2rad((max_values[1] % 90));
102 float dx = 1 * cos(alpha);
103 float dy = 1 * y_factor * sin(alpha);
107 printf(
"p1=(%f,%f) p2=(%f, %f)\n", x[0], y[0], x[1], y[1]);
108 printf(
"r=%f phi=%f alpha=%f dx=%f dy=%f p1=(%f,%f) p2=(%f,%f)\n\n",
109 r, phi, alpha, dx, dy, x1, y1, x2, y2);
114 for (
unsigned int i = 0; i < num_vals; ++i) {
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.