josch/3dpcp
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
3dpcp/src/grid/hough.cc
josch 0281b65175 initial commit
2012-09-16 14:33:11 +02:00

991 lines
28 KiB
C++
Raw Blame History

/*
* 2D hough transform for line detection implementation
*
* Copyright (C) Hartmut Surmann
*
* Released under the GPL version 3.
*
*/
/***************************************************************************
* hough.c
***************************************************************************
* PROJECT : 3D-LASER
* AUTHOR : Hartmut Surmann
***************************************************************************
* DESCRIPTION
* Implements some basic hough functions to detect lines in laserscans
* It writes a GnuPlot file in the debug case.
* NOTE : This is only a test program, it is'nt intended to be the
* definitive evaluation tool.
***************************************************************************
* SYNTAX : hough [depth level]
* ARGUMENTS
* depth level : default is ScanAZ which means find all lines
: default datafile ist test20.dat
***************************************************************************/
/* COMPILE it with (it needs test20.dat)
gcc -o hough -g -O2 -m486 -Wall hough.c -lm -DSTANDALONE
gcc -o hough -pg -a -O2 -m486 -Wall hough.c -lm -DSTANDALONE
*/
#include "grid/hough.h"
#include <iostream>
/***************************************************************************
* CONSTANTS & MACROS
***************************************************************************/
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
/***********************FOR HOUGH TRANSFORMATION***************************/
int DATA_RANGE = 725;
int ScanAZ = 721;
#define SKIP 0
#define EPSILON 3.1 // absoluter abstand zwischen geradengleichung und wert
//#define DIST 600 // wert zum quadrat ab diesem Wert haben wir eine Luecke
#define RAD2DEG 57.29578 // umrechnungsfaktor bogenmass grad
//#define DIST 6.9 // ab diesem Wert haben wir eine Luecke
#define MIN(a,b) ((a)<(b)?(a):(b))
#define MAX(a,b) ((a)>(b)?(a):(b))
#define SQR(a) ((a)*(a))
/*********************** HOUGH TRANSFORMATION***************************/
/***************************************************************************
* DATA STRUCTURE DEFINITIONS
***************************************************************************/
/***************************************************************************
* PUBLIC GLOBAL VARIABLES
***************************************************************************/
double *sin_theta = NULL, *cos_theta = NULL;
int max_trigo_size = 0;
int **k_index;
double alpha = M_PI / ScanAZ;
int MAX_DISTANCE = 750; // war vorher define jetzt von aussen manipulierbar
#ifdef STANDALONE
/***************************************************************************
* PRIVATE FUNCTION DEFINITIONS
***************************************************************************/
/**
*-------------------------------------------------------------------------*
* NAME : AddNumberToFileName
* DESCRIPTION
* Add a number to a filename. The number will be inserted at the end of
* the name an before the last extension or '.'
* PARAMETERS
* 'src' : The original file name.
* 'dest' : The resulting file name.
* 'number' : The number to add.
* RESULT
* A pointer to the buffer in which is stored the resulting file name.
* Always equal to 'dest'
*-------------------------------------------------------------------------
*/
char *AddNumberToFileName(char *src, char *dest, long number)
{
long aux;
/* Find the last '.' */
aux = strlen(src);
while ( (src[aux] != '.') && (aux >= 0))
aux--;
/* Adds the number */
if (aux == -1)
sprintf(dest, "%s%03ld", src, number);
else
{
strncpy(dest, src, aux);
sprintf(dest + aux, "%03ld%s",number, src + aux);
}
return dest;
}
#else //STANDALONE
extern char *AddNumberToFileName(char *src, char *dest, long number);
//extern unsigned long GetCurrentTimeInMilliSec(void);
//extern __inline__ unsigned long long int rdtsc();
#endif
/**************************************************************************/
/***********************START HOUGH TRANSFORMATION***************************/
/**************************************************************************/
#define dist_func(x1,x2,y1,y2) ((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))
/**
*
*/
double distance_func(double x1,double x2,
double y1, double y2)
{
double x,y;
x = x1 - x2;
y = y1 - y2;
x = x * x;
y = y * y;
return (sqrt(x + y));
}
/**
* compute sin(theta) table from 0 to resolution
*/
int calc_sin_theta(int resolution, double *sin_theta)
{
int j;
double resolution_d = (double)resolution; /* (double) form of resolution */
double theta;
for (j=0; j < resolution; j++) {
theta = M_PI * (j/(resolution_d) - 0.5);
sin_theta[j] = sin(theta);
}
return 0;
}
/**
* compute cos(theta) table from 0 to resolution
*/
int calc_cos_theta(int resolution, double *cos_theta)
{
int j;
double resolution_d = (double)resolution; /* (double) form of resolution */
double theta;
for (j=0; j < resolution; j++) {
theta = M_PI * (j/(resolution_d) - 0.5);
cos_theta[j] = cos(theta);
}
return 0;
}
/**
*
*/
void SHT_set_max_distance(int max_distance, int drange)
{
MAX_DISTANCE = max_distance;
DATA_RANGE = drange + 2;
ScanAZ = drange;
}
/**
*
*/
int SHT_get_max_distance(void)
{
return(MAX_DISTANCE);
}
/**
* Allocate space for the histogram.
* histogram is an array of pointers to arrays.
*/
int **SHT_alloc_histogram(int x_max, int y_max)
{
int x;
int **histogram;
histogram = (int **)calloc(x_max, sizeof(int *));
if (histogram == NULL) {
fprintf(stderr, "malloc failed in alloc_histogram() in sht_line.c\n");
exit(1);
}
for (x = 0; x < x_max; x++) {
histogram[x] = (int *)calloc(y_max , sizeof(int));
if (histogram[x] == NULL) {
fprintf(stderr, "malloc failed in alloc_histogram() in sht_line.c\n");
exit(1);
}
}
return(histogram);
}
/**
*sin_theta and cos_theta must be global
*/
int sht_init(int sht_resolution)
{
int i;
if (max_trigo_size < sht_resolution) {
sin_theta = (double *) realloc(sin_theta,sizeof(double)*sht_resolution);
cos_theta = (double *) realloc(cos_theta,sizeof(double)*sht_resolution);
max_trigo_size = sht_resolution;
}
calc_sin_theta(sht_resolution, sin_theta);
calc_cos_theta(sht_resolution, cos_theta);
k_index = (int **)malloc(sizeof(int *)*DATA_RANGE);
for (i = 0; i < DATA_RANGE; i++) {
k_index[i] = (int *)malloc(sizeof(int)*sht_resolution);
}
return 0;
}
/**
*
*/
void SHT_init_histogram(int **histogram, int x_max, int y_max)
{
int x, y;
for (x=0; x < x_max; x++) {
for (y=0; y < y_max; y++) {
histogram[x][y] = 0;
}
}
}
/**
*
*/
void SHT_free_histogram(int **histogram, int x_max)
{
int x,i;
for (x = 0; x < x_max; x++) {
free(histogram[x]);
}
free(histogram);
for (i = 0; i < DATA_RANGE; i++) {
free(k_index[i]);
}
}
/**
* nr_pts are the number of scan points to be used (black points)
* resolution is the resolution used over
* e.g. resolution = 100
* then we produce a 100 x 100 histogram which covers the
* relevent area of rho-theta space.
* histogram is stored as an array of pointers to arrays.
*/
int SHT_hough_transform(int nr_pts, double *x, double *y,
int resolution, double max_rho_d, int **histogram,
double xmin, double ymin)
{
//wykobi::trigonometry_tables;
int i, j, k;
double rho;
int res_half = resolution >> 1;
double mult_factor = resolution/(2.0*max_rho_d);
/* Find Hough transform of points. */
for (i=0; i < nr_pts; i++) {
if ((fabs(x[i]+xmin) < MAX_DISTANCE) && (fabs(y[i]+ymin) < MAX_DISTANCE)){
for (j=0; j < resolution; j++) {
rho = cos_theta[j]*(double )x[i] + sin_theta[j]*(double )y[i];
k = (int)(rho * mult_factor) + res_half;
if ((k < 0) || (k >= resolution))
printf("ERROR: %d %f %f %d %f %d %f %f %f\n",
i,x[i],y[i],j,rho,k,max_rho_d,xmin,ymin);
else {
histogram[j][k]++;
k_index[i][j] = k;
}
}
}
}
return 0;
}
/**
* Find average value of histogram (only consider
* non-zero entries in histogram).
* Use this as a model of the background noise value
* of the histogram.
* When the largest histogram value is less than this
* average, then we will have removed all maxima from
* histogram.
*/
unsigned long SHT_Find_average_value_of_historgram(int resolution, int **histogram)
{
unsigned long sum = 0, average = 0, count = 0;
int i,j;
for (i=0; i < resolution; i++) {
for (j=0; j < resolution; j++) {
if (histogram[i][j] > 0) {
sum += histogram[i][j];
count++;
}
}
}
if (count != 0) average = sum / count;
return average;
}
/**
* Find max of histogram
*/
unsigned long SHT_Find_max_value_of_historgram(int resolution, int **histogram,
int *imax, int *jmax)
{
register int maximum = 0;
register int *startptr;
register int i,j;
*imax=0; *jmax = 0;
for (i=0; i < resolution; i++)
for (j=0, startptr = histogram[i];j < resolution; startptr++,j++) {
if (*startptr > maximum) {
maximum = *startptr;
*imax = i; *jmax = j;
}
}
return maximum;
}
/**
*
*/
int SHT_Print_histogram(FILE *fpr, int resolution, int **histogram, int average)
{
int i, j;
fprintf(fpr,"# %d\n", resolution);
for (i=0; i < resolution; i++) {
for (j=0; j < resolution; j++) {
if (histogram[i][j] > 0) fprintf(fpr,"%d %d %d\n", i, j,histogram[i][j]);
}
fprintf(fpr,"\n");
}
return 0;
}
/**
* The line is in the form:
* y = mx + b
* and |m| <= 1.0
*/
int SHT_calc_line(int resolution, double max_rho_d, int **histogram,
int i, int j, double *m, double *b)
{
double rho;
double resolution_d = (double)resolution;
int ret_val = 0;
rho = 2.0*max_rho_d * ( (((double )j) - resolution_d/2.0) / resolution_d );
*m = -cos_theta[i]/sin_theta[i];
if (fabs(*m) <= 1.0) {
*b = rho/sin_theta[i];
ret_val = 0;
}
else {
*m = 1.0/ *m;
*b = rho/cos_theta[i];
ret_val = 1;
}
return ret_val;
}
/**
*
*/
int SHT_show_line(int resolution, double max_rho_d, int **histogram,
int i, int j, double *x0, double *y0,
double *x1, double *y1)
{
double m, b;
int go_over_x = 0;
go_over_x = SHT_calc_line(resolution, max_rho_d, histogram, i, j, &m, &b);
if (!go_over_x) {
*x0 = -150; *x1 = 150;
*y0 = m * *x0+b; *y1 = m * *x1+b;
}
else {
*y0 = 0.0; *y1 = 300;
*x0 = m* *y0+b; *x1 = m* *y1+b;
}
return 0;
}
// hier weiter harti 17.08.2000 fprint ersetzen in x bzw y_line_pts
double SHT_find_line(int nr_pts, double *x, double *y,
int resolution, double max_rho_d, int **histogram,
int ipkt, int jpkt, int *marker,
double length_of_a_line,
int *nr_line_pts,
double *x_line_pts, double *y_line_pts,
double xmin, double ymin)
{
double m, b;
int count = 0;
double x0,y0,xa,xe,ya,ye;
double *lx = new double[DATA_RANGE];
double *ly = new double[DATA_RANGE];
int *index = new int[DATA_RANGE];
int lastindex = 0;
double *dist = new double[DATA_RANGE];
double maxlinedist = 0.0, linedist = 0.0;
int go_over_x = 0;
int i, j;
go_over_x = SHT_calc_line(resolution, max_rho_d, histogram, ipkt, jpkt, &m, &b);
/* Find Hough transform of points. */
for (i=0; i < nr_pts; i++) {
if ((fabs(x[i]+xmin) < MAX_DISTANCE) && (fabs(y[i]+ymin) < MAX_DISTANCE)){
x0 = (m*y[i]+b+0.5); // +0.5 because int truncate
y0 = (m*x[i]+b+0.5); // +0.5 because int truncate
if ( ((marker[i] == 4) || (marker[i] == 0)) && (((!go_over_x) && (fabs(y[i] -y0)< EPSILON)) ||
(( go_over_x) && (fabs(x[i] -x0)< EPSILON)))) {
// now we are on the line
// lx contains all points which are on the line
lx[count] = x[i]; ly[count] = y[i]; index[count]=i;
// dist contains the differents between the points on the line
if (count > 0)
dist[count] = dist_func(x[i],lx[count-1],
y[i],ly[count-1]);
else
dist[count]=0;
if (dist[count] > 2500 ) {
linedist = dist_func(lx[count-1],lx[lastindex],
ly[count-1], ly[lastindex]);
if (linedist > maxlinedist) maxlinedist = linedist;
// is the founded line long enough
if (linedist > length_of_a_line) {
// ok show the line
xa = (m*ly[lastindex]+b+0.5);
xe = (m*ly[count-1]+b+0.5);
ya = (m*lx[lastindex]+b+0.5);
ye = (m*lx[count-1]+b+0.5);
x_line_pts[*nr_line_pts] = (go_over_x?xa:lx[lastindex]);
y_line_pts[(*nr_line_pts)++] = (go_over_x?ly[lastindex]:ya);
x_line_pts[*nr_line_pts] = (go_over_x?xe:lx[count-1]);
y_line_pts[(*nr_line_pts)++] = (go_over_x?ly[count-1]:ye);
// mark all these points
for (j = lastindex; j <= count-1;j++) {
if (marker[index[j]] == 4) marker[index[j]] = 1;
else marker[index[j]] = 3;
}
}
lastindex=count;
}
count++;
}
}
}
linedist = dist_func(lx[count-1] ,lx[lastindex],
ly[count-1], ly[lastindex]);
if (linedist > maxlinedist) maxlinedist = linedist;
// is the founded line long enough
if (linedist > length_of_a_line) {
// ok show the line
xa = (int)(m*ly[lastindex]+b+0.5);
xe = (int)(m*ly[count-1]+b+0.5);
ya = (int)(m*lx[lastindex]+b+0.5);
ye = (int)(m*lx[count-1]+b+0.5);
x_line_pts[*nr_line_pts] = (go_over_x?xa:lx[lastindex]);
y_line_pts[(*nr_line_pts)++] = (go_over_x?ly[lastindex]:ya);
x_line_pts[*nr_line_pts] = (go_over_x?xe:lx[count-1]);
y_line_pts[(*nr_line_pts)++] = (go_over_x?ly[count-1]:ye);
// mark all these points
for (j = lastindex; j <= count-1;j++) marker[index[j]] = 1;
if (count-1 > lastindex) {
marker[index[lastindex]] = 3; /* anfangs und endpunkte extra markieren */
marker[index[count-1]] = 3;
}
}
delete[] lx;
delete[] ly;
delete[] index;
delete[] dist;
return maxlinedist;
}
/**
* falls wir nicht einen orginal scan haben sondern nur wild unsotierte Punkte
* 1cm genauigkeit 2 cm luecke wird akzeptiert
*/
double SHT_find_unsorted_line(int nr_pts, double *x, double *y,
int resolution, double max_rho_d, int **histogram,
int ipkt, int jpkt, int *marker,
double length_of_a_line,
int *nr_line_pts,
double *x_line_pts, double *y_line_pts,
double xmin, double ymin)
{
double m, b;
int count = 0;
double x0,y0;
double maxlinedist = 0.0, linedist = 0.0;
int go_over_x = 0;
int i;
int maxlen_index = 0, minlen_index = 0, *line_pts, *li, startpkt = 0, luecke = 0, lasti= 0;
double len = 0.0, maxlen = 0.0, minlen = 0;
int ref_index = -1;
go_over_x = SHT_calc_line(resolution, max_rho_d, histogram, ipkt, jpkt, &m, &b);
/* Find Hough transform of points. */
// first calc maximal len of all possible lines
for (i=0; i < nr_pts; i++) {
if ((fabs(x[i]+xmin) < MAX_DISTANCE) && (fabs(y[i]+ymin) < MAX_DISTANCE)){
x0 = (m*y[i]+b+0.5); // +0.5 because int truncate
y0 = (m*x[i]+b+0.5); // +0.5 because int truncate
if ( ((marker[i] == 4) || (marker[i] == 0)) && (((!go_over_x) && (fabs(y[i] -y0)< EPSILON)) ||
(( go_over_x) && (fabs(x[i] -x0)< EPSILON)))) {
if (ref_index == -1) { // init
ref_index = i;
maxlen = minlen = 0.0;
maxlen_index = minlen_index = i;
len = 0.0;
}
else {
len = distance_func(x[i], x[ref_index],
y[i], y[ref_index]);
if (go_over_x) {
if ((y[i] - y[ref_index]) < 0 ) {
len *= -1.0;
if (len < minlen) {
minlen = len;
minlen_index = i;
}
}
else {
if (len > maxlen) {
maxlen = len;
maxlen_index = i;
}
if (len < minlen) {
minlen = len;
minlen_index = i;
}
}
}
else { // go over y
if ((x[i] - x[ref_index]) < 0 ) {
len *= -1.0;
if (len < minlen) {
minlen = len;
minlen_index = i;
}
if (len > maxlen) {
maxlen = len;
maxlen_index = i;
}
}
else {
if (len < minlen) {
minlen = len;
minlen_index = i;
}
if (len > maxlen) {
maxlen = len;
maxlen_index = i;
}
}
}
} // else loop
count++;
} // on the point
} // in distance
} // loop
maxlinedist = distance_func(x[maxlen_index], x[minlen_index],
y[maxlen_index], y[minlen_index]);
line_pts = (int *) calloc((int)(maxlinedist+200),sizeof(int));
li = (int *) calloc((int)(2.0*maxlinedist+200),sizeof(int));
// now calc historgramm of the line
for (i=0; i < nr_pts; i++) {
if ((fabs(x[i]+xmin) < MAX_DISTANCE) && (fabs(y[i]+ymin) < MAX_DISTANCE)){
x0 = (m*y[i]+b+0.5); // +0.5 because int truncate
y0 = (m*x[i]+b+0.5); // +0.5 because int truncate
if ( ((marker[i] == 4) || (marker[i] == 0)) && (((!go_over_x) && (fabs(y[i] -y0)< EPSILON)) ||
(( go_over_x) && (fabs(x[i] -x0)< EPSILON)))) {
len = distance_func(x[i], x[minlen_index],
y[i], y[minlen_index]);
if (len > (maxlinedist+0.5)) {
printf("PROBLEM %d (%f,%f) %f %f\n",i,x[i],y[i],len,maxlinedist);
maxlinedist = len;
} line_pts[(int)(len)]++;
li[(int)(len)] = i; // index fuer den Punkt
marker[i] = 1; // nur einfache pkt
}
}
}
// startpunkt
startpkt = 0;
linedist = 0.0;
luecke = -1;
lasti = 0;
// and at last calc the sub lines
for (i = 0; i <= (int)(maxlinedist); i++) {
//printf("%d %d %d\n",i,line_pts[i],startpkt);
if (line_pts[i] > 0) {
luecke = 0;
lasti = i;
}
else luecke++;
// startpunkt sezten
if ( (line_pts[i] > 0) && (startpkt == 0)) {
x_line_pts[*nr_line_pts] = x[li[i]];
y_line_pts[(*nr_line_pts)++] = y[li[i]];
startpkt = 1;
}
// endpunkt
else if ( ((line_pts[i] == 0) && (startpkt == 1) && (luecke > 3)) || (i == (int)(maxlinedist))) {
x_line_pts[*nr_line_pts] = x[li[lasti]];
y_line_pts[(*nr_line_pts)++] = y[li[lasti]];
len = distance_func(x_line_pts[*nr_line_pts - 2], x_line_pts[*nr_line_pts - 1],
y_line_pts[*nr_line_pts - 2], y_line_pts[*nr_line_pts - 1]);
if (len > linedist) linedist = len;
startpkt = 0;
luecke = -1;
}
}
if (startpkt == 1) {
x_line_pts[*nr_line_pts] = x[li[lasti]];
y_line_pts[(*nr_line_pts)++] = y[li[lasti]];
}
free(line_pts);
free(li);
/*
printf("%d points are on the line, %d lines found\n",count,*nr_line_pts/2);
fflush(stdout);
*/
return linedist;
}
/**
*
*/
int SHT_remove_line(int nr_pts, double *x, double *y,
int resolution, double max_rho_d, int **histogram,
int *marker, double xmin, double ymin)
{
int i,j;//, k;
int nr_removed_points = 0;
for (i=0; i < nr_pts; i++) {
if ((marker[i] == 1) || (marker[i] == 3)) {
marker[i] += 1; // from 1 to 2 or from 3 to 4
if ((fabs(x[i]+xmin) < MAX_DISTANCE)&&(fabs(y[i]+ymin) < MAX_DISTANCE)){
nr_removed_points++;
for (j = 0; j < resolution; j++) {
histogram[j][k_index[i][j]]--;
}
}
}
}
return nr_removed_points;
}
/**
* -------------------------------------------------------------------------*
* NAME : SHT_get_hough_lines
* DESCRIPTION
* Gets the lines in a laserscan
* PARAMETERS
* ---------
* RESULT
* number of looped iterations
*-------------------------------------------------------------------------
*/
int SHT_get_hough_lines(int nr_pts, double *x, double *y,
int sht_resolution, double max_rho_d,
int **sht_histogram, int depth,
int *nr_line_pts,
double *x_line_pts, double *y_line_pts,
double xmin, double ymin, int unsorted)
{
#ifdef STANDALONE
//debug
char filename[256], auxfilename[256];
unsigned long prev_clock;
FILE *sht_fpr_histogramm;
strcpy(filename,"plane.dat");
//prev_clock = GetCurrentTimeInMilliSec();
unsigned long sht_average;
sht_average = SHT_Find_average_value_of_historgram(sht_resolution, sht_histogram);
#endif
// sht
unsigned long sht_maximum;
int sht_imax, sht_jmax;
int *sht_marker = new int[ScanAZ];
double sht_length_of_a_line;
int i = 0;
double m,b;
// init marker
for (i = 0; i < nr_pts; i++) sht_marker[i] = 0;
*nr_line_pts = 0;
// hough
SHT_hough_transform(nr_pts, x, y, sht_resolution, max_rho_d, sht_histogram,xmin,ymin);
sht_maximum = SHT_Find_max_value_of_historgram(sht_resolution, sht_histogram,
&sht_imax, &sht_jmax);
i = 0;
while ((sht_maximum > 0) && (i < depth)) {
sht_length_of_a_line = -0.1;
#ifdef STANDALONE
printf("\n====================================================%d %f\n",i,sht_length_of_a_line);
//printf("%d %ld\n",i,GetCurrentTimeInMilliSec()-prev_clock);
printf("Hough Transformation beendet. Aver: %ld Max %ld, at (%d,%d)\n",
sht_average, sht_maximum, sht_imax,sht_jmax);
fflush(stdout);
// debug Kontroll output of the planes
AddNumberToFileName(filename, auxfilename, i);
sht_fpr_histogramm = fopen(auxfilename,"w");
SHT_Print_histogram(sht_fpr_histogramm, sht_resolution, sht_histogram, sht_average);
fclose(sht_fpr_histogramm);
#endif
SHT_calc_line(sht_resolution, max_rho_d, sht_histogram, sht_imax, sht_jmax, &m, &b);
if (unsorted) {
SHT_find_unsorted_line(nr_pts, x, y, sht_resolution, max_rho_d, sht_histogram,
sht_imax, sht_jmax, sht_marker, sht_length_of_a_line,
nr_line_pts, x_line_pts, y_line_pts, xmin,ymin);
}
else {
SHT_find_line(nr_pts, x, y, sht_resolution, max_rho_d, sht_histogram,
sht_imax, sht_jmax, sht_marker, sht_length_of_a_line,
nr_line_pts, x_line_pts, y_line_pts, xmin,ymin);
}
SHT_remove_line(nr_pts, x, y, sht_resolution, max_rho_d,
sht_histogram, sht_marker,xmin,ymin);
sht_maximum = SHT_Find_max_value_of_historgram(sht_resolution, sht_histogram,
&sht_imax, &sht_jmax);
i++;
}
delete[] sht_marker;
return i;
}
/**
* Kontroll Ausgabe
*/
int print_hough_lines(int sht_nr_line_pts, int *sht_x_line_pts, int *sht_y_line_pts)
{
int i;
double dist;
static FILE *fpr = NULL;
if (fpr == NULL)
fpr = fopen("hough-lines.dat","w");
printf("# found %d lines\n",(int)(sht_nr_line_pts*0.5));
fprintf(fpr,"# found %d lines\n",(int)(sht_nr_line_pts*0.5));
for (i = 0; i < sht_nr_line_pts; i+=2) {
dist = distance_func((double)(sht_x_line_pts[i]),(double)(sht_x_line_pts[i+1]),
(double)(sht_y_line_pts[i]),(double)(sht_y_line_pts[i+1]));
// 1 pkt linien nicht anzeigen
// if (dist > 0.0) {
if (dist > -0.1) {
fprintf(fpr,"#%d %f\n",i/2,dist);
fprintf(fpr,"%d %d\n",sht_x_line_pts[i],sht_y_line_pts[i]);
fprintf(fpr,"%d %d\n\n",sht_x_line_pts[i+1],sht_y_line_pts[i+1]);
}
}
//fclose(fpr);
return 0;
}
/**************************************************************************/
/***********************END HOUGH TRANSFORMATION***************************/
/**************************************************************************/
#ifdef STANDALONE_OLD
int main(int argc, char ** argv)
{
//debug and init, help
FILE *fpr; // *sht_fpr_gerade
int i, NrPts;
char buffer[256], *ptr = buffer;
int dist[ScanAZ], angle[ScanAZ];
double amplitude[DATA_RANGE];
int x[DATA_RANGE];
int y[DATA_RANGE];
// double h1,h2,h3,h4;
int h1=1,h2=1,h3,h4;
double scale_val = 1.0;
// hough
int **sht_histogram; // histogramm der hough
int sht_resolution = 200; // aufloesung der hough
int sht_nr=ScanAZ; // ScanAz heisst alle; Anzahl der Durchlaeufe durch die Hough
// Speicherstruktur Anzahl der Linienpunkte (d.h) Nr_Linien = sht_nr_line_pts/2,
// x,y, 0=Anfang, 1=Ende, ....
int sht_nr_line_pts=0, sht_x_line_pts[2*ScanAZ], sht_y_line_pts[2*ScanAZ];
int xmin = 2000, xmax = -2000, ymin = 2000, ymax = -2000;
double max_rho_d = 0.0,local_dist=0.0;
if (argc == 2)
sht_nr = atoi(argv[1]);
else sht_nr = 100;
// READ input files
fpr = fopen("test20.dat","r");
// erste zeile ueberlesen
while ((*ptr++ = fgetc(fpr)) != 10) ;
NrPts = atoi(&(buffer[2]));
printf("NrPts: %d\n",NrPts);
for (i = 0; i < NrPts; i++) {
fscanf(fpr,"%d %d",&h3,&h4);
angle[i] = (int)(h1);
dist[i] = (int)h2;
amplitude[i] = h2;
if (i >= SKIP) {
x[i-SKIP] = (int)(h3 * scale_val);
y[i-SKIP] =(int)(h4 * scale_val);
if (x[i-SKIP] > xmax) xmax = x[i-SKIP];
if (x[i-SKIP] < xmin) xmin = x[i-SKIP];
if (y[i-SKIP] > ymax) ymax = y[i-SKIP];
if (y[i-SKIP] < ymin) ymin = y[i-SKIP];
local_dist = sqrt((x[i-SKIP] - x[i-SKIP-1])* (x[i-SKIP] - x[i-SKIP-1]) +
(y[i-SKIP] - y[i-SKIP-1])* (y[i-SKIP] - y[i-SKIP-1]));
}
}
fclose(fpr);
max_rho_d = sqrt(SQR(xmax-xmin) + SQR(ymax-ymin));
printf("Maxima:\n%d %d %d %d %f\n",xmin,xmax,ymin,ymax,max_rho_d);
if (xmax > MAX_DISTANCE) xmax = MAX_DISTANCE;
if (ymax > MAX_DISTANCE) ymax = MAX_DISTANCE;
if (xmin < -MAX_DISTANCE) xmin = -MAX_DISTANCE;
if (ymin < -MAX_DISTANCE) ymin = -MAX_DISTANCE;
max_rho_d = sqrt(SQR(xmax-xmin) + SQR(ymax-ymin));
printf("New Maxima:\n%d %d %d %d %f\n",xmin,xmax,ymin,ymax,max_rho_d);
for (i = 0; i < NrPts - 2*SKIP; i++) {
x[i] -= xmin;
y[i] -= ymin;
}
// START with the hough
// init block
/* Andreas 29.9.2000
if (max_rho_d > 500) sht_resolution = (int)(max_rho_d * 0.5);
else sht_resolution = (int)(max_rho_d * 0.75);
*/
sht_resolution = (int)(max_rho_d * 0.75);
printf("Hough Transformation berechnen mit Aufloesung %d\n",sht_resolution);fflush(stdout);
sht_init(sht_resolution);
sht_histogram = SHT_alloc_histogram(sht_resolution, sht_resolution);
// end init
SHT_get_hough_lines(NrPts-2*SKIP, x, y, sht_resolution, max_rho_d, sht_histogram, sht_nr,
&sht_nr_line_pts, sht_x_line_pts, sht_y_line_pts, xmin, ymin);
for (i = 0; i < sht_nr_line_pts; i++) {
sht_x_line_pts[i] = (sht_x_line_pts[i] + xmin) / scale_val;
sht_y_line_pts[i] = (sht_y_line_pts[i] + ymin) / scale_val;
}
// Kontroll Ausgabe
print_hough_lines(sht_nr_line_pts, sht_x_line_pts, sht_y_line_pts);
printf("end\n");fflush(stdout);
// hier ist was merkwuerdig bei res = 200 stuertz der Rechner ab
// exit block
SHT_free_histogram(sht_histogram, sht_resolution);
// end exit block
return 0;
}
#endif
#ifdef STANDALONE
int main(int argc, char ** argv)
{
int sht_resolution;
int **sht_histogram;
int sht_nr = 80; // Nr
int sht_nr_line_pts = 0,sht_nr_line = 0;
double sht_x_line_pts[10000], sht_y_line_pts[10000] ;
double max_rho_d;
int result;
int i;
double array_x[10000], array_y[10000]; // these two arrays build
double xmax, ymax, xmin, ymin;
float x, y;
int temp;
FILE *fpr;
xmax = ymax = xmin = ymin = 0;
// READ input files
fpr = fopen("points.dat","r");
i = 0;
while (!feof(fpr)) {
fscanf(fpr,"%f %f",&x, &y);
array_x[i] = (double)x;
array_y[i] = (double)y;
printf("%d %g %g \n",temp, array_x[i],array_y[i]);
if (array_x[i] > xmax) xmax = array_x[i];
if (array_x[i] < xmin) xmin = array_x[i];
if (array_y[i] > ymax) ymax = array_y[i];
if (array_y[i] < ymin) ymin = array_x[i];
i++;
}
fclose(fpr);
printf("***%f %f %f %f \n",xmax, ymax, xmin, ymin);
printf("*%d \n",i);
SHT_set_max_distance((int)MAX(xmax,ymax)+1,i);
for (int j = 0; j < i; j++) {
array_x[j] -= xmin;
array_y[j] -= ymin;
}
max_rho_d = sqrt(SQR(xmax-xmin) + SQR(ymax-ymin));
sht_resolution = (int)(max_rho_d);
sht_init(sht_resolution);
sht_histogram = SHT_alloc_histogram(sht_resolution, sht_resolution);
result = SHT_get_hough_lines(i,
array_x,
array_y,
sht_resolution,
max_rho_d,
sht_histogram,
sht_nr,
&sht_nr_line_pts,
sht_x_line_pts,
sht_y_line_pts,
xmin,
ymin);
printf("SHT_get_hough_lines: %d (%d)",result, sht_nr_line_pts);
fflush(stdout);
fpr = fopen("lines.dat","w");
for (int j = 0; j < sht_nr_line_pts; j+=2) {
sht_x_line_pts[j] += xmin;
sht_y_line_pts[j] += ymin;
sht_x_line_pts[j+1] += xmin;
sht_y_line_pts[j+1] += ymin;
if (SQR(sht_x_line_pts[j] - sht_x_line_pts[j+1]) +
SQR(sht_y_line_pts[j] - sht_y_line_pts[j+1]) > 225)
{
fprintf(fpr,"%g %g \n",sht_x_line_pts[j], sht_y_line_pts[j]);
fprintf(fpr,"%g %g \n\n",sht_x_line_pts[j+1], sht_y_line_pts[j+1]);
}
}
return 0;
}
#endif
Reference in a new issue View git blame Copy permalink