OpenCV3.0 Vizを用いた3次元点群の表示(VisualStudio2013, C++, OpenCV3.0)

#viz::WCloudを見つけたので3次元点群を色付きで表示してみた

##3次元点群可視化用のVizWindow生成

• viz::Viz3d myWindow("Point Cloud");

##rgb, depthデータの読み込み

• サンプル（rgb.png, depth.png）がmodules/rgbd/testdata/rgbd/odometryの中にあります

	float fx = 525.0f, // default
		  fy = 525.0f,
		  cx = 319.5f,
		  cy = 239.5f;

	Mat colorImage;
	Mat depth, depth_flt;

	colorImage = imread("rgb.png");
	depth = imread("depth.png", -1);
	imshow("rgb", colorImage);
	imshow("depth", depth);

	depth.convertTo(depth_flt, CV_32FC1, 1.f / 5000.f);
	depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0);
	depth = depth_flt;

3次元点群の生成

	int height = 480, width = 640;
	Mat pCloud(height, width, CV_32FC3);
	for (int y = 0; y < 480; y++){
		for (int x = 0; x < 640; x++){
			if (depth.at<float>(y, x) < 8.0 && depth.at<float>(y, x) > 0.4){
				//RGB-D Dataset
				float Z = depth.at<float>(y, x);
				float X = (x - cx) * Z / fx;
				float Y = (y - cy) * Z / fy;
				pCloud.at<Vec3f>(y, x) = Vec3f(X, Y, Z);
			}
			else{
				//RGB-D Dataset
				pCloud.at<Vec3f>(y, x) = Vec3f(0.f, 0.f, 0.f);
			}
		}
	}

##VizWindowに表示

	viz::WCloud wcloud(pCloud, colorImage);
	myWindow.showWidget("CLOUD", wcloud);
	myWindow.spin();

• 3次元点群と色画像のサイズは同じでないといけません
• showWidgetの名前を"CLOUD2"のように変えていくことで簡単に点群を増やして表示できます
• ループの中で点群を増やす場合、myWindow.spinOnce(1, true);で更新すればいいです

ソースコードはこちら

#実行準備
OpenCV3.0の導入についてはこちら

Additional Include Directories
$(OPENCV_DIR)\build\include;
C:\Program Files (x86)\PCL 1.7.2\3rdParty\VTK\include\vtk-5.10;

Additional Library Directories
$(OPENCV_DIR)\build\x86\vc12\lib;
C:\Program Files (x86)\PCL 1.7.2\3rdParty\VTK\lib\vtk-5.10;

opencv_vtk_lib.hppはopencv300\build\includeの下に置いてください

※widget_accessorを使わないときVTKは不要です。

#実行結果
最初はこんな感じ。表示するときの座標系が違う。

ドラッグで回転
Shift+ドラッグで平行移動
Ctrl+ドラッグで角度調整
スクロールでズーム

#関連記事
Creating Widgets (Viz+VTK)