【MATLABで点群処理】Farthest Point Samplingの実装

この記事では

点群のダウンサンプリングの1手法であるFarthest Point Samplingについて、MATLABコードを記します。

環境

MATLAB R2025a Pre-release
※特に新しい関数を使っているわけではないので、もっと古くても動くと思います。

Farthest Point Sampling

点群のダウンサンプリング法には様々なものが提案されていますが、決めた点数ぴったりに間引きつつ、物体形状はできるだけ損なわないという利点を持つのがFPSです。

手順は次の通りです。

1. まず適当にサンプリング点を1点決める
2. サンプリング点から全点までの距離を計算する。
3. 距離が最大となる点を次のサンプリング点とする。
4. 直前のサンプリング点から全点までの距離を計算する。
5. 全点について、2で計算した距離と4で計算した距離のうち、小さい方を最小距離とする。
6. 5で選択した最小距離が最大となる点を次のサンプリング点とする。
7. 4-6を、規定の点数となるまで繰り返す。

要はそれまでに選択した点の全てから遠い点を順次選んでいく、という力技です。計算効率は良くないですが、均一な点が得られます。

MATLAB実装

MATLABにはpcdownsampleという点群を間引くための関数はありますが、ランダムサンプリングや、ボクセルグリッドサンプリングは用意されているものの、FPSはないため、実装してみました。

function [pt_down,idx] = pcdonwsample_fps(pt, k)
% 入力:
%   P (Nx3 double): 入力点群データ (N: 点数, 3: x,y,zの3次元点群を想定)
%   k (int)       : サンプリングする点数
%
% 出力:
%   pt_down: 間引いた後の点群オブジェクト
%   idx: 間引きに使った入力点群オブジェクトの点のインデックス

% 有効な点だけで処理する
[pt_valid,validIdx] = removeInvalidPoints(pt);

N = pt_valid.Count;
P = pt_valid.Location;
% --- 初期化 ---
first_idx = 1;
sampled_indices = repmat(first_idx,[k 1]); % サンプリングされた点のインデックスを格納

% 各点から、選択済み点群への最短距離を保持する配列 (最初は無限大)
min_distances = inf(N, 1);

% --- メインループ (k-1回繰り返す) ---
for i = 2:k
    % --- 一個前のサンプリング点を選択 ---
    last_selected_point = P(sampled_indices(i-1), :);

    % 前回選択された点(last_selected_point)と *全ての点* との距離を計算
    diff_vectors = P - last_selected_point; % 差分
    dists_to_last = vecnorm(diff_vectors, 2, 2); % 各行のL2ノルム(距離)を計算 (Nx1のベクトル)

    % 各点について、これまでの最短距離と、前回選択点への距離の小さい方を採用
    min_distances = min(min_distances, dists_to_last);

    % 最も遠い点を選択
    [~, current_farthest_idx] = max(min_distances);

    % --- 結果を格納、次回の準備 ---
    sampled_indices(i) = current_farthest_idx;    
end

pt_down = pt_valid.select(sampled_indices);
idx = validIdx(sampled_indices);
end

使ってみます。

% データ読込み
pt = pcread('teapot.ply')
k = 5000;

% FPS実行
pt_down = pcdonwsample_fps(pt, k)

figure;
pcshow(pt);
title('オリジナル');

figure;
pcshow(pt_down);
title('FPSによる間引き後')

実行結果

pt = 

  pointCloud のプロパティ:

     Location: [41472×3 single]
        Count: 41472
      XLimits: [-3 3.4340]
      YLimits: [-2 2]
      ZLimits: [0 3.1500]
        Color: []
       Normal: []
    Intensity: []

pt_down = 

  pointCloud のプロパティ:

     Location: [5000×3 single]
        Count: 5000
      XLimits: [-2.9984 3.4281]
      YLimits: [-1.9994 1.9994]
      ZLimits: [0 3.1493]
        Color: []
       Normal: []
    Intensity: []

きちんと狙った点数ぴったりになっていることがわかります。

オリジナル

FPSの結果

均一に間引かれつつもボクセルグリッドみたいな規則性は見られず、でも元の形をきちんと保持している様子がわかります。しっかり検証したわけではないですが、たぶん動いてると思います。たぶん…。

まとめ

この記事ではFPSをMATLABで実装してみました。基本的な点群処理の機能ですが、PointNet++や異常検知で使われるPatchCoreなど使いどころは多いです。いろんな手法を使いこなしていきましょう！