RANSACで直線検出

RANSACとは

外れ値の影響を除外してモデルのパラメータを推定する手法。こちら に最小二乗法とRANSACの比較があり分かりやすいです。

アルゴリズム

ここでは直線検出を例にします。

1. 検出範囲内の点群データから2点をランダムサンプリング
2. 選んだ2点を結ぶ直線の傾きと切片を算出
3. 検出範囲内の点群データの1点にコスト関数を適用
4. コスト関数の結果が、閾値以下となったデータ数をカウント
5. 1~4を繰り返し、カウント数が最大となった傾きと切片を推定値とする

コスト関数

直線検出では、点と直線の距離の公式を用いる。
$$
d=\frac{|ax+by+c|}{\sqrt{a^2+b^2}}\tag{1}
$$
コスト関数次第で、どんな形状も検出することができる。

実装

2DLidar点群から壁までの距離と角度を推定するプログラムを作りました。
LidarはUTM30LXを使用しています。

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
import random
import numpy as np
import math

# UTM30LX
reso = math.radians(0.25)
bias = math.radians(45)
# parameter
min_range = 440 # Lidarの探索範囲
max_range = 640 # Lidarの探索範囲
interation = 100
threshold = 10 # 閾値[mm]

def ransac(msg):
    best_p_cnt = 0
    for i in range(interation):
        ## 全データから2点をランダムサンプリング
        rp1 = np.random.randint(low = min_range, high = (len(msg.ranges) - 1) / 2 - 1)
        rp2 = np.random.randint(low = (len(msg.ranges) - 1) / 2 , high = max_range)
        rp1_dist = msg.ranges[rp1] * 1000.0
        rp2_dist = msg.ranges[rp2] * 1000.0
        rp1_y = rp1_dist * math.sin(rp1 * reso - bias)
        rp1_x = -1.0 * rp1_dist * math.cos(rp1 * reso - bias)
        rp2_y = rp2_dist * math.sin(rp2 * reso - bias)
        rp2_x = -1.0 * rp2_dist * math.cos(rp2 * reso - bias)
        ## 選んだ2点から傾きと切片を算出
        slope = (rp2_y - rp1_y) / (rp2_x - rp1_x)
        intercept = rp1_y - slope * rp1_x
        p_cnt = 0
        ## 全データにコスト関数を適用
        for j in range(rp1, rp2):
            p_dist = msg.ranges[j] * 1000.0
            p_y = p_dist * math.sin(j * reso - bias)
            p_x = -1.0 * p_dist * math.cos(j * reso - bias)
            ## コスト関数
            d = math.fabs(slope * p_x - p_y + intercept) / math.sqrt(slope**2 + 1**2)
            ## コスト関数の結果が、閾値以下となったデータ数をカウント
            if d <= threshold:
                p_cnt += 1
        if p_cnt > best_p_cnt:
            best_p_cnt = p_cnt
            best_slope = slope
            best_intercept = intercept
    deg = math.degrees(-math.atan(best_slope)) 
    dist = best_intercept * math.cos(-math.atan(best_slope))
    print(deg, dist)

rospy.init_node('ransac')
scan_sub = rospy.Subscriber('scan', LaserScan, ransac)
rospy.spin()