平面で凸包を計算するアルゴリズム

## 概要
仕事でGPS位置情報の分析をしている。ユーザーのGPSの軌跡から何らかのモデルを作る場合、全軌跡の凸包を特徴量とするような場合がある。というわけで凸包を計算したくなり調べた。いくつかのアルゴリズムが見つかるが、Graham Scanを実装したので内容をメモしておく。

## アルゴリズム
以下では、次のような点群の凸包をGraham Scanのアルゴリズムに従って、実際に計算してみる


```python
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

from importlib import reload
import drawer
```



```python
ps = np.array([
    [1,1],
    [-1,1],
    [-1,-1],
    [1,-1],
    [0,0.5],
    [-0.5,0],
    [0,-0.5],
    [0.5,0]
])
```


![0.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/0df620e0-3fbd-7f2b-a90e-803649afa7ab.png)


### 重心を計算する
まずは、与えられた点の重心を計算する


```python
def calc_center(ps):
    return np.mean(ps, axis=0)
center = calc_center(ps)
```


![1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/873b25cc-6a89-a87f-d9ce-ab239e63174d.png)


### 重心からの角度を計算する
「`ps`の各点と重心を結ぶ線」と「x軸に平行な線」の角度を計算する。
`ps[0]`に対しては、下図のような角度を計算することになる。これを、各点に対して計算する。

![2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/2837f362-3dac-3cd4-de4c-d4fa874ecadb.png)


```python
def normalize(ps):
    return (ps.T / np.linalg.norm(ps, axis=1)).T
```



```python
def calc_angle(p):
    p2 = normalize(p)
    ac = np.arccos(p2[:,0])
    ac[np.where(p2[:,1] < 0)] *= -1
    ac[np.where(p2[:,1] < 0)] += 2*np.pi
    return ac * 180 / np.pi
```



```python
ps2 = ps - center
ps3 = normalize(ps2)
angles = calc_angle(ps3)
print("angles=", angles)
```

angles= [ 45. 135. 225. 315.  90. 180. 270.   0.]


### 重心からの角度の小さい順に並べる


```python
sorted = ps[np.argsort(angles)]
```

![3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/64be2871-616d-02d2-a2bb-fd2fd364d352.png)

### 左下の点が先頭に来るように、配列を回転


```python
ll = sorted[0]
start = 0
for i in range(1, len(sorted)):
    cur = sorted[i]
    if (cur[0] < ll[0]) or (cur[0] == ll[0] and cur[1] < ll[1]):
        ll = cur
        start = i
rotated = np.vstack([sorted[start:], sorted[:start]])
```

左下の点 = (x,y)座標の辞書式順序で最小の点は、必ず凸包に含まれることに注意する。つまり、上のコードで`rotated[0]`は、必ず答えに含まれる

![4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/7faf9b23-e63d-c479-f9d0-3bc5894d8ac7.png)


ここで`rotated`の点を、0から順にたどり、線でつないでみる。

![5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/68bb54a2-d1b4-3d6b-a5e1-3535eb1820c7.png)

今、上の多角形の辺の上を、0番目の頂点から順番に歩く旅人を想像してみる。
この旅人は、各頂点にたどり着くと、次の頂点に向かって進行方向を変更する。このとき、
この多角形の凹んでいる頂点の上では進行方向を時計回りに回転させていることが分かる。

もう少し数学的に言うと、`rotated[n+1]`が直線`rotated[n-1]rotated[n]`の右側にある場合、
旅人は、`rotated[n]`において進行方向を時計回りに回転させる。そして、そのような点は、この多角形の凹んでいるところに対応する。

つまり、旅人の軌跡をシミュレートしたときに、進行方向が時計回りに変わる点は、凸包の頂点ではないとして除外できる。


```python
def on_left(a, b, c):
    """
    Parameters
    ----------
    a, b, c: np.ndarray of shape (1, 2).
    
    Returns
    -------
    ret: bool.
    Standing on point a, watching point b, if c is left of the line ab, returned value is True.
    """
    mat = np.ones((3,3))
    mat[:,:2] = [a,b,c]
    return np.linalg.det(mat) > 0
```



```python
answer = []
n = len(rotated)
for i in range(n):
    if on_left(rotated[i-1], rotated[i], rotated[(i+1)%n]):
        answer.append(rotated[i])
answer = np.array(answer)
print(answer)
```

```{output}
[[-1. -1.]
 [ 1. -1.]
 [ 1.  1.]
 [-1.  1.]]
```

![6.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/46664f3f-ec9e-a07e-8ee6-922515df0b29.png)


この例では、上の手順によって正しく凸包を得ることができるが、実は、まだ少し足らない部分がある。

`rotated`が下のような値になる場合を考える


```python
rotated2 = np.array([
    [-1, -1],
    [0, -0.7],
    [0.3, 0],
    [1, -1],
    [1, 1],
    [-1, 1]
])
```

![7.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/b47fdb3d-12e5-cd1c-1370-07c982b90d89.png)

これに対して、先ほどと全く同じアルゴリズムで計算してみる。


```python
answer2 = []
n = len(rotated2)
for i in range(n):
    if on_left(rotated2[i-1], rotated2[i], rotated2[(i+1)%n]):
        answer2.append(rotated2[i])
answer2 = np.array(answer2)
print(answer2)
```

```
[[-1.  -1. ]
 [ 0.  -0.7]
 [ 1.  -1. ]
 [ 1.   1. ]
 [-1.   1. ]]
```

![8.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/abab4690-0d7b-c9cd-b785-36ddf37f120f.png)

結果は、上の図のとおり上手く行かない。

これは、`rotated2[0] -> rotated2[1] -> rotated2[2]`とたどったときには、`rotated[1]`での回転方向は反時計回りだったが、`rotated2[2]`が削除されたことで、ルートが`rotated2[0] -> rotated2[1] -> rotated2[3]`と変化し、それに伴い`rotated2[1]`での回転方向が変わったためである。

つまり、頂点nを削除することで直前の頂点n-1で回転方向が変わる可能性があることを考慮し、頂点nの削除後に、頂点n-1の回転方向が時計回りに変化した場合は頂点n-1も削除する（それによってさらに前の頂点n-2の方向が変わったらn-2も削除する・・と必要な限り続く）というケアをしなければならない。

これは、少しややこしいが、以下のようなコードで計算することができる。


```python
answer3 = [rotated2[0], rotated2[1]]
n = len(rotated2)
for i in range(2, n+1):
    answer3.append(rotated2[i%n])
    while len(answer3) >= 3 and not on_left(answer3[-3], answer3[-2], answer3[-1]):
        answer3 = answer3[:-2] + answer3[-1:]
answer3 = answer3[:-1]
print(answer3)
```

```
[array([-1., -1.]), array([ 1., -1.]), array([1., 1.]), array([-1.,  1.])]
```

このコードは、`for`ループの中に`while`が入り、$n^2$の計算時間がかかりそうに見えるが、
頑張ってよく考えると、O($n$)で実行できることが分かる（各点は、一度だけanswer3にappendされる。
そして、最大で1度だけanswer3からdropされる、ので）。


![9.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/b3def6bd-44c1-1b8d-74c6-8dc89571e994.png)


## 凸包を計算する関数
以上をまとめると、下のような関数を得る。


```python
def graham_scan(ps):
    """
    Parameters
    ----------
    ps: np.ndarray of shape (n, 2)
    
    Returns
    -------
    convex hull: np.ndarray of shape (m, 2), where m <= n.
    """
    center = calc_center(ps)
    ps2 = ps - center
    angle = calc_angle(ps2)
    ps3 = ps[np.argsort(angle)]
    if len(ps) <= 3:
        return ps3
    start = 0
    p0 = ps3[0]
    n = len(ps)
    for i in range(1, n):
        cur = ps3[i]
        if (cur[0] < p0[0]) or \
            (cur[0] == p0[0] and cur[1] < p0[1]):
            p0 = cur
            start = i
    ps4 = np.vstack([ps3[start:], ps3[:start], [ps3[start]]])
    ret = []
    ret.append(ps4[0])
    ret.append(ps4[1])
    for p in ps4[2:]:
        ret.append(p)
        while len(ret) >= 3 and not on_left(ret[-3], ret[-2], ret[-1]):
            ret = ret[:-2] + ret[-1:]
    return np.array(ret[:-1])
```

## 乱数で実験してみる


```python
ps3 = np.random.normal(0, 1, 200).reshape((-1,2))
gs = graham_scan(ps3)
gs
```

![10.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/7973/51ffa607-79f3-4152-91e4-0ea1c433de9d.png)

良さそう。

しかし、実は、このアルゴリズムは数値的に安定ではない（多分）。
それに関しては、また別途で記事にしたい。