OpenCV3.0 RGB-D Odometry 評価プログラムをVizを用いてかっこよく表示する。（Visual Studio 2013, C++, OpenCV3.0, 3次元点群処理）

#OpenCV3.0 RGB-D Odometry 評価プログラム

• OpenCV3.0のmodulesにはrgbdモジュールが新たに追加されている。
• RGB-D (Direct法)によるOdometry推定に加えて、ICPとRGB-D ICPがある。
• サンプルには親切にTUMのRGB-D Datasetを用いたodometry評価用のプログラムがある。
• Vizを用いて、3次元点群とカメラの軌跡を表示してみる。
• さらに、ICL-NUIMデータセットも用いれるようにした。

#Vizを用いてかっこよく表示する

• VizWindow初期化

	viz::Viz3d myWindow("Point Cloud");
	/// Pose of the widget in camera frame
	cv::Affine3d cloud_pose = cv::Affine3d().translate(cv::Vec3d(0.0f, 0.0f, 0.0f));
	/// Pose of the widget in global frame
	cv::Affine3d cloud_pose_global = cloud_pose;
	cv::Affine3d cam_pose;

• 3次元点群の表示（10フレームごとに初期フレームへ変換した3次元点群を表示）
• 数フレームごとやダウンサンプリングして表示しないと重くなる

			//10フレームごと変換して表示
			Mat rot = Rts[count](Rect(0, 0, 3, 3)).t();
			Mat tvec = Rts[count](Rect(3, 0, 1, 3)).t();
			if (count % 10 == 0){
				int downSamplingNum = 4; //e.g. 4
				Mat image2(image.rows / downSamplingNum, image.cols / downSamplingNum, CV_8UC3);
				resize(image, image2, image2.size(), 0, 0, INTER_LINEAR);
				Mat pCloud(image.rows / downSamplingNum, image.cols / downSamplingNum, CV_64FC3);

				for (int y = 0; y < 480; y += downSamplingNum){
					for (int x = 0; x < 640; x += downSamplingNum){
						if (depth.at<float>(y, x) < 8.0 && depth.at<float>(y, x) > 0.4){
							//RGB-D Dataset
							Mat pmat(1, 3, CV_64F);
							pmat.at<double>(0, 2) = (double)depth.at<float>(y, x);
							pmat.at<double>(0, 0) = (x - cx) * pmat.at<double>(0, 2) / fx;
							pmat.at<double>(0, 1) = (y - cy) * pmat.at<double>(0, 2) / fy;
							pmat = (pmat)*rot + tvec;
							Point3d p(pmat);
							pCloud.at<Point3d>(y / downSamplingNum, x / downSamplingNum) = p;
							pmat.release();
						}
						else{
							//RGB-D Dataset
							pCloud.at<Vec3d>(y / downSamplingNum, x / downSamplingNum) = Vec3d(0.f, 0.f, 0.f);
						}
					}
				}

				viz::WCloud wcloud(pCloud, image2);
				string myWCloudName = "CLOUD" + NumToString(count);
				myWindow.showWidget(myWCloudName, wcloud, cloud_pose_global);

• カメラ軌跡の表示（frustumにimageを表示することもできる）

cam_pose = cv::Affine3d(rot.t(), tvec);
			viz::WCameraPosition cpw(0.1); // Coordinate axes
			viz::WCameraPosition cpw_frustum(cv::Matx33d(cameraMatrix), /*image,*/ 0.1, viz::Color::white()); // Camera frustum
			string widgetPoseName = "CPW" + NumToString(count);
			string widgetFrustumName = "CPW_FRUSTUM" + NumToString(count);
			myWindow.showWidget(widgetPoseName, cpw, cam_pose);
			myWindow.showWidget(widgetFrustumName, cpw_frustum, cam_pose);
			myWindow.spinOnce(1, true);

#実験環境

• Visual Studio 2013
• OpenCV3.0 (rgbd, viz)
• PC : intel Core i7-4600 CPU @2.1~2.7GHz 8GBメモリ

#実行準備

• OpenCV3.0の導入はこちら
  • OpenCV 3.0 の拡張モジュール群 opencv_contrib の導入が必要
• modulesのrgbd, vizのincludeファイルはopencv300\build\includeの下にコピペしておくこと
• ソースコードはこちら
• データセットのダウンロード(TUMデータセット、ICL-NUIMデータセット)
• 実行時にRgbd, ICP, RgbdICPのいずれかを引数として指定する。（Properties→Debugging→Command Arguments)

TUMデータセット:RGB-D SLAMの評価用データセット。軌跡の評価には、モーションキャプチャシステムにより得た高精度な真値が用いられる。※evaluate_toolsを用いてRGBとDepthフレームの同期associations.txtファイルの生成が必要。

ICL-NUIMデータセット：RGB-D SLAMの評価用データセット。軌跡だけでなく3次元再構築の評価もできる。データセットはPOV-Rayのシミュレーションにより生成されている。

#結果
ダウンサンプリングせずに(downSamplingNum=1)表示した。ICPで推定したTUMデータセットfr1/xyzの結果。

• RGB-D ICPの精度が比較的良い。次にRGB-D(Direct法)。ICPは構造がはっきりしている環境でないとうまくいかない。
• RGB-Dが一番高速。（RGB-D ~30ms, ICP ~70ms, RGB-D ICP ~100ms）

#関連記事

• Vizを用いた3次元点群の表示