最近有人问怎么对球状的三维点云进行拟合,得到球心的位置和半径,对于这类问题,我首先想到的是二维图像中圆检测的方法霍夫变换,但是霍夫变换是在图像中进行的,而这是三维点云,如果是体素的话还有可能。

另外比较常用的拟合方法是ransac,不过提问者说ransac速度较慢不予考虑,所以最合适的还是最小二乘法了,然而在求导的时候会发现这个问题用普通最小二乘法是不能解的,因为求导之后是一个非线性的超定方程,这样就会变得非常复杂了,所以最好的方法还是结合能量最小化的方法,用高斯牛顿法试了一下,效果很好。

首先生成一堆点云,近似圆的形状,这个过程很简单,先随机生成很多点,然后单位化就成圆了,然后再随机设定长度。

高斯牛顿法需要给定一个初始的圆心c和半径r,这个对结果影响不大,随机给定就可以了,比如c0=(0.4768, 0.0457, 0.0253), r0=0.0884

然后进行迭代求解更新c和r,这个很快的,我用matlab迭代10次只需要7ms的时间,就得到了最终收敛的c和r。

具体的迭代算法推导过程是这样的,主要是雅可比矩阵的求解,其它都是标准过程

结果如下,其中c=(-0.0013, 0.0038, -0.0007),r=0.9969,我随机生成点云的时候是默认圆心在原点,半径为1的,所以效果很好了。

而且这个对初始值不敏感,把初始位置改到其它比较远的地方,也能收敛

代码如下:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
n = 1000;
epsilon=10e-6;

%% random points
sp = rand(n,3)*2-1;
slen = sqrt(sum(sp.^2,2));
sp = sp ./ (repmat(slen, 1, 3)+epsilon);
sr = rand(n,1)*0.2+0.9;
p = sp.*repmat(sr, 1, 3);

%pcshow(p)

%% fitting
c0=[3,4,5];%rand(1,3);
r0=rand(1,1);

c=c0;
r=r0;

tic
for k = 1:100
    vc = repmat(c,n,1);
    vr = repmat(r, n, 1);
    dc = 2*(vc-p);
    dr = -2*vr;
    J = [dc,dr];
    F= sum((vc-p).^2,2)-vr.^2;
    delta = J\(-F);
    c=c+delta(1:3)';
    r=r+delta(4);
    delta_sq = sum(delta.^2);
    if delta_sq < 0.01
        break;
    end
end
toc