三维点云的圆柱面拟合

1. 本文档包含：三维点云文件points.txt和C++项目文件CylinderFitting。 2. 本文档通过对三维点云的圆柱拟合，得到圆柱的半径、圆柱轴线单位方向向量和轴线起始位置三个主要参数。 3. 本文档算法源自“学编程的小蜜蜂”的matlab程序：http://download.csdn.net/detail/erlongz/5815557

//圆柱拟合程序：读取points.txt文件中的点云数据，拟合得到圆柱半径值.
//
//此程序依据“学编程的小蜜蜂”的matlab源程序： http://download.csdn.net/detail/erlongz/5815557
//
//2014.12.24  By HorizonDong.


#include “cv.h“
#include 
#include    
#define  MAX_POINT_NUM 200000  //最大点数。若点数较之更大，则可能会导致堆栈溢出，此时需进行相应的堆栈设置。
using namespace std;

//函数声明：
void Reconstruction（）;
int main（ ）
{   
     Reconstruction（）;                    //三维点的重建、存储      
     cout<<“done!“<     return 0;
}
////////////////////////////////////////////////////////////////////////////
//圆柱拟合程序
//输入：  1、点云文件points.txt.
//           其存储格式为 x1 y1 z1
//                        x2 y2 z2
//                        x3 y3 z3
//                        .
//                        .
//                        .

//输出：  1、循环迭代次数loop_times;
//        2、拟合得到的半径R.

void Reconstruction（）   
{
     int i=0j=0;              //循环用变量
	 int int_temp=0;           //用于存储待拟合的点数
	 int loop_times=0pp=0;    //loop_time等价于t，表示迭代循环次数
     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};  //用于存储点坐标

     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 ）;     //参数矩阵
     //变量初始化
     cvZero（C）;cvZero（W）;cvZero（N）;cvZero（N_inv）;cvZero（UU）;
     cvSetIdentity（para）;
      
////从points.txt文件读入三维坐标点
   ifstream Points_in（“points.txt“）;
   int_temp=0;
   if （Points_in.is_open（）） 
   {
     while （!Points_in.eof（））   //尚未到达文件结尾
	 { 
        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文件末行为空行，须扣除一个点

   //求解重心、初始化
	 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;
//迭代循环，最优化矩阵para
  while（while_flag==true）       //即（max（abs（para（1:7）））>0.00001）               
  {
	//1. a、b、c的符号修正
	  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 同时为0
       a=a/s+0.0000001;b=b/s+0.0000001;c=c/s+0.0000001;
	//2. 计算矩阵B和L
      for（i=0;i

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       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