You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
992 lines
28 KiB
C++
992 lines
28 KiB
C++
/*
|
|
* 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
|
|
|
|
|