使用非线性优化拟合空间圆

Posted 2021-12-11 gaoyixue

1、先使用matlab生成100个空间圆上的点

使用下图推导的式子生成。

 1 x0=1;
 2 y0=1;
 3 z0=1;%(x0,y0,z0)是空间圆的圆心
 4 nx=1;
 5 ny=1;
 6 nz=1;
 7 n=[nx;ny;nz];%是空间圆的法线
 8 u=[ny;-nx;0];
 9 vx=nx*nz;
10 vy=ny*nz;
11 vz=-nx*nx-ny*ny;
12 v=[vx;vy;vz];
13 
14 u=u/sqrt(ny*ny+nx*nx);
15 v=v/sqrt(vx*vx+vy*vy+vz*vz);
16 r=1;%空间圆的半径
17 n=100;%生成的空间点个数
18 h=2*pi/(n-1);
19 xyz=zeros(n,3);%生成的空间点共100个
20 
21 for i=1:n
22     t=(i-1)*h;
23     xyz(i,1)=x0+r*(u(1)*cos(t)+v(1)*sin(t));
24     xyz(i,2)=y0+r*(u(2)*cos(t)+v(2)*sin(t));
25     xyz(i,3)=z0+r*(u(3)*cos(t)+v(3)*sin(t));
26 end

加入随机噪声：

noise=randn(size(xyz))*0.05;%随机噪声是均值为0方差为(0.05)*(0.05)的正态分布
xyz_observed=xyz+noise;

将生成的带有噪声的100个空间点写入文件，以便ceres-solver使用。

2、使用ceres-solver拟合空间圆

 1 #include<cstdio>
 2 #include<vector>
 3 #include<iostream>
 4 #include<fstream>
 5 #include<string>
 6 #include<ceres/ceres.h>
 7 #include<gflags/gflags.h>
 8 #include<glog/logging.h>
 9 using namespace std;
10 using namespace ceres;
11 
12 class DistanceFromCircleCost {
13 public:
14     DistanceFromCircleCost(double xx,double yy,double zz,double nx,double ny,double nz):xx_(xx),yy_(yy),zz_(zz),nx_(nx),ny_(ny),nz_(nz){}
15     template <typename T>
16     bool operator()(const T* const x,const T* const y,const T* const z, 
17         const T* const nx, const T* const ny, const T* const nz,
18         const T* const m,T* residual)const
19     {
20         T deltax = T(xx_) - x[0];
21         T deltay = T(yy_) - y[0];
22         T deltaz = T(zz_) - z[0];
23         T r = m[0] * m[0];
24         residual[0] = r*r - deltax*deltax - deltay*deltay - deltaz*deltaz;
25         residual[1] = deltax*nx[0] + deltay*ny[0] + deltaz*nz[0];
26         residual[2] = nx[0] * nx[0] + ny[0] * ny[0] + nz[0] * nz[0] - T(nx_*nx_ + ny_*ny_ + nz_*nz_);
27         return true;
28     }
29 private:
30     double xx_, yy_,zz_,nx_,ny_,nz_;
31 };
32 
33 int main(int argc,char** argv)
34 {
35     google::InitGoogleLogging(argv[0]);
36 
37     double x, y, z, nx, ny, nz, r;
38 
39     string filename="C:\\\\Users\\\\gaoyixue\\\\Desktop\\\\1.txt";
40     ifstream fin(filename.c_str());
41     string line;
42     getline(fin, line);
43     char *pend;
44     //读取第一行，格式是 圆心x 圆心y 圆心z 半径 法线x 法线y 法线z
45     x = strtod(line.c_str(), &pend);
46     y = strtod(pend, &pend);
47     z = strtod(pend, &pend);
48     r = strtod(pend, &pend);
49     nx = strtod(pend, &pend);
50     ny = strtod(pend, &pend);
51     nz = strtod(pend, nullptr);
52 
53     fprintf(stderr, "got x,y,z,r,nx,ny,nz %lg %lg %lg %lg %lg %lg %lg\\n", x, y, z, r, nx, ny, nz);
54 
55     double initial_x = x;
56     double initial_y = y;
57     double initial_z = z;
58     double initial_r = r;
59     double initial_nx = nx;
60     double initial_ny = ny;
61     double initial_nz = nz;
62 
63     double m = ceres::sqrt(r);
64     Problem problem;
65     double xx, yy, zz;
       //读取100个空间点坐标 格式是 x坐标 y坐标 z坐标
66     while (getline(fin,line))
67     {
68         xx = strtod(line.c_str(), &pend);
69         yy = strtod(pend, &pend);
70         zz = strtod(pend, nullptr);
71         //std::cout << "got (" << xx << "," << yy << ")\\n";
72         CostFunction *costfunc = new AutoDiffCostFunction<DistanceFromCircleCost, 3, 1, 1, 1, 1, 1, 1, 1>(new DistanceFromCircleCost(xx, yy, zz,nx,ny,nz));
73         problem.AddResidualBlock(costfunc, new CauchyLoss(0.5), &x, &y, &z, &nx, &ny, &nz, &m);
74     }
75 
76     //std::cout << "got " << numpoints << " points.\\n";
79     Solver::Options options;
80     options.linear_solver_type = DENSE_QR;
81     options.max_num_iterations = 500;
82     Solver::Summary summary;
83 
84     Solve(options, &problem, &summary);
85 
86     r = m*m;
87     std::cout << summary.BriefReport() << "\\n";
88     std::cout << "x:" << initial_x << "->" << x << "\\n";
89     std::cout << "y:" << initial_y << "->" << y << "\\n";
90     std::cout << "z:" << initial_z << "->" << z << "\\n";
91     std::cout << "r:" << initial_r << "->" << r << "\\n";
92     std::cout << "nx:" << initial_nx << "->" << nx << "\\n";
93     std::cout << "ny:" << initial_ny << "->" << ny << "\\n";
94     std::cout << "nz:" << initial_nz << "->" << nz << "\\n";
95     return 0;
96 }

运行结果如下：