Point Cloud Libraryのレンジデータを画像として保存

個人的な必要性があって、レンジデータを画像として保存する関数を書きました。
PCL(Point Cloud Library)1.2まではpcl::visualization::RangeImageVisualizerというクラスにsavePPM()という関数があって、これでレンジデータを画像に保存できたようなのですが、どうもver1.3以降この関数が見当たらなくなってました。
ここのチュートリアルでは、ちゃんと解説されてるんですが、、、、

というわけで、已む無く自作しました。PCLのpcl::RangeImageをOpenCVのcv::Mat型へ変換し、ファイル保存します。
まだPCLは使い始めたばかりなので車輪の再発明をしている可能性もありますが、同じように悩む人のために公開しておきます。

Point Cloud Libraryとはそもそもなんじゃい？という方は、こちらの解説記事を御覧ください。
DERiVE【シリーズ「PCLを触ってみよう!」第一回】3D点群処理ライブラリ「Point Cloud Library」の概要

尚、ポイントクラウドデータ(PCD)ファイルは、こちらからサンプルを取得できます。

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#ifndef INFINITY
#define INFINITY HUGE_VAL
#endif

#ifndef NAN
#define NAN 0xffc00000
#endif

void main()
{
	// ポイントクラウドデータをファイルから読み込み
	pcl::PointCloud cloud
	pcl::io::loadPCDFile("data/trex/row_0.pcd", cloud);

	// レンジデータパラメータ
	float angularResolution = (float) (  0.5f * (M_PI/180.0f));  //   解像度0.5度（ラジアン表記）
	float maxAngleWidth     = (float) (360.0f * (M_PI/180.0f));  // 横の最大角度幅
	float maxAngleHeight    = (float) (180.0f * (M_PI/180.0f));  // 縦の最大角度幅
	Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);  // 移動
	pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;  // カメラ座標原点を原点とする
	float noiseLevel=0.00;
	float minRange = 0.0f;
	int borderSize = 1;

	// ポイントクラウドからレンジデータを生成
	pcl::RangeImage rangeImage;
	rangeImage.createFromPointCloud(cloud, angularResolution, maxAngleWidth, maxAngleHeight,
		sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);

	// レンジデータの最大値と最小値を取得
	float min_range, max_range;
	rangeImage.getMinMaxRanges(min_range, max_range);
	float min_max_range = max_range - min_range;

	// 結果画像領域を生成
	cv::Mat& image(rangeImage.height, rangeImage.width, CV_8UC3);

	unsigned char r,g,b;
	for(int y=0; y < rangeImage.height; y++){
		for(int x=0; x(y,x)[0] = b;
			image.at(y,x)[1] = g;
			image.at(y,x)[2] = r;
		}
	}
	// 画像を保存
	cv::imwrite("range_image.png", image);
}

結果はこんな感じです。