OpenCVのVizモジュールを使ってカメラの位置と点群を表示

三次元再構成なんかの論文では、よくカメラの位置を四角錘で可視化した図が乗ってたりしますが、その図もOpenCVのVizモジュールを使えば簡単に実現できます。

ここにチュートリアルもありますが、一応解説を日本語で書いておきます。

尚、Vizモジュールを使うためにはVTKをあらかじめインストールした上で、CMake上でVTKのパスを指定してOpenCVをビルドする必要があります。

#include 

using namespace std;
using namespace cv;

void main(){
	// 3Dを表示するWindow生成
	string winname = "Viz Camera Pose";
	viz::Viz3d myWindow(winname);

	// 画面に座標軸を表示
	myWindow.showWidget("Coordinate Widget", viz::WCoordinateSystem());

	// カメラパラメータ（3x3の３次元座標を２次元画像平面へ投影するための行列）
	double k_elms[] = { 9.803769e+02, 0.000000e+00, 6.900000e+02, 0.000000e+00, 9.757217e+02, 2.441228e+02, 0.000000e+00, 0.000000e+00, 1.000000e+00 };
	Matx33d K(k_elms);

	// カメラウィジェット作成（青）
	viz::WCameraPosition wcamera(K, 1.0, viz::Color::blue());

	// 画面にカメラ追加
	myWindow.showWidget("Camera", wcamera);

	// カメラの姿勢を設定
	Mat T = Mat::eye(4, 4, CV_32FC1);
	T.at(2, 3) = -2.0;  // Z座標の設定
	myWindow.setWidgetPose("Camera", cv::Affine3f(T));

	// 円柱状の点群を作成
	int N = 10000;
	float r = 1.0;
	Mat cloud_mat(N, 1, CV_32FC3);
	Vec3f pt;
	for (int i = 0;i < N;i++) {
		float deg = CV_PI / 90 * (i % 180);
		pt(0) = r * std::cos(deg);
		pt(1) = r * std::sin(deg);
		pt(2) = r * CV_PI / 90 * (i / 180) + 0.5;
		cloud_mat.at(i, 0) = pt;
	}

	// 点群ウィジェット（白）を作成
	viz::WCloud wcloud(cloud_mat, viz::Color::white());

	// 点群ウィジェットを画面へ追加
	myWindow.showWidget("Cloud", wcloud);

	// 表示
	myWindow.spin();
}

3Dを表示するためのウィンドウはcv::viz::Viz3dクラスから生成します。

	// 3Dを表示するWindow生成
	string winname = "Viz Camera Pose";
	viz::Viz3d myWindow(winname);

ここではViz3dクラスから生成したmyWindowに対し、メンバ変数showWidget()を使用してカメラウィジェットや点群ウィジェットを追加しています。

	// 画面にカメラ追加
	myWindow.showWidget("Camera", wcamera);

ウィジェットの姿勢はデフォルトでは単位行列（つまり原点）ですが、Viz3dのメンバ変数setWidgetPose()によって変更できます。

カメラの位置を指定する場合、カメラ座標系から世界座標系への4x4変換行列を指定します。

	// カメラの姿勢を設定
	Mat T = Mat::eye(4, 4, CV_32FC1);
	T.at(2, 3) = -2.0;  // Z座標の設定
	myWindow.setWidgetPose("Camera", cv::Affine3f(T));

点群を表示するためにはviz::WCloudクラスから点群ウィジェットを作成します。まず、CV_32FC3, CV_32FC4, CV_64FC3, CV_64FC4,
いずれかの型のcv::Matを作成し、その行列の各要素に点群の点の座標を格納します。ここではCV_32FC3型（32bit3チャネルの浮動小数点型）のcv::Matを作成し、それぞれのチャネルがX、Y、Z座標を表します。

	Mat cloud_mat(N, 1, CV_32FC3);

点群に対しても同様にsetWidgetPose()を使って姿勢を変更できますが、ここでは指定してません。

最後にshowWidget()で点群を追加した後、spin()で作成した画面を表示させます。

	// 点群ウィジェット（白）を作成
	viz::WCloud wcloud(cloud_mat, viz::Color::white());

	// 点群ウィジェットを画面へ追加
	myWindow.showWidget("Cloud", wcloud);

	// 表示
	myWindow.spin();