RustでRANSACを実装して、点群データから平面検出をする

こんにちは！株式会社コードリックの足立です。
弊社では、点群データを利用したサービスも開発しています。
今回は、点群データから平面を検出するプログラムをRustで作成したのでその紹介をします。

やりたいこと

以下の図は、LiDARセンサーから取得したデータをFAST-LIOを呼ばれる手法でマッピングした点群データです。
写っているのは弊社の事務所で、イメージがつきやすいように実際の写真と並べておきます。



目を凝らせばなんとなく机のようなもの、椅子のようなもの、モニタのようなものが見えてくるかもしれません。

点群は単純に3次元座標にプロットされた点の集合なので、どの点とどの点が物理的につながっているかなどの物理世界の情報は、完全に欠落してしまいます。
しかしながら、頑張ればある程度復元できる情報もあるので、今回は手法がよく知られている平面検出をやってみます！

RANSAC

平面検出を行うために、RANSACと呼ばれる手法を用います。
RANSACは数学的なモデルを作るときに使える汎用的な手法で、Random sample consensusから名前が付けられています。
consensus（コンセンサス）とは、外れ値ではない値(inlier)から生成される数学的モデルのことを指し、
ランダムで得られたモデルを使い、外れ値を除いた新しいデータセットを作り、そのデータセットからモデルを作成するということを繰り返し行います。
一番よくできたモデルを採択すれば、確率的ではあるものの、ある程度よいモデルをお手軽に得ることができます。

今回は以下のように実装しました。
1. 点群から3点ランダムに選んで、平面を1つ作る
2. 点群の全データに対して、平面との距離を計算し、閾値以下の点を平面に所属させる
3. 平面に所属する点の数が大きければ大きいほどよいモデルとして採択する

これらは実際のRANSACとは異なる部分があります。
2と3の間に、本当は所属する点で再度モデルを作る必要がありますが、平面フィッティングに特異値分解の計算が必要で、諸般の事情(記事の執筆期限の都合)で今回は省略しました。

実装

Rustによる実装を行いました。Rustだから困る部分というのは特になく、スムーズに実装することができました。
以下にRANSACのメインロジックの部分のみ掲載します。

use crate::plane3d::Plane;
use crate::point3d::Point3D;
use crate::point_cloud::PointCloud;
use rand::Rng;
/// 点群に対してRANSACを行い平面検出を行う
pub fn ransac(point_cloud: PointCloud, threshold: f32, iterations: usize) -> (Plane, Vec<Point3D>) {
    // 最良のモデル
    let mut best_plane: Plane = Plane::new_without_color(0.0, 0.0, 0.0, 0.0);
    let mut point_in_best_plane: Vec<Point3D> = vec![];
    let mut best_plane_size: usize = 0;
    // 進捗度表示
    let mut progress: usize = 0;
    for _ in 0..iterations {
        if iterations >= progress * iterations / 10 {
            println!("{}% done", progress * 10);
            progress += 1;
        }
        // 平面に属する点を集める
        let mut point_in_plane: Vec<Point3D> = vec![];
        let mut plane_size: usize = 0;
        let mut used_points: Vec<Point3D> = Vec::new();
        let mut unused_points: Vec<Point3D> = Vec::new();
        let mut rng = rand::thread_rng();
        let mut random_points = vec![];
        // 3点をランダムに選んで平面を作成
        for _ in 0..3 {
            let random_point: Point3D =
                point_cloud.points[rng.gen_range(0..point_cloud.points.len())].clone();
            random_points.push(random_point.clone());
            used_points.push(random_point.clone());
        }
        let plane: Plane =
            Plane::gen_plane_from_points(&random_points[0], &random_points[1], &random_points[2]);
        // 平面に属する点を集める
        for point in point_cloud.points.iter() {
            if !used_points.contains(point) {
                let distance = point.distance_to_plane(&plane);
                if distance < threshold {
                    point_in_plane.push(point.clone());
                    used_points.push(point.clone());
                    plane_size += 1;
                } else {
                    unused_points.push(point.clone());
                }
            }
        }
        // 平面に属する点が多ければよいモデルとして更新
        if plane_size > best_plane_size {
            point_in_best_plane = point_in_plane;
            best_plane = plane;
            best_plane_size = plane_size;
        }
    }
    (best_plane, point_in_best_plane)
}

載せきれない部分に関しては、以下のレポジトリを確認してください。
https://github.com/adachi-codelic/ransac-rust

実験

完成したので早速使ってみましょう！
iteration = 1000, threshold = 0.001として得られた平面がこちらです。

ちょうど床の部分の平面を検出することができました！

またRANSACを繰り返し適用していけば、部屋の壁をくり抜くことも可能です。

まとめ

今回はRANSACというアルゴリズムで、点群データから平面を検出するプログラムを作成しました。
今流行りのRustで書きましたが、(趣味で使っているのもあり)特につまずくことなく書くことができました。
弊社では、まだRustを使った案件はありませんが、使用するコンセンサスが高まれば、今後は使っていく案件などもあるかもしれません。
以上で終わりです。
次の記事はもっと面白いものになると思うので、ぜひブックマークをお願いします！