2016-12-12

ＩＣＰアルゴリズムメモ

アルゴリズムコンピュータビジョンプログラミング

何したい

3次元点の位置合わせをしたい．

アルゴリズム

ICP（Iterative Closest Point）はデータ形状をモデル形状に合わせるアルゴリズム．
点の対応は未知でも良いけど，大まかな位置合わせは行っている前提で，高精度に位置を合わせる．

モデル形状： $A$
データ形状： $B$
収束判定閾： $\tau$

1. 大まかに位置を合わせる．

初期並進： $\textbf{t}_0$
初期回転： $\textbf{R}_0$

2. 近傍点の計算する．
データ点 $\textbf{b}_i$ から最も近いモデル形状 $A$ の点を
${ 　\begin{equation} \boldsymbol{\alpha}_i = \mathop{argmin}\limits_{\textbf{a}_i \in A}||\textbf{a}_i-\textbf{b}_i|| \end{equation} }$
とする．

3. 位置合わせパラメータの計算する．

重心を計算する．

${\displaystyle 　\begin{equation} \boldsymbol{\mu}_{a} = \frac{1}{N}\sum^{N}_{i=1} \boldsymbol{\alpha}_i 　,\qquad \boldsymbol{\mu}_{b} = \frac{1}{N}\sum^{N}_{i=1} \textbf{b}_i \end{equation} }$

共分散行列を計算する．

${\displaystyle \begin{equation} \boldsymbol{\Sigma}_{ab} = \frac{1}{N}\sum^{N}_{i=1} (\boldsymbol{\alpha}_i-\boldsymbol{\mu}_a)(\boldsymbol{b}_i-\boldsymbol{\mu}_b)^{\top} \end{equation} }$

共分散行列を特異値分解する．

${\displaystyle \begin{equation} \boldsymbol{\Sigma}_{ab} = \boldsymbol{U} \begin{pmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3 \end{pmatrix} \boldsymbol{V}^{\top} \end{equation} }$

回転行列を計算する．

${\displaystyle \begin{equation} \boldsymbol{R} = \boldsymbol{U} \begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(\boldsymbol{U}\boldsymbol{V}^{\top}) \end{pmatrix} \boldsymbol{V}^{\top} \end{equation} }$

並進ベクトルを計算する．

${\displaystyle \begin{equation} \boldsymbol{t} = \boldsymbol{\mu}_a - \boldsymbol{R}\boldsymbol{\mu}_b \end{equation} }$

4. 誤差を計算する．
${\displaystyle \begin{equation} d_{err} = \frac{1}{N} \sum^{N}_{i=1} || \boldsymbol{\alpha}_i - \boldsymbol{R}\boldsymbol{b}_i - \boldsymbol{t} ||^{2} \end{equation} }$
　
5. 収束判定をする．

ひとつ前の2乗距離誤差： $pred_{err}$

もし，
$\displaystyle pred_{err} - d_{err} < \tau$
なら， $\textbf{R}$ と $\textbf{t}$ を返して終了する．
そうでなければ， $pred_{err} \gets d_{err}$ として2へ戻る．

メモ

最近傍の点の対応付けは，KD-treeなどのデータ構造を用いると高速に行えるそう．
収束を高速化したり，形状のスケールを調節するパラメータを推定したりする派生もある．

参考

「コンピュータビジョン最先端ガイド３」，アドコム・メディア株式会社

2016-11-12

ＰＣＬインストールとＰＬＹ出力

PCL プログラミング C++

何したい

3次元点群の見栄えを良くするためにアウトライヤーを取り除きます．
このアウトライヤー除去は，PCL(Point Cloud Library)のフィルタリングを使う．
使い方を記録します．

環境

Ubunut14.04

インストール

Prebuilt binaries for Linux - Point Cloud Library (PCL)を参考にコマンドを入力します．

sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl
sudo apt-get update
sudo apt-get install libpcl-all

アウトライヤー除去のサンプルプログラム

除去のアルゴリズムはK-means法を使って，近傍の点の距離の平均値が大きいもの（離れている）を除去する．
下のサンプルは，PCL公式のフィルタリングでアウトライヤー除去をするプログラムの出力部分を
PLY形式でも出力するように改良した．

// statistical_removal.cpp
#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

int
main (int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

    // Fill in the cloud data
    pcl::PCDReader reader;
    // Replace the path below with the path where you saved your file
    reader.read<pcl::PointXYZ> ("table_scene_lms400.pcd", *cloud);

    std::cerr << "Cloud before filtering: " << std::endl; 
    std::cerr << *cloud << std::endl;

    // Create the filtering object
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud);
    sor.setMeanK (50);
    sor.setStddevMulThresh (1.0);
    sor.filter (*cloud_filtered);

    std::cerr << "Cloud after filtering: " << std::endl;
    std::cerr << *cloud_filtered << std::endl;

    pcl::PCDWriter writer;
    pcl::PLYWriter ply_writer;
    writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);
    ply_writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.ply", *cloud_filtered, false);

    sor.setNegative (true);
    sor.filter (*cloud_filtered);
    writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);
    ply_writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.ply", *cloud_filtered, false);

    return (0);
}

プログラムのコンパイルは，CMakeLists.txtを書いて

# CMakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(statistical_removal)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_definitions("-Wall -std=c++11")

add_executable (statistical_removal statistical_removal.cpp)
target_link_libraries (statistical_removal ${PCL_LIBRARIES})

cmakeしてmakeをする．