PCLVisualizer上でクリックした画像平面上の点の三次元座標を計算する

PCLVisualizerは点群を表示するためのGUIクラスで、マウスドラッグによって任意の視点から点群を見ることができます。

さて、ここで今の視点が世界座標系上のどの位置からなのか、またGUI上をクリックしたとき、この「画像平面上の」点は世界座標系上のどこなのかを取得する方法を調べました。(クリックした点のエピポーラ線を求めたかったので)

(追記：単に点群の中の点を選択したいだけなら、PCLVisualizerのregisterPointPickingCallback()メンバ関数を利用して、PointPickingEventを引数に持つ関数を自分で書いてあげれば、Shift Key + マウスクリックで選択した点のIDや座標を取得できます。

この記事でやろうとしているのは、クリックしたGUI画面の２次元座標を世界座標系の三次元座標へ変換することです。）

PCLVisualizerの説明はこのチュートリアルを参考にしてください。
以下の説明は上記チュートリアルの内容を踏まえて（つまりそれくらいの使い方はわかっているものとして）書きます。

カメラパラメータの取得

現在の視点のカメラパラメータですが、以下の関数で簡単に取得できます。

	boost::shared_ptr<:visualization::pclvisualizer> viewer(new pcl::visualization::PCLVisualizer("PCL Viewer"));

	/*************
	（省略）
	PointCloudのセットやマウスコールバック関数指定など
	詳しくはPCLのチュートリアル参照
	**************/
	std::vector<:visualization::camera> Cameras;
	viewer->getCameras(Cameras);

このCameraクラスを用いてView Matrix (世界座標系をカメラ座標系へ変換する4x4行列)とProjection Matrix (カメラ座標系をGUIの二次元座標上へ投影する4x4行列)を得ることができます。

	Eigen::Matrix4d prj_matrix, view_matrix;
	Cameras[0].computeProjectionMatrix(prj_matrix);	// Projection Matrix取得
	Cameras[0].computeViewMatrix(view_matrix);	// View Matrix取得

このView Matrixがそのまま現在の視点の位置/姿勢を表します。

なお、以下の関数でより直接的にカメラの位置/姿勢についてのパラメータを得ることもできます。（View MatrixやProjection Matrix自体が、以下のパラメータを用いて計算されている）

	int win_w = Cameras[0].window_size[0];	// GUI画面の幅
	int win_h = Cameras[0].window_size[1];	// GUI画面の高さ

	Eigen::Vector3d camera_pos;	//世界座標系における仮想カメラ座標
	camera_pos << Cameras[0].pos[0],Cameras[0].pos[1], Cameras[0].pos[2];

	Eigen::Vector3d camera_focal;	//仮想カメラの視線方向ベクトル
	camera_focal << Cameras[0].focal[0],Cameras[0].focal[1], Cameras[0].focal[2];

	Eigen:: Vector3d camera_up;	//仮想カメラの上方向（カメラ座標系のYベクトル）
	camera_up << Cameras[0].view[0],Cameras[0].view[1], Cameras[0].view[2];

	double near = Cameras[0].clip[0];	//奥行方向の表示範囲（手前）
	double far = Cameras[0].clip[1];	//奥行方向の表示範囲（奥）

	double fovy = Cameras[0].fovy	//画面の高さが焦点となす角度（radian）

なおCameraクラスにはwindow_posというウィンドウ位置を表すメンバ変数もあるのですが、これは常に(0,0)となっており、どうも使われていないようです。

クリック点の三次元座標算出

クリックした点のGUI上の２次元座標は、マウスイベントを用いて以下の関数で取得できます。（マウスイベントの取得方法はチュートリアル参照）

	/* pcl::visualization::MouseEvent mouse_event */
	click_x = mouse_event.getX();
	click_y = mouse_event.getY();

さて、通常ならここでProjection Matrixを利用すればクリック点のカメラ座標系における三次元座標が求まりそうですが、実はこのProjection Matrixは縦横ともに[-1,+1]に正規化された座標に投影されるようになっています。

したがってマウスイベントで取得した座標を正規化した上で、Projection Matrixの逆行列をかけてカメラ座標系におけるマウスクリック点の座標を求めます。また、View Matrixの逆行列をかけて世界座標系におけるクリック点の三次元座標を求めます。

	// 座標の正規化
	Eigen::Vector4d norm_click_pt;
	norm_click_pt << 2.0 * click_x / win_w - 1.0,
		2.0 * click_y / win_h - 1.0,
		1.0, 1.0;

	// カメラ座標系におけるクリック点の算出
	Eigen::Vector4d click_pt_camera = prj_matrix.inverse() * norm_click_pt;
	click_pt_camera(3) = 1.0;

	// 世界座標系におけるクリック点の算出
	click_pt_world = view_matrix.inverse() * click_pt_camera;

ただし、Projection Matrixからは直接焦点距離は求まらないので、ここでは焦点距離（カメラの焦点から画像平面までの距離）を1として計算しています。