はじめに

前回の記事でつかえるようにしたlidarにransacを実装して、壁などの直線を認識する。

動作環境

ubuntu20.04
ROS2 foxy
UTM-30LX

RANSACとは

RANSACはロバスト推定のアルゴリズムに一つです。測定値に外れ値が含まれていることを考慮し、その影響を押さいることを目的にしています。

理論

1.測定された点群の中からランダムに２つの点を取る。

2.その２点を通る直線を考える。

3.各点と線との距離がある設定された閾値よりも小さい個数を数える。

4.１から３を決められた回数繰り返す。
5.繰り返した中で、数え上げた点の数が一番多い直線を認識結果とする。

実装

実際のコード全体

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
import math
import matplotlib.pyplot as plt
import random
import numpy as np

class LidarNode(Node):
    def __init__(self):
        super().__init__('lidar_sample_node')
        self.ranges =[]
        self.points =[]
        self.plot_x =[]
        self.plot_y =[]
        self.lidar_sub = self.create_subscription(
            LaserScan,'/scan',self.lidar_cb,10
        )
        self.xnew =[]
        self.ynew =[]
        self.state = False
        
    def get_param(self,p_1,p_2):
        a = p_2[1]-p_1[1]
        b = p_1[0]-p_2[0]
        c = -1*p_1[0]*b-p_1[0]*a
        return a,b,c
    
    def euclid(self,params,point):
        a = params[0]
        b = params[1]
        c = params[2]
        x = point[0]
        y = point[1]
        noum =  math.sqrt(a**2+b**2)
        dist = math.fabs(a*x+b*y+c)/noum
        return dist
    
    def model(self,params,x):
        a = params[0]
        b = params[1]
        c = params[2]
        katakumi = -a/b
        seppenn = -c/b
        return katakumi*x+seppenn
    
    def ransac(self,max_loop,threshold,points):
        best_p_cnt = 0
        best_param =[]
        for i in range(max_loop):
            ran_num_1 = random.randint(0,len(points)-1)
            ran_num_2 = random.randint(0,len(points)-1)
            if ran_num_1 == ran_num_2:
                continue
            point_1 = points[ran_num_1]
            point_2 = points[ran_num_2]
            
            a,b,c = self.get_param(point_1,point_2)
            param =[]
            param.append(a)
            param.append(b)
            param.append(c)
            p_cnt =0
            for j in range(0,len(points)):
                p = points[j]
                dist = self.euclid(param,p)
                if dist <= threshold:
                    p_cnt+=1
            if p_cnt>best_p_cnt:
                best_p_cnt = p_cnt
                best_param = param
            
            print(best_p_cnt)
                
        if best_p_cnt <=150:
            print("No line ")
            self.state = False
        else:
            self.state = True
            self.xnew = np.linspace(-2,2,10)
            self.ynew =[]
            for k in range(0,len(self.xnew)):
                self.ynew.append(self.model(params=best_param,x=self.xnew[k])
    
    def lidar_cb(self,msg):
        self.ranges =msg.ranges
        self.points =[]
        self.plot_x =[]
        self.plot_y =[]
        for i in range(0,len(self.ranges)):
            point =[0,0]
            theta = math.pi*(225-0.2497*i)/180
            length = self.ranges[i]
            point[0]=length*math.cos(theta)
            point[1]=length*math.sin(theta)
            self.points.append(point)
            self.plot_x.append(point[0])
            self.plot_y.append(point[1])
        self.ransac(max_loop=300,threshold=0.001,points=self.points)    
        
        
        plt.cla()
        plt.plot(self.plot_x,self.plot_y,'o',label='lider_points')
        if self.state is True:
            plt.plot(self.xnew,self.ynew,label='lidar line')
        plt.legend(loc='lower right')
        plt.grid()
        plt.xlim(-2,2)
        plt.ylim(-2,2)
        plt.pause(0.001)
        
def main():
    rclpy.init()
    node = LidarNode()
    print('lidar_sample_node is started')
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()

if __name__=='__main__':
    main()

解説

get_param関数は、二点の座標を入れるとax+by+c=0の直線の式のパラメータa,b,cを返してくれる。
euclid関数はgetparamで得られたパラメータと点の座標を入れると、点と直線の距離の公式から距離を返してくれる。
model関数はmatplotで可視化するときの関数。直線のパラメータから直線をplotするときに使う。
ransac関数はransacを使って直線の推定を行う。引数のmax_loopは、何回ランダムに２点を取るかの最大値,thresholdは閾値,pointsは点群の配列である。
lidar_cbは受け取った/Scanトピックの座標をｘ，ｙ座標系に変換し、ransacを実行しその結果を表示しています。
実際に動かすと以下のようになります。

最後に

今回は簡単にransacを使って直線を認識してみました。
閾値のパラチュンはする時間がなく適当に行いましたが、1.5~2.0mくらいの距離まではうまく認識できていました。
ロボコンなどでは、壁の位置がわかれば機体がいま壁に対してどのような角度に向いているの修正にも使えるかもしれません。
今回は直線しか推定していませんがはじめに取る点を増やして、評価関数(今回は直線の式)を変えてあげれば他の形状の物体(ポールなど)も認識できるのではないかと思います。

参考文献

https://qiita.com/smurakami/items/14202a83bd13e55d4c09