課題: Rigid-ICP法の実装

課題1: Point to Point ICPの実装

これまでに解説した内容を元にしてPoint to Point ICPを実装しよう。マーチング・キューブ法の課題と同様に、最初の時点では実装すべき箇所にNOT_IMPL_ERROR()と書かれており、実行するとこの箇所でエラーが発生するようになっている。

ICPに必要な計算を繰り返す部分についてはテンプレートで既に与えてあるので、Point to Point ICPの1ステップ分の処理を実装すれば良い。実装に必要な変更箇所は以下の通り。

• 入力頂点のそれぞれに対して、ターゲット頂点の中で最も近い点をk-d木を用いて探索する。その後、探索の結果を$N \times 3$の大きさの行列の$\mathbf{X}$と$\mathbf{P}$を用意し、その各行に入力点の座標と近傍点の座標をそれぞれ格納する。
• 入力点の位置の平均と近傍点の位置の平均を求めて、$\mathbf{X}$, $\mathbf{P}$の各行に格納された座標から、それぞれ減算する。
• Eigenを用いて特異値分解を行う関数 (eigenSVDがsvd.h内に用意されている)を用いて、回転行列$\mathbf{R}$を求め、さらにそれを用いて平行移動料$\mathbf{t}$を求める。

上記の実装すべき内容についてはテンプレート内にも同じものが記述されているので、そちらも参考にすると良い。

課題2: Point to Plane ICPの実装

Point to Point ICPの課題と同様に、剛体変換を求める計算を繰り返す部分についてはテンプレートで既に与えてあるので、Point to Plane ICPの1ステップ分の処理を実装すれば良い。実装に必要な変更箇所は以下の通り。

• Point to Plane ICPの説明中で述べた行列$\mathbf{A} \in \mathbb{R}^{6 \times 6}$とベクトル$\mathbf{b} \in \mathbb{R}^{6}$を用意し、ゼロで初期化しておく。
• その後、入力の各点に対して、ターゲット点内の最近傍点をk-d木を用いて求めつつ、$\mathbf{A}$ならびに$\mathbf{b}$を説明に従って計算する。
• 部分ピボット付きLU分解による線形問題の解法 PartialPivLU を用いて、$\mathbf{A}\mathbf{u} = \mathbf{b}$を解く。興味のある人はそれ以外の線形ソルバを用いても良い。
• 前工程で得られた$\mathbf{u} \in \mathbf{R}^6$から回転行列$\mathbf{R}$と平行移動料$\mathbf{t}$を求める。回転軸と回転量から回転行列を求める際には、icp.cpp内に実装された rodrigues関数を使うと良い。

こちらについても、Point to Point ICPの課題と同様に、実装すべき内容はソースコード中にも記述してあるので、そちらも参考にすると良い。