一剑行天下's Blog
Happy coding
通过求点到二叉平面的距离来处理点云数据
一剑行天下
posted @ 2008年12月18日 19:04
in 点云数据处理
, 2509 阅读
整个文件的构成是：
首先是主文件main.c
内容如下：
#include<stdio.h>
#include <malloc.h>
#include <math.h>
#include "plane.h"
/* for(i=0;i<n;i++)
{
fputc(c[i],fp);
putchar(c[i]);
}*/
int main(int argc, char* argv[])
{
FILE *fp=NULL;
char *name1="venus.asc";
int i,j,m;
if((fp=fopen(name1,"r"))==NULL)
{
printf("open file name1 error");
return 0;
}
double arr[3];
int p_num=0;
fscanf(fp, "%d", &p_num);
fclose(fp);
//double (*point_array)[3]=(double((*)[3]))malloc(3*p_num*sizeof(double));
double **point_array = (double **)malloc(p_num*sizeof(double*));
for (int i=0; i< p_num; i++){
point_array[i] = (double*)malloc(3*sizeof(double));
}
char *file1=NULL;
double d=0;
if (argc==1)
{
if ((fp=fopen("venus.asc","r"))== NULL)
{
printf("Error in open file mbr.asc\n");
return 1;
}
file1="venus.asc";
}
else
{
file1=argv[1];
if ((fp=fopen(argv[1],"r"))== NULL)
{
printf("Error in open file %s\n", argv[1]);
return 1;
}
}
/* if((fp=fopen(file1,"r"))==NULL)
{
cout<<"open file "<<argv[1]<<" error"<<endl;
return 1;
}*/
fscanf (fp, "%d", &p_num);
fclose(fp);
read_point_file(point_array, file1);
Cch_plane plane;
double **p1=(double**)point_array;
plane= cch_least_plane(p1,p_num);
int k;
m=p_num;
for(i=0;i<m;i++)
{
d=cch_point_to_plane_distance(*(point_array+i), plane.center, plane.vector);
k=m;
if(fabs(d)>210)
{
for(j=i;j<=k-1;j++)
point_array[j]=point_array[j+1];
m=m--;
}
}
fp=fopen("venus1.asc","w");
int n=0;
double int_arr[3];
fprintf(fp,"%d\n",m);
for(i=1;i<=m;i++)
{
int_arr[0]=point_array[i-1][0];
int_arr[1]=point_array[i-1][1];
int_arr[2]=point_array[i-1][2];
int ret=fprintf(fp,"%lf %lf %lf\n", int_arr[0],int_arr[1],int_arr[2]);
//if(ret!=3)
// break;
n++;
}
fclose(fp);
}
#include <malloc.h>
#include <math.h>
#include "plane.h"
/* for(i=0;i<n;i++)
{
fputc(c[i],fp);
putchar(c[i]);
}*/
int main(int argc, char* argv[])
{
FILE *fp=NULL;
char *name1="venus.asc";
int i,j,m;
if((fp=fopen(name1,"r"))==NULL)
{
printf("open file name1 error");
return 0;
}
double arr[3];
int p_num=0;
fscanf(fp, "%d", &p_num);
fclose(fp);
//double (*point_array)[3]=(double((*)[3]))malloc(3*p_num*sizeof(double));
double **point_array = (double **)malloc(p_num*sizeof(double*));
for (int i=0; i< p_num; i++){
point_array[i] = (double*)malloc(3*sizeof(double));
}
char *file1=NULL;
double d=0;
if (argc==1)
{
if ((fp=fopen("venus.asc","r"))== NULL)
{
printf("Error in open file mbr.asc\n");
return 1;
}
file1="venus.asc";
}
else
{
file1=argv[1];
if ((fp=fopen(argv[1],"r"))== NULL)
{
printf("Error in open file %s\n", argv[1]);
return 1;
}
}
/* if((fp=fopen(file1,"r"))==NULL)
{
cout<<"open file "<<argv[1]<<" error"<<endl;
return 1;
}*/
fscanf (fp, "%d", &p_num);
fclose(fp);
read_point_file(point_array, file1);
Cch_plane plane;
double **p1=(double**)point_array;
plane= cch_least_plane(p1,p_num);
int k;
m=p_num;
for(i=0;i<m;i++)
{
d=cch_point_to_plane_distance(*(point_array+i), plane.center, plane.vector);
k=m;
if(fabs(d)>210)
{
for(j=i;j<=k-1;j++)
point_array[j]=point_array[j+1];
m=m--;
}
}
fp=fopen("venus1.asc","w");
int n=0;
double int_arr[3];
fprintf(fp,"%d\n",m);
for(i=1;i<=m;i++)
{
int_arr[0]=point_array[i-1][0];
int_arr[1]=point_array[i-1][1];
int_arr[2]=point_array[i-1][2];
int ret=fprintf(fp,"%lf %lf %lf\n", int_arr[0],int_arr[1],int_arr[2]);
//if(ret!=3)
// break;
n++;
}
fclose(fp);
}
2。读点云数据的文件pointsread.c
内容如下：
#include<stdio.h>
#include"plane.h"
void read_point_file(double **p, const char *name1)
{
FILE *fp=NULL;
if((fp=fopen(name1,"r"))==NULL)
{
printf("open file name1 error");
return;
}
double arr[3];
int nn=0;
fscanf(fp, "%d", &nn);
int n=0;
while(!feof(fp))
{
int ret=fscanf(fp,"%lf %lf %lf",&arr[0],&arr[1],&arr[2]);
if(ret!=3)
break;
p[n][0]=arr[0];
p[n][1]=arr[1];
p[n][2]=arr[2];
n++;
}
fclose(fp);
}
#include"plane.h"
void read_point_file(double **p, const char *name1)
{
FILE *fp=NULL;
if((fp=fopen(name1,"r"))==NULL)
{
printf("open file name1 error");
return;
}
double arr[3];
int nn=0;
fscanf(fp, "%d", &nn);
int n=0;
while(!feof(fp))
{
int ret=fscanf(fp,"%lf %lf %lf",&arr[0],&arr[1],&arr[2]);
if(ret!=3)
break;
p[n][0]=arr[0];
p[n][1]=arr[1];
p[n][2]=arr[2];
n++;
}
fclose(fp);
}
3。生成二叉平面的文件least_plane.c
内容如下：
#include<stdio.h>
#include <stdio.h>
#include <malloc.h>
#include <math.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include"plane.h"
/*计算点到平面的距离,有正负；
double cch_point_to_plane_distance(Cch_point *point,Cch_point * plane_point, Cch_point * plane_vector)
{
cch_unit_vector_m2(plane_vector);
double vector[3];
vector[0] = point->x - plane_point->x;
vector[1] = point->y - plane_point->y;
vector[2] = point->z - plane_point->z;
double distance;
distance = vector[0]*plane_vector->x + vector[1]*plane_vector->y + vector[2]*plane_vector->z;
return distance;
}*/
gsl_matrix *
matrix_mul(gsl_matrix * a, gsl_matrix * b) //计算矩阵的乘积a*b
{
int i,j,k;
double aa,bb,m = 0;
gsl_matrix * c = gsl_matrix_alloc(a->size1,b->size2);
if (a->size2 != b->size1)
{
printf ("matrix multiply matrix Error!\n");
return NULL;
}
for(i=0; i < a->size1; i++)
{
for(j=0; j < b->size2; j++)
{
for(k=0; k < a->size2; k++)
{
aa = gsl_matrix_get(a, i, k);
bb = gsl_matrix_get(b, k, j);
m+=aa*bb;
}
gsl_matrix_set(c,i,j,m);
m = 0;
}
}
return c;
}
gsl_matrix *
matrix_T(gsl_matrix * a) //计算矩阵的转置
{
int i,j;
gsl_matrix * aT = gsl_matrix_alloc(a->size2, a->size1);
for(i = 0; i < aT->size1; i++)
for(j = 0; j < aT->size2; j++)
gsl_matrix_set(aT,i,j,gsl_matrix_get(a,j,i));
return aT;
}
gsl_matrix *
m2_matrix_n(gsl_matrix * U, gsl_vector * S, gsl_matrix * V) //利用SVD分解求矩阵A的逆
{
int i,j;
gsl_matrix * s = gsl_matrix_alloc(U->size2,U->size2);
gsl_matrix * u = gsl_matrix_alloc(U->size2,U->size1);
gsl_matrix * f = gsl_matrix_alloc(V->size1,s->size2);
gsl_matrix * An = gsl_matrix_alloc(f->size1,u->size2);
gsl_matrix_set_zero(s);
for(i = 0; i < s->size1; i++)
gsl_matrix_set(s,i,i,1/(gsl_vector_get(S,i)));
u = matrix_T(U);
//f = matrix_mul(V,s); //利用自己编写的函数计算矩阵的乘积
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, V, s, 0.0, f);//利用GSL函数直接求矩阵的乘积
//An = matrix_mul(f,u);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, f, u, 0.0, An);
return An;
}
/********************************
* 函数名称：cch_least_plane
* 函数功能：least_plane
* 输入参数：待拟合的点
* 输出参数：无
* 返 回 值：拟合直线
* 日 期：2008-8-18
* *******************************/
Cch_plane cch_least_plane(double **point,int num)
{
gsl_matrix * A = gsl_matrix_alloc(num,4);
int i;
for(i=0;i<num;i++)
{
gsl_matrix_set (A, i,0, point[i][0]);
gsl_matrix_set (A, i,1, point[i][1]);
gsl_matrix_set (A, i,2, point[i][2]);
gsl_matrix_set (A, i,3, 1);
}
gsl_matrix * AT;
AT=matrix_T(A);
gsl_matrix *ATA;
ATA=matrix_mul(AT,A);
gsl_matrix * V = gsl_matrix_alloc(A->size2,A->size2);
gsl_vector * S = gsl_vector_alloc(A->size2);
gsl_linalg_SV_decomp_jacobi(ATA,V,S);
int S_min_id=0;
double S_min=gsl_vector_get(S,0);
for(i=1;i<4;i++)
{
if(gsl_vector_get(S,i)<S_min)
{
S_min=gsl_vector_get(S,S_min_id);
S_min_id=i;
}
}
Cch_plane plane;
plane.vector.x=gsl_matrix_get(V,0,S_min_id);
plane.vector.y=gsl_matrix_get(V,1,S_min_id);
plane.vector.z=gsl_matrix_get(V,2,S_min_id);
double plane_d;
plane_d=gsl_matrix_get(V,3,S_min_id);
double plane_vector_max;
if(fabs(plane.vector.x) > fabs(plane.vector.y))
plane_vector_max=plane.vector.x;
else
plane_vector_max=plane.vector.y;
if(fabs(plane.vector.z) > fabs(plane_vector_max))
plane_vector_max=plane.vector.z;
if(plane_vector_max == plane.vector.z)
{
plane.center.x=0;
plane.center.y=0;
plane.center.z=-plane_d/plane.vector.z;
}
else if(plane_vector_max == plane.vector.x)
{
plane.center.x=-plane_d/plane.vector.x;
plane.center.y=0;
plane.center.z=0;
}
else if(plane_vector_max == plane.vector.y)
{
plane.center.x=0;
plane.center.y=-plane_d/plane.vector.y;
plane.center.z=0;
}
else
printf("plane vector has error!\n");
double plane_vector_m;
plane_vector_m = plane.vector.x*plane.vector.x
+ plane.vector.y*plane.vector.y
+ plane.vector.z*plane.vector.z;
plane_vector_m=sqrt(plane_vector_m);
plane.vector.x=plane.vector.x/plane_vector_m;
plane.vector.y=plane.vector.y/plane_vector_m;
plane.vector.z=plane.vector.z/plane_vector_m;
gsl_matrix_free(A);
gsl_matrix_free(AT);
gsl_matrix_free(ATA);
gsl_matrix_free(V);
gsl_vector_free(S);
return plane;
}
#include <stdio.h>
#include <malloc.h>
#include <math.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include"plane.h"
/*计算点到平面的距离,有正负；
double cch_point_to_plane_distance(Cch_point *point,Cch_point * plane_point, Cch_point * plane_vector)
{
cch_unit_vector_m2(plane_vector);
double vector[3];
vector[0] = point->x - plane_point->x;
vector[1] = point->y - plane_point->y;
vector[2] = point->z - plane_point->z;
double distance;
distance = vector[0]*plane_vector->x + vector[1]*plane_vector->y + vector[2]*plane_vector->z;
return distance;
}*/
gsl_matrix *
matrix_mul(gsl_matrix * a, gsl_matrix * b) //计算矩阵的乘积a*b
{
int i,j,k;
double aa,bb,m = 0;
gsl_matrix * c = gsl_matrix_alloc(a->size1,b->size2);
if (a->size2 != b->size1)
{
printf ("matrix multiply matrix Error!\n");
return NULL;
}
for(i=0; i < a->size1; i++)
{
for(j=0; j < b->size2; j++)
{
for(k=0; k < a->size2; k++)
{
aa = gsl_matrix_get(a, i, k);
bb = gsl_matrix_get(b, k, j);
m+=aa*bb;
}
gsl_matrix_set(c,i,j,m);
m = 0;
}
}
return c;
}
gsl_matrix *
matrix_T(gsl_matrix * a) //计算矩阵的转置
{
int i,j;
gsl_matrix * aT = gsl_matrix_alloc(a->size2, a->size1);
for(i = 0; i < aT->size1; i++)
for(j = 0; j < aT->size2; j++)
gsl_matrix_set(aT,i,j,gsl_matrix_get(a,j,i));
return aT;
}
gsl_matrix *
m2_matrix_n(gsl_matrix * U, gsl_vector * S, gsl_matrix * V) //利用SVD分解求矩阵A的逆
{
int i,j;
gsl_matrix * s = gsl_matrix_alloc(U->size2,U->size2);
gsl_matrix * u = gsl_matrix_alloc(U->size2,U->size1);
gsl_matrix * f = gsl_matrix_alloc(V->size1,s->size2);
gsl_matrix * An = gsl_matrix_alloc(f->size1,u->size2);
gsl_matrix_set_zero(s);
for(i = 0; i < s->size1; i++)
gsl_matrix_set(s,i,i,1/(gsl_vector_get(S,i)));
u = matrix_T(U);
//f = matrix_mul(V,s); //利用自己编写的函数计算矩阵的乘积
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, V, s, 0.0, f);//利用GSL函数直接求矩阵的乘积
//An = matrix_mul(f,u);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, f, u, 0.0, An);
return An;
}
/********************************
* 函数名称：cch_least_plane
* 函数功能：least_plane
* 输入参数：待拟合的点
* 输出参数：无
* 返 回 值：拟合直线
* 日 期：2008-8-18
* *******************************/
Cch_plane cch_least_plane(double **point,int num)
{
gsl_matrix * A = gsl_matrix_alloc(num,4);
int i;
for(i=0;i<num;i++)
{
gsl_matrix_set (A, i,0, point[i][0]);
gsl_matrix_set (A, i,1, point[i][1]);
gsl_matrix_set (A, i,2, point[i][2]);
gsl_matrix_set (A, i,3, 1);
}
gsl_matrix * AT;
AT=matrix_T(A);
gsl_matrix *ATA;
ATA=matrix_mul(AT,A);
gsl_matrix * V = gsl_matrix_alloc(A->size2,A->size2);
gsl_vector * S = gsl_vector_alloc(A->size2);
gsl_linalg_SV_decomp_jacobi(ATA,V,S);
int S_min_id=0;
double S_min=gsl_vector_get(S,0);
for(i=1;i<4;i++)
{
if(gsl_vector_get(S,i)<S_min)
{
S_min=gsl_vector_get(S,S_min_id);
S_min_id=i;
}
}
Cch_plane plane;
plane.vector.x=gsl_matrix_get(V,0,S_min_id);
plane.vector.y=gsl_matrix_get(V,1,S_min_id);
plane.vector.z=gsl_matrix_get(V,2,S_min_id);
double plane_d;
plane_d=gsl_matrix_get(V,3,S_min_id);
double plane_vector_max;
if(fabs(plane.vector.x) > fabs(plane.vector.y))
plane_vector_max=plane.vector.x;
else
plane_vector_max=plane.vector.y;
if(fabs(plane.vector.z) > fabs(plane_vector_max))
plane_vector_max=plane.vector.z;
if(plane_vector_max == plane.vector.z)
{
plane.center.x=0;
plane.center.y=0;
plane.center.z=-plane_d/plane.vector.z;
}
else if(plane_vector_max == plane.vector.x)
{
plane.center.x=-plane_d/plane.vector.x;
plane.center.y=0;
plane.center.z=0;
}
else if(plane_vector_max == plane.vector.y)
{
plane.center.x=0;
plane.center.y=-plane_d/plane.vector.y;
plane.center.z=0;
}
else
printf("plane vector has error!\n");
double plane_vector_m;
plane_vector_m = plane.vector.x*plane.vector.x
+ plane.vector.y*plane.vector.y
+ plane.vector.z*plane.vector.z;
plane_vector_m=sqrt(plane_vector_m);
plane.vector.x=plane.vector.x/plane_vector_m;
plane.vector.y=plane.vector.y/plane_vector_m;
plane.vector.z=plane.vector.z/plane_vector_m;
gsl_matrix_free(A);
gsl_matrix_free(AT);
gsl_matrix_free(ATA);
gsl_matrix_free(V);
gsl_vector_free(S);
return plane;
}
4.求点到平面距离的文件distance.c
内容如下：
#include"plane.h"
//计算点到平面的距离,有正负；
double cch_point_to_plane_distance(double *point,Cch_point plane_point, Cch_point plane_vector)
{
double vector[3];
vector[0] = point[0] - plane_point.x;
vector[1] = point[1] - plane_point.y;
vector[2] = point[2]- plane_point.z;
double distance;
distance = vector[0]*plane_vector.x + vector[1]*plane_vector.y + vector[2]*plane_vector.z;
return distance;
}
//计算点到平面的距离,有正负；
double cch_point_to_plane_distance(double *point,Cch_point plane_point, Cch_point plane_vector)
{
double vector[3];
vector[0] = point[0] - plane_point.x;
vector[1] = point[1] - plane_point.y;
vector[2] = point[2]- plane_point.z;
double distance;
distance = vector[0]*plane_vector.x + vector[1]*plane_vector.y + vector[2]*plane_vector.z;
return distance;
}
5。头文件plane.h
内容如下：
#include <malloc.h>
#include <math.h>
#include<stdio.h>
typedef struct Cch_point{
double x,y,z;
}Cch_point;
typedef struct Cch_plane{
Cch_point center;
Cch_point vector;
}Cch_plane;
Cch_plane cch_least_plane(double **point,int num);
double cch_point_to_plane_distance(double *point,Cch_point plane_point, Cch_point plane_vector);
void read_point_file(double **p, const char *name1);
#include <math.h>
#include<stdio.h>
typedef struct Cch_point{
double x,y,z;
}Cch_point;
typedef struct Cch_plane{
Cch_point center;
Cch_point vector;
}Cch_plane;
Cch_plane cch_least_plane(double **point,int num);
double cch_point_to_plane_distance(double *point,Cch_point plane_point, Cch_point plane_vector);
void read_point_file(double **p, const char *name1);
6。makefile文件
内容如下：
VPATH =./
CFLAGS = -g -std=c99 `pkg-config --cflags glib-2.0`
LIBS = `pkg-config --libs glib-2.0`
MLIBS = -lgsl -lm -lgslcblas
OBJ=main.o distance.o least_plane.o pointsread.o
main : $(OBJ)
$(CC) $(LIBS) $(MLIBS) $(OBJ) -o main
%.o : %.c
$(CC) -c $(CFLAGS) -I$(VPATH) $< -o $@
clean:
rm $(OBJ) main
CFLAGS = -g -std=c99 `pkg-config --cflags glib-2.0`
LIBS = `pkg-config --libs glib-2.0`
MLIBS = -lgsl -lm -lgslcblas
OBJ=main.o distance.o least_plane.o pointsread.o
main : $(OBJ)
$(CC) $(LIBS) $(MLIBS) $(OBJ) -o main
%.o : %.c
$(CC) -c $(CFLAGS) -I$(VPATH) $< -o $@
clean:
rm $(OBJ) main
7。点云数据文件venus.asc
部分内容如下;
19928
4.554705 199.1733 811.394049
3.584999 199.5536 1011.1685
3.740701 918.7712 1211.61608
3.796498 210.5667 711.51842
3.798301 197.8998 9.092709
3.892305 198.6992 6.968771
3.918405 202.3646 0.5173612
3.932501 197.3231 11.20268
3.962199 201.4767 10.76896
3.964703 202.2824 8.68437
3.972898 199.0001 15.57984
4.000503 201.1363 4.34869
4.003095 197.7905 14.73202
4.554705 199.1733 811.394049
3.584999 199.5536 1011.1685
3.740701 918.7712 1211.61608
3.796498 210.5667 711.51842
3.798301 197.8998 9.092709
3.892305 198.6992 6.968771
3.918405 202.3646 0.5173612
3.932501 197.3231 11.20268
3.962199 201.4767 10.76896
3.964703 202.2824 8.68437
3.972898 199.0001 15.57984
4.000503 201.1363 4.34869
4.003095 197.7905 14.73202
8。make以前所包含的文件有
9。make以后所包含的文件有
10。程序运行以后生成的点云文件venus1.asc
部分内容如下；
19926
3.584999 199.553600 1011.168500
3.796498 210.566700 711.518420
3.798301 197.899800 9.092709
3.892305 198.699200 6.968771
3.918405 202.364600 0.517361
3.932501 197.323100 11.202680
3.962199 201.476700 10.768960
3.964703 202.282400 8.684370
3.972898 199.000100 15.579840
4.000503 201.136300 4.348690
4.003095 197.790500 14.732020
3.584999 199.553600 1011.168500
3.796498 210.566700 711.518420
3.798301 197.899800 9.092709
3.892305 198.699200 6.968771
3.918405 202.364600 0.517361
3.932501 197.323100 11.202680
3.962199 201.476700 10.768960
3.964703 202.282400 8.684370
3.972898 199.000100 15.579840
4.000503 201.136300 4.348690
4.003095 197.790500 14.732020
评论 (0)