//圓柱擬合程序：讀取points.txt文件中的點(diǎn)云數(shù)據(jù)，擬合得到圓柱半徑值.
//
//此程序依據(jù)“學(xué)編程的小蜜蜂”的matlab源程序：?http://download.csdn.net/detail/erlongz/5815557
//
//2014.12.24??By?HorizonDong.


#include?“cv.h“
#include?
#include????
#define??MAX_POINT_NUM?200000??//最大點(diǎn)數(shù)。若點(diǎn)數(shù)較之更大，則可能會(huì)導(dǎo)致堆棧溢出，此時(shí)需進(jìn)行相應(yīng)的堆棧設(shè)置。
using?namespace?std;

//函數(shù)聲明：
void?Reconstruction（）;
int?main（?）
{???
?????Reconstruction（）;????????????????????//三維點(diǎn)的重建、存儲(chǔ)??????
?????cout<<“done!“<?????return?0;
}
////////////////////////////////////////////////////////////////////////////
//圓柱擬合程序
//輸入：??1、點(diǎn)云文件points.txt.
//???????????其存儲(chǔ)格式為?x1?y1?z1
//????????????????????????x2?y2?z2
//????????????????????????x3?y3?z3
//????????????????????????.
//????????????????????????.
//????????????????????????.

//輸出：??1、循環(huán)迭代次數(shù)loop_times;
//????????2、擬合得到的半徑R.

void?Reconstruction（）???
{
?????int?i=0j=0;??????????????//循環(huán)用變量
	?int?int_temp=0;???????????//用于存儲(chǔ)待擬合的點(diǎn)數(shù)
	?int?loop_times=0pp=0;????//loop_time等價(jià)于t，表示迭代循環(huán)次數(shù)
?????double?a=1b=1c=1;
	?double?x0=0y0=0z0=0;
	?double?D=0s=0S=0dx=0dy=0dz=0;
	?double?R=0;???????????????????????????????//半徑
	?double?d_temp1=0d_temp2=0d_temp3=0;
	?double?B[MAX_POINT_NUM][7]={0};
	?double?L[MAX_POINT_NUM]???={0};
?????double?worldVetex[MAX_POINT_NUM][3]={0};??//用于存儲(chǔ)點(diǎn)坐標(biāo)

?????double?mean_x=0mean_y=0mean_z=0;
	?bool?while_flag=1;
?????CvMat*?C????=?cvCreateMat（?2?7?CV_64FC1?）;
?????CvMat*?W????=?cvCreateMat（?2?1?CV_64FC1?）;
?????CvMat*?N????=?cvCreateMat（?9?9?CV_64FC1?）;
	?CvMat*?N_inv=?cvCreateMat（?9?9?CV_64FC1?）;
	?CvMat*?UU???=?cvCreateMat（?9?1?CV_64FC1?）;
?????CvMat*?para?=?cvCreateMat（?9?1?CV_64FC1?）;?????//參數(shù)矩陣
?????//變量初始化
?????cvZero（C）;cvZero（W）;cvZero（N）;cvZero（N_inv）;cvZero（UU）;
?????cvSetIdentity（para）;
??????
////從points.txt文件讀入三維坐標(biāo)點(diǎn)
???ifstream?Points_in（“points.txt“）;
???int_temp=0;
???if?（Points_in.is_open（））?
???{
?????while?（!Points_in.eof（））???//尚未到達(dá)文件結(jié)尾
	?{?
????????Points_in>>worldVetex[int_temp][0]>>worldVetex[int_temp][1]>>worldVetex[int_temp][2];
		int_temp++;?
	?}??
???}
???else
???{
????????cout<<“open?fail!“<???}
???int_temp=int_temp-1;?????????//由于points.txt文件末行為空行，須扣除一個(gè)點(diǎn)

???//求解重心、初始化
	?for（i=0;i	?{?
????????d_temp1+=worldVetex[i][0];
????????d_temp2+=worldVetex[i][1];
????????d_temp3+=worldVetex[i][2];
	?}
?????mean_x=d_temp1/int_temp;?mean_y=d_temp2/int_temp;mean_z=d_temp3/int_temp;
?????x0=mean_x;y0=mean_y;z0=mean_z;
?????R?=20;
//迭代循環(huán)，最優(yōu)化矩陣para
??while（while_flag==true）???????//即（max（abs（para（1:7）））>0.00001）???????????????
??{
	//1.?a、b、c的符號(hào)修正
	??if（a<0）?
	??{
?????????a=-a;b=-b;c=-c;
	??}
??????if（a==0）
	??{?
?????????if（b<0）
		?{
???????????b=-b;c=-c;
		?}
?????????if（b==0）
		?{
		???if（c<0）
?????????????c=-c;
		?}
	??}?
???????s=sqrt（pow（a2）+pow（b2）+pow（c2））+0.0000001;???????//防止a?b?c?同時(shí)為0
???????a=a/s+0.0000001;b=b/s+0.0000001;c=c/s+0.0000001;
	//2.?計(jì)算矩陣B和L
??????for（i=0;i

?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----

?????文件???????7149??2014-12-24?21:08??Cylinderfitting\3D_reconstruction.cpp

?????文件???????3710??2013-10-26?16:42??Cylinderfitting\3D_reconstruction.dsp

?????文件????????559??2013-10-26?16:42??Cylinderfitting\3D_RECONSTRUCTION.DSW

?????文件?????115712??2014-12-24?21:09??Cylinderfitting\3D_reconstruction.ncb

?????文件??????54784??2014-12-24?21:09??Cylinderfitting\3D_RECONSTRUCTION.OPT

?????文件????????268??2014-12-24?21:09??Cylinderfitting\3D_reconstruction.plg

?????文件????8343552??2013-09-25?14:40??Cylinderfitting\3D_reconstruction.sdf

?????文件????????900??2013-09-25?14:28??Cylinderfitting\3D_reconstruction.sln

????..A..H.?????10752??2013-09-25?14:40??Cylinderfitting\3D_reconstruction.suo

?????文件???????6338??2013-09-25?14:39??Cylinderfitting\3D_reconstruction.vcxproj

?????文件????????143??2013-09-25?14:26??Cylinderfitting\3D_reconstruction.vcxproj.user

?????文件???????4553??2014-12-24?17:21??Cylinderfitting\CylinderFitting.dsp

?????文件????????555??2014-12-24?17:07??Cylinderfitting\CylinderFitting.dsw

?????文件??????33792??2014-12-24?17:21??Cylinderfitting\CylinderFitting.ncb

?????文件??????49664??2014-12-24?17:21??Cylinderfitting\CylinderFitting.opt

?????文件???????1837??2014-12-24?17:16??Cylinderfitting\CylinderFitting.plg

?????文件?????585802??2014-12-24?21:09??Cylinderfitting\Debug\3D_reconstruction.exe

?????文件?????846428??2014-12-24?21:09??Cylinderfitting\Debug\3D_reconstruction.ilk

?????文件?????357824??2014-12-24?21:09??Cylinderfitting\Debug\3D_reconstruction.obj

?????文件????2622312??2014-12-24?19:41??Cylinderfitting\Debug\3D_reconstruction.pch

?????文件????1147904??2014-12-24?21:09??Cylinderfitting\Debug\3D_reconstruction.pdb

?????文件??????99328??2014-12-24?21:09??Cylinderfitting\Debug\vc60.idb

?????文件?????143360??2014-12-24?21:09??Cylinderfitting\Debug\vc60.pdb

?????文件????1373720??2014-12-24?16:50??Cylinderfitting\points.txt

?????目錄??????????0??2014-12-24?21:09??Cylinderfitting\Debug

?????目錄??????????0??2014-12-24?21:09??Cylinderfitting

-----------?---------??----------?-----??----

?????????????15810946????????????????????26