Engineering
Glossvation Engineering
IoT・エッジ

RealSense 複数カメラの ICP + FPFH キャリブレーション — ボード不要で現場展開

複数の深度カメラの座標系を点群ベースで統一する ICP キャリブレーションの実装メモ。FPFH による初期対応付け、PCL (C++) でのパイプライン、精度特性と誤差累積の実測値。

Glossvation ·

解きたい問題

複数の深度カメラ (Intel RealSense 等) を一列に配置し、長尺の物体全体を計測したい。 各カメラは独立した座標系で 3D 点群を返すので、それらを 1 つの共通座標系に揃える 必要があります。

従来は キャリブレーションボード (チェッカーボード・ArUco マーカー) を各カメラ間に置いて位置合わせする手法が定番でしたが、

  • ボードの位置を物理的に調整する手間
  • 現場 (工場ライン等) では毎回ボード設置が難しい
  • カメラ数が増えると作業量が爆発

という課題がありました。本稿では 点群ベースの ICP + FPFH でボードなしキャリブレーションを行う実装を解説します。

参考: 点群ベースのマルチカメラキャリブレーション (JSPE 2017)

前提条件

本稿の内容は、以下のセットアップを前提にしています。

ハードウェア / 設置

  • 深度カメラ (RealSense シリーズ等) を 2 台以上 使用
  • カメラ同士の 視野を 30% 以上重ねて 配置 (完全に独立した視野だと特徴点の対応付けができない)
  • 各カメラの設置位置はおおよそ ±30° 以内の精度で手動粗合わせ済 (ICP の初期値代わり)
  • カメラは 起動後 10〜30 分ウォームアップ 済 (深度値ドリフト対策)

ソフトウェア

  • C++17 対応コンパイラ (GCC 9+ / Clang 10+) と CMake 3.15+
  • PCL (Point Cloud Library) 1.12+ (apt install libpcl-dev or source build)
  • 各カメラから .pcd / .ply 形式で点群を取得できる環境 (RealSense SDK: librealsense2 + rs2::pointcloud)

計測シーン

  • 対象物に 特徴のある形状 (角・溝・凹凸) が含まれている
  • 計測中はカメラが動かない (三脚等で固定)
  • 屋内 or 日陰 での計測 (直射日光は IR パターン投影式センサーの精度を大きく落とす)

必要な精度

  • 本稿の手法で到達する精度は 1 ペアあたり ±15〜20mm 程度
  • サブ mm の高精度が必要な用途 (精密加工・医療等) では、この手法では不足。構造光スキャナや産業用レーザートラッカーを検討

キャリブレーションをしないと何が起きるか

「各カメラが独立した 3D 座標で点群を返す」とはつまり、同じ対象物を計測しても カメラごとに違う座標値 が出力されるということです。

対象物 (実際の位置 X=1000mm)
  CAM0 の座標系 → P = (120, 45, 950)    ← CAM0 から見た相対座標
  CAM1 の座標系 → P = (-230, 55, 1020)  ← CAM1 から見た相対座標
  CAM2 の座標系 → P = (-580, 48, 1010)  ← CAM2 から見た相対座標

これらを そのまま統合しようとした場合 の実測影響:

作業キャリブなし本手法適用後
全長計測 (CAM0 の端 → CAM4 の端)数百 mm オーダーのズレ (各カメラ設置位置のラフ精度に完全依存)±60〜80 mm (累積誤差)
複数カメラの点群統合同じ対象物が 複数の場所に重複して見える1 つのクリーンな統合点群になる
物体サイズ測定カメラ切替地点で寸法が破綻する連続した寸法が取れる

つまり キャリブレーションは 「あった方が良い」 ではなく 「やらないと使い物にならない」 のがマルチカメラ計測の実情です。本稿の手法は、その必須作業を ボードなしで数分で実行可能にする のが目的です。

全体像

[CAM0] [CAM1] [CAM2] [CAM3] [CAM4]
  📦      📦      📦      📦
  ↑ 隣接カメラの重複視野に段ボール等の特徴物を配置
  1. 全カメラが同じ段ボール (特徴の多い物体) を撮影
  2. 各点群から FPFH (Fast Point Feature Histograms) を計算して局所形状を特徴ベクトル化
  3. 隣接カメラ間で特徴ベクトルが近い点を対応付け → グローバルマッチング
  4. ICP (Iterative Closest Point) で精密位置合わせ
  5. 全カメラの変換行列 (R, t) が決まり、共通座標系に統一

ICP 単体だけだと初期姿勢が悪いと局所解に陥るので、FPFH で粗マッチ → ICP で精細化 の 2 段構えが実務の定番です。

FPFH とは

各 3D 点の周辺の形状を 33 次元のヒストグラム で表現する特徴量 (Rusu, 2009)。

CAM0 の点 P ──FPFH→ [0.1, 0.3, 0.0, 0.2, ..., 0.05] (33 次元)
CAM1 の点 Q ──FPFH→ [0.1, 0.3, 0.0, 0.2, ..., 0.06] (33 次元)
                         ↑ ベクトルが近い → 対応点候補

同じ形状の角・エッジ・曲面は近い値になるので、「対応点を探す」問題がシンプルなベクトル距離計算に帰着します。

PCL (Point Cloud Library) による実装

C++ + PCL で書いた最小パイプラインを示します。実運用では C++ の方が RealSense SDK (librealsense2) と直接統合しやすく、エッジデバイス (Jetson / x86 IPC) でもネイティブに動きます。

CMakeLists.txt

cmake_minimum_required(VERSION 3.15)
project(icp_calibration)

find_package(PCL 1.12 REQUIRED COMPONENTS common io registration features filters)

add_executable(icp_calibration icp_calibration.cpp)
target_link_libraries(icp_calibration PRIVATE ${PCL_LIBRARIES})
target_include_directories(icp_calibration PRIVATE ${PCL_INCLUDE_DIRS})

icp_calibration.cpp

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/registration/icp.h>

using PointT = pcl::PointXYZ;
using PointNormalT = pcl::PointNormal;
using FeatureT = pcl::FPFHSignature33;

static constexpr float VOXEL = 0.01f;  // 1cm ダウンサンプリング

// ダウンサンプリング + 法線推定 + FPFH 計算
void preprocess(const pcl::PointCloud<PointT>::Ptr& in,
                pcl::PointCloud<PointNormalT>::Ptr& out,
                pcl::PointCloud<FeatureT>::Ptr& features) {
  // ダウンサンプリング
  pcl::PointCloud<PointT>::Ptr down(new pcl::PointCloud<PointT>);
  pcl::VoxelGrid<PointT> vg;
  vg.setInputCloud(in);
  vg.setLeafSize(VOXEL, VOXEL, VOXEL);
  vg.filter(*down);

  // 法線推定
  pcl::NormalEstimationOMP<PointT, PointNormalT> ne;
  ne.setInputCloud(down);
  ne.setRadiusSearch(VOXEL * 2.0);
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  ne.compute(*normals);

  out->resize(down->size());
  for (size_t i = 0; i < down->size(); ++i) {
    out->points[i].x = down->points[i].x;
    out->points[i].y = down->points[i].y;
    out->points[i].z = down->points[i].z;
    out->points[i].normal_x = normals->points[i].normal_x;
    out->points[i].normal_y = normals->points[i].normal_y;
    out->points[i].normal_z = normals->points[i].normal_z;
  }

  // FPFH 特徴量
  pcl::FPFHEstimationOMP<PointNormalT, PointNormalT, FeatureT> fpfh;
  fpfh.setInputCloud(out);
  fpfh.setInputNormals(out);
  fpfh.setRadiusSearch(VOXEL * 5.0);
  fpfh.compute(*features);
}

// FPFH によるグローバル (粗) マッチング: RANSAC Prerejective
Eigen::Matrix4f global_register(
    const pcl::PointCloud<PointNormalT>::Ptr& src,
    const pcl::PointCloud<PointNormalT>::Ptr& tgt,
    const pcl::PointCloud<FeatureT>::Ptr& src_feat,
    const pcl::PointCloud<FeatureT>::Ptr& tgt_feat) {
  pcl::SampleConsensusPrerejective<PointNormalT, PointNormalT, FeatureT> align;
  align.setInputSource(src);
  align.setSourceFeatures(src_feat);
  align.setInputTarget(tgt);
  align.setTargetFeatures(tgt_feat);
  align.setMaximumIterations(100000);
  align.setNumberOfSamples(3);
  align.setCorrespondenceRandomness(5);
  align.setSimilarityThreshold(0.9f);
  align.setMaxCorrespondenceDistance(VOXEL * 1.5f);
  align.setInlierFraction(0.25f);

  pcl::PointCloud<PointNormalT> aligned;
  align.align(aligned);
  return align.getFinalTransformation();
}

// ICP で精細化 (Point-to-Plane)
Eigen::Matrix4f refine_icp(
    const pcl::PointCloud<PointNormalT>::Ptr& src,
    const pcl::PointCloud<PointNormalT>::Ptr& tgt,
    const Eigen::Matrix4f& init) {
  pcl::IterativeClosestPointWithNormals<PointNormalT, PointNormalT> icp;
  icp.setInputSource(src);
  icp.setInputTarget(tgt);
  icp.setMaxCorrespondenceDistance(VOXEL);
  icp.setMaximumIterations(50);
  icp.setTransformationEpsilon(1e-8);

  pcl::PointCloud<PointNormalT> aligned;
  icp.align(aligned, init);
  std::cout << "ICP converged: " << icp.hasConverged()
            << ", fitness: " << icp.getFitnessScore() << std::endl;
  return icp.getFinalTransformation();
}

int main() {
  pcl::PointCloud<PointT>::Ptr cam0(new pcl::PointCloud<PointT>);
  pcl::PointCloud<PointT>::Ptr cam1(new pcl::PointCloud<PointT>);
  pcl::io::loadPCDFile("cam0.pcd", *cam0);
  pcl::io::loadPCDFile("cam1.pcd", *cam1);

  auto src = pcl::make_shared<pcl::PointCloud<PointNormalT>>();
  auto tgt = pcl::make_shared<pcl::PointCloud<PointNormalT>>();
  auto src_feat = pcl::make_shared<pcl::PointCloud<FeatureT>>();
  auto tgt_feat = pcl::make_shared<pcl::PointCloud<FeatureT>>();

  preprocess(cam0, src, src_feat);
  preprocess(cam1, tgt, tgt_feat);

  auto rough = global_register(src, tgt, src_feat, tgt_feat);
  std::cout << "Rough transform:\n" << rough << std::endl;

  auto fine = refine_icp(src, tgt, rough);
  std::cout << "Final transform:\n" << fine << std::endl;
  return 0;
}

ビルド & 実行

mkdir build && cd build
cmake ..
make -j$(nproc)
./icp_calibration

得られる fine は Eigen::Matrix4f の 4×4 剛体変換行列 (回転 R + 並進 t)。CAM0 の点を CAM1 の座標系に変換できます。

Jetson / 組み込みでの注意

  • PCL のビルドは重い (数十分)。Jetson ではキャッシュイメージを使い回すか、apt install libpcl-dev で既存パッケージを利用
  • OpenMP (-fopenmp) を有効にしないと法線推定・FPFH が CPU 1 コアしか使えず遅い
  • RealSense SDK と組み合わせる場合は rs2::pointcloud → pcl::PointCloud 変換関数を別途用意する

カメラチェーンへの拡張

隣接ペアで変換行列を順次求め、最初のカメラを基準に連鎖させます。

T_01 : CAM0 → CAM1
T_12 : CAM1 → CAM2
T_23 : CAM2 → CAM3

CAM0 基準に全カメラの座標を揃える:
  CAM0: I (単位行列)
  CAM1: T_01
  CAM2: T_12 @ T_01
  CAM3: T_23 @ T_12 @ T_01

誤差の実測値 (断面測定想定)

項目誤差備考
深度センサー精度 (3m 想定)±10〜20 mmセンサー物理限界
ICP キャリブレーション 1 ペアの RMSE±15〜20 mmFPFH 粗マッチ → ICP 精細化後
4 ペア (CAM0→CAM4) の累積誤差±60〜80 mm単純加算、実測はもう少し小さい

累積誤差の軽減策:

  • ループクロージャ: CAM0 と CAM4 の重複視野があれば閉ループで誤差分散
  • Bundle Adjustment: 全ペアを同時最適化 (Open3D multiway_registration)
  • カメラ数を減らす (センサーの有効距離を活かす)

現場適用のポイント

1. 段ボール・箱の配置

  • 隣接カメラの重複視野 に置く (最低 1 ペアあたり 30% 以上の重複)
  • 均一な面は特徴量が少なく対応付けに失敗 → 角・溝・凹凸のある物体 を選ぶ
  • 動かないように固定 (計測中にずれると累積誤差)

2. ダウンサンプリング解像度

  • VOXEL = 1cm だと処理 1〜3 秒、精度 ±15mm
  • VOXEL = 5mm だと処理 10 秒超、精度は数 mm 改善だが負荷大
  • 現場要件に合わせたトレードオフ を事前実験で決める

3. 外れ値除去

  • RANSAC で粗外れを弾いた後、ICP でさらに閾値内の点だけで最適化
  • max_correspondence_distance は「許容する対応距離」。センサー精度と同程度に設定

落とし穴

1. 初期姿勢が極端にズレていると収束しない

ICP は局所最適化なので、カメラ配置が物理的に大きくずれていると FPFH 粗マッチも失敗する。 カメラ設置時に ±30° 程度の精度で手動粗合わせ しておくと安定。

2. 深度センサーのウォームアップ

深度センサーは 起動後 10〜30 分でレーザー温度が安定 するまで深度値がドリフトする。 キャリブレーション前に必ずウォームアップ運用を。

3. 日光下での計測

太陽光 (特に直射) は IR パターン投影式センサーの精度を大きく落とす。 屋外用途なら ToF 系や LiDAR を検討。

4. 変換行列の保存と再読込

キャリブレーション結果は JSON / NumPy 配列で保存し、本番計測では再計算せず読み込む。

np.save("T_01.npy", fine.transformation)
# ...
T = np.load("T_01.npy")

5. 温度変化で再キャリブが必要

工場の季節変動 (気温 ±10℃) で機械的歪みが入り、変換行列が合わなくなることがある。 月次 or 季節変わり目に自動再キャリブ するフローを組む。

まとめ

  • 複数 RealSense カメラの座標系統一は FPFH + ICP の 2 段パイプラインで、ボードなしで実現可能
  • Open3D を使えば実装は数十行
  • 1 ペアあたり ±15〜20mm の RMSE、カメラ数×ペア数 の累積誤差を見積もる
  • 本番運用では 段ボール配置・ウォームアップ・定期再キャリブ を手順化
#RealSense #ICP #FPFH #点群 #PCL #C++

関連記事