Вычисление расстояния между двумя конечными 3D цилиндрами

При решении задачи выделения цилиндрических объектов из облака точек, полученного в результате трехмерного сканирования, возникла задача определения минимального расстония между двумя цилиндрами. Близкие между собой конечные цилиндры (найденные при помощи RANSAC алгоритма) и имеющие "почти" одинаковые оси, нужно было объединять в иерархическую структуру, которая по-идее представляет какой-либо единый объект.

Найти готовых скриптов (предпочтительно на Python, или C)  для решения данной задачи не удалось, поэтому пришлось решать эту задачу практически с нуля.

Модель трехмерного конечного цилиндра определяется 9 скалярными параметрами: три параметра задают одну из точек оси  симметрии цилиндра, три параметра  - направление этой оси в пространстве, один - радиус, и оставшиеся 2 - начало и конец цилиндра как координаты вдоль оси симметрии. Конечно, можно ограничиться меньшим числом параметров, например, можно потребовать чтобы норма вектора, указывающего направление оси цилиндра, была всегда единичной; это позволит избавиться от одного параметра; также можно наложить дополнительные требования на положение точки на оси симметрии; однако, для данной задачи гораздо важней простая интерпретация параметров, используемых для описания цилиндра нежели минимизация их количества.

Я использовал следующий класс для модели конечного 3d  цилиндра:

 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
37
38
39
40
41
42
43
44
import numpy as np
import scipy.optimize as opt

class Cylinder:
    def __init__(self, origin, direction, radius, start, stop):
        '''
      Model of a finite cylinder.
    '''
       
        self.o = origin
        self.l = direction/np.linalg.norm(direction)
        self.r = radius
        self.a = start
        self.b = stop
        self.n = self._get_n(origin,self.l)
        self.amat = self._a_mat(origin,self.l)
        self.iamat = np.linalg.pinv(self.amat)

    def __str__(self):
        return 'Cylinder: o=(%.5f,%.5f,%.5f), 
                   l=(%.5f,%.5f,%.5f), r=%.5f,a=%.5f,b=%.5f'%
                  (self.o[0],self.o[1],self.o[2],self.l[0],self.l[1],self.l[2],self.r,self.a,self.b) 
    
    def _get_n(self,r0,l):
        '''
        Transform orthogonal vector to l-vector
        '''
        EPS1 = 1.0e-13
        if abs(np.cross(r0,l).any())>EPS1:
            n = np.cross(r0,l)
            n = n/np.linalg.norm(n)
        elif abs(np.cross(l, [0.0,0.0,1.0]).any())>EPS1:
            n = np.cross(l,[0.0,0.0,1.0])
        elif abs(np.cross(l, [0.0,1.0,0.0]).any())>EPS1:
            n = np.cross(l,[0.0,1.0,0.0])
        elif abs(np.cross(l, [1.0,0.0,0.0]).any())>EPS1:
            n = np.cross(l,[1.0,0.0,0.0])
        return np.array(n)
    
    def _a_mat(self,r0,l):
        '''l,n - assumed to be unit vectors'''
        n = self._get_n(r0,l)
        e = np.cross(n,l)
        return np.matrix([e,n,l])

 

Дополнительно к модельным параметрам (origin, direction, radius, start, stop) для представителя класса цилиндр определены  вспомогательные функции: _get_n и _a_mat, используемые для вычисления расстояния между двумя цилиндрами.

Вычисление минимального расстояния между цилиндрами формулируется как оптимизационная задача следующего вида:

$$\begin{array}{l}r_1 (t_1,\varphi_1 ) = o_1 + a_1\cdot l_1 + l_1\cdot t_1 \cdot (b_1-a_1) + R_1\cdot A_1^{-1}\cdot U(\varphi_1)\cdot A_1 \cdot n_1 \\ r_2 (t_2,\varphi_2)  = o_2 + a_2\cdot l_2 + l_2\cdot t_2 \cdot (b_2-a_2) + R_2\cdot A_2^{-1}\cdot U(\varphi_2)\cdot A_2 \cdot n_2 \\ \min\limits_{t_1,\varphi_1,t_2,\varphi_2}||r_1(t_1,\varphi_1)-r_2(t_2,\varphi_2)||\end{array}$$

где  \(r_1, r_2\) - радиус-вектора точек, расположенных на первом и втором цилиндрах соответственно; \(t_1,\varphi_1\), \(t_2,\varphi_2\) - параметры определяющие положения точек \(r_1, r_2\)  на цилиндрах.

На представленном ниже рисунке представлена интерпретация параметров \(t,\varphi\) в зависимости от положения точки \(r\) на поверхности цилиндра. В формулах выше добавлен индекс, означающий принадлежность первому, либо второму цилиндру. Матрицы \(R, A\) - ответственны за вращение вокруг оси симметрии цилиндра и преобразование между исходной системой координат и локальной \((l,n,l\times n)\) соответственно.

Пояснение задачи оптимизации

Положение каждой из точек \(r_1, r_2\) задается двумя параметрами, которые изменяются в заданных пределах; таким образом, задача нахождения минимального расстояния между двумя конечными цилиндрами математически формулируется как задача отыскания минимума функции \(\|r_1- r_2\|\) при \(t_1 \in [0,1], \varphi_1\in[0, 2\pi], \varphi_2\in[0,2\pi]\).

 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
def cydistance(c1,c2):
    '''
    Get distance between cylinders
    '''
    assert isinstance(c1,Cylinder) and isinstance(c2,Cylinder),'Inputs should be instances of the Cylinder class'
    def r_mat(phi):
        'rotation matrix'
        return np.matrix([[np.cos(phi), -np.sin(phi), 0.0], [np.sin(phi), np.cos(phi), 0.0], [0.0,0.0,1.0]])
 
    def distance(x0):
        '''
        0<=t1,t2<=1
        0<=phi1,phi2<=2pi
        '''
        t1,phi1,t2,phi2 = x0[0],x0[1],x0[2],x0[3]
        r1 = np.matrix(c1.o+c1.a*c1.l+c1.l*t1*(c1.b-c1.a)).T+c1.r*c1.iamat*r_mat(phi1)*c1.amat*np.matrix(c1.n).T
        r2 = np.matrix(c2.o+c2.a*c2.l+c2.l*t2*(c2.b-c2.a)).T+c2.r*c2.iamat*r_mat(phi2)*c2.amat*np.matrix(c2.n).T
        return ((r1-r2).T*(r1-r2))[0,0]
    x0 = opt.differential_evolution(distance,[(0.0,1.0), (0,2.0*np.pi),(0.0,1.0),(0.0,2.0*np.pi)])
    print x0.success, 'Result = ',np.sqrt(distance(x0.x))
    return np.sqrt(distance(x0.x))

Функция cydistance находит минимальное расстояние между двумя представителями класса Cylinder; в ней осуществляется вышеупомянутая оптимизация \(\|(r_1-r_2)\|\).

Для целей оптимизации с ограничениями на значения значения параметров, используется функция differential_evolution из пакета SciPy. Данная функция оказалась наиболее подходящей (в смысле получения правильного результата с минимальными усилиями по настройке)  из представленных в пакете SciPy для решения этой задачи.

Также неплохой результат дает алгоритм последовательной оптимизации \(\|(r_1-r_2)\|\) по каждому из параметров по-очереди. В этом случае каждая итерация включает четыре одномерных оптимизационных задачи на заданных интервалах. Итерационный процесс быстро сходится. В силу своей простоты такой подход также может быть рекомендован для решения задачи определения минимального расстояния между двумя конечными цилиндрами в пространстве.

Скрипт для вычисления минимального расстояния между двумя трехмерными конечными цилиндрами можно скачать перейдя по ссылке:

Python скрипт для вычисления минимального расстояния между цилиндрами в пространстве (3,5 KB)

Для работы скрипта предполагается установленный Python 2.7.x,  а также библиотеки  SciPy v. 0.15+. Выполнить тестовый расчет можно запустив: python test.py ( в консоли linux), либо: запустив test.py в Windows.

 

blog comments powered by Disqus