PCLについての備忘録

#※随時更新中につき不完全な記事です

#PCLのインストール

PCL、3rdPartyともにall-in-oneインストーラを使用したものと
3rdPartyはインストーラで、PCLは自分でcmakeを使ってビルドする２パターンを試しました。
結果的にはあまり変わらなかったので、all-in-oneが楽かと思います。


ググるとでてくるので詳細は略
自分はこちらのページを参考にさせていただきました。
[Building PCL with Visual Studio ](https://gist.github.com/UnaNancyOwen/59319050d53c137ca8f3)
##環境
CPU: Intel(R) Core(TM) i7-8700K
GPU: NVIDIA GeForce GTX 1080 Ti

Windows 10 Home 64bit

Visual Studio Community 2017 (version 15.9.2)
Visual C++ SDK : VC15 (version 14.16)
CMake 3.13.0
CUDA 9.2
Point Cloud Library 1.8.1

#自分の今使っているコード
```main.cpp
#define BOOST_TYPEOF_EMULATION	//typeof_impl.hpp error issue
#define _CRT_SECURE_NO_WARNINGS
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <opencv2/opencv.hpp>   // Include OpenCV API
#include <stdio.h>
#include <direct.h>
#include <string.h>
#include <fstream>
#include <vector>
#include <math.h>
#include <numeric>
#include <time.h>
#include <unordered_set>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/filter.h>
#include <pcl/visualization/pcl_visualizer.h>
//#include <cmath>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/superres/optical_flow.hpp>

#include <opencv2/ximgproc.hpp>
using namespace std;
typedef cv::Vec<uchar, 3> Vec3b;

// ビューワー起動時の一回だけ呼ばれる
void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor(0.2, 0.2, 0.2); //black0-white1 default=0.2
	cout << "viewerOneOff" << std::endl;
}

// ビューワー起動中の毎フレーム実行される
//void viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
//{
//	cout << "viewerPsycho" << std::endl;
//}
void print4x4Matrix(const Eigen::Matrix4d & matrix)
{
	printf("Rotation matrix :\n");
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}


//画像上の座標から世界座標に変換する
//RealSenseSDKから計算部分だけとってきた
//解像度変えたらパラメータも変える 640*480
void deproject_pixel_to_point(float point[3], float intr[6], int px, int py, float depth)
{
	float x = (px - intr[4]) / intr[2];
	float y = (py - intr[5]) / intr[3];

	point[0] = depth * x;
	point[1] = depth * y;
	point[2] = depth;
}

int main()
{
	//todo: 画像名のstringつくってその組み合わせでやる
	ifstream read_matches("C:/DM/Deepmatching/ObjectDetection/output_point/frame_034_frame_047.txt");
	if (!read_matches)
	{
		std::cout << "read_matchesが開けませんでした。" << std::endl;
		std::cin.get();
		return 0;
	}
	//とりあえず10000点
	int match_points[10000][4];

	int i = 0;
	while (!read_matches.eof())
	{
		string word;
		read_matches >> match_points[i][0] >> match_points[i][1] >> match_points[i][2] >> match_points[i][3];

		if (match_points[i][0] == 0 && match_points[i][1] == 0) 	break;

		cout << "match[" << i << "]:(" << match_points[i][0] << "," << match_points[i][1] << "),(" << match_points[i][2] << "," << match_points[i][3] << ")" << endl;
		i++;

		if (i >= 10000) {
			cout << "too many match points" << endl;
			return 0;
		}

	}
	int match_size = i;
	cout << "match_size = " << match_size << endl;

	//端っこ除去するかどうか
	bool remove_flag = true;
	float intr[6] = {640, 480, 612.331055, 612.460938, 321.421997, 240.308655};

	//画像の読み込み
	char img_dir[100] = "images";
	char c_img[100] = { 0 };
	char d_img[100] = { 0 };
	sprintf(c_img, "%s/frame_034.png", img_dir);
	sprintf(d_img, "%s/Depth_034.png", img_dir);
	cv::Mat point_c = cv::imread(c_img, 1);
	if (point_c.empty()) {
		std::cout << "failed to load c_image." << std::endl;
		return 1;
	}
	cout << "c_img1 ->" << c_img << endl;
	std::cout << "c_image.size(): " << point_c.size() << std::endl;
	cv::Mat point_d = cv::imread(d_img, 0);
	if (point_d.empty()) {
		std::cout << "failed to load d_image." << std::endl;
		return 1;
	}

	std::cout << "Debug Position1" << std::endl;	//debug

	//cloudのサイズを決定するため、いったん物体領域を数える

	int count1 = 0;
	for (int h = 0; h < 480; h++) {
		for (int w = 0; w < 640; w++) {
			if ((point_d.at<uchar>(h, w) < 50) || ((point_c.at<Vec3b>(h, w)[2] == 255) && (point_c.at<Vec3b>(h, w)[1] == 255) && (point_c.at<Vec3b>(h, w)[0] == 255)) || (h < 30) || (460 < h) || (w < 30) || (580 < w)) {
				continue;
			}
			else {
				count1++;
			}
		}
	}

	// ひとつめのCloudの定義
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZRGBA>);
	cloud_source->width = count1;
	cloud_source->height = 1;
	//リサイズしたらwidthが640*480でheightが1になる
	cloud_source->points.resize(cloud_source->width * cloud_source->height);
	//NaNを含む可能性があるならfalseにする。基本はfalseでよい
	cloud_source->is_dense = false;

	cout << "Cloud1 Size : " << cloud_source->width * cloud_source->height << std::endl;


	//PointCloudを作成
	i = 0;
	for (int h = 0; h < 480; h++) {
		for (int w = 0; w < 640; w++) {

			//外側の白い領域は点に変換しない
			if (remove_flag) {
				if ((point_d.at<uchar>(h, w) < 50) || ((point_c.at<Vec3b>(h, w)[2] == 255) && (point_c.at<Vec3b>(h, w)[1] == 255) && (point_c.at<Vec3b>(h, w)[0] == 255)) || (h < 30) || (460 < h) || (w < 30) || (580 < w)) {
					continue;
				}
			}
//			pcl::PointXYZRGBA &point = p_cloud->points[w + h * p_cloud->width];
			pcl::PointXYZRGBA &point = cloud_source->points[i++];
			float p[3];
			//(w,h,d)を世界座標(p[0],p[1],p[2])に変換
			deproject_pixel_to_point(p, intr, w, h, (255-point_d.at<uchar>(h, w))*4);

			point.x = p[0];
			point.y = -p[1];
			point.z = -p[2];
			point.r = point_c.at<Vec3b>(h, w)[2];
			point.g = point_c.at<Vec3b>(h, w)[1];
			point.b = point_c.at<Vec3b>(h, w)[0];
			point.a = 0;

		}
	}

	std::cout << "Debug Position2" << std::endl;	//debug

	//cloudからNaNを除去する
	//除去しないとうまく動かない関数多いらしい、ICPの場合は無くても大丈夫そうだけど
//	std::vector<int> mapping1;
//	pcl::removeNaNFromPointCloud(*cloud_source, *cloud_source, mapping1);

	//ICP用の対応点のみのクラウド
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_source_match(new pcl::PointCloud<pcl::PointXYZRGBA>);
	cloud_source_match->width = match_size;
	cloud_source_match->height = 1;
	cloud_source_match->points.resize(cloud_source_match->width * cloud_source_match->height);
	cloud_source_match->is_dense = false;

	for (int i = 0; i < match_size; i++) {
		
			pcl::PointXYZRGBA &point = cloud_source_match->points[i];
			float p[3];
			deproject_pixel_to_point(p, intr, match_points[i][1], match_points[i][0], (255-point_d.at<uchar>(match_points[i][1], match_points[i][0]))*4);

			point.x = p[1];
			point.y = -p[0];
			point.z = -p[2];
			point.r = point_c.at<Vec3b>(match_points[i][1], match_points[i][0])[2];
			point.g = point_c.at<Vec3b>(match_points[i][1], match_points[i][0])[1];
			point.b = point_c.at<Vec3b>(match_points[i][1], match_points[i][0])[0];
			point.a = 0;
		
	}

	std::vector<int> mapping_source_match;
	pcl::removeNaNFromPointCloud(*cloud_source_match, *cloud_source_match, mapping_source_match);

		//二つ目の点群についても作成～NaN除去まで同様
	memset(c_img, 0, strlen(c_img));
	memset(c_img, 0, strlen(d_img));
	sprintf(c_img, "%s/frame_047.png", img_dir);
	sprintf(d_img, "%s/Depth_047.png", img_dir);

	cv::Mat point_c2 = cv::imread(c_img, 1);
	if (point_c2.empty()) {
		std::cout << "failed to load c_image2." << std::endl;
		return 1;
	}
	cout << "c_img2 ->" << c_img << endl;

	std::cout << "c_image2.size(): " << point_c2.size() << std::endl;
	//		imshow("color",point_c2);
	//		cv::waitKey(0);
	cv::Mat point_d2 = cv::imread(d_img, 0);
	if (point_d2.empty()) {
		std::cout << "failed to load d_image2." << std::endl;
		return 1;
	}
	//	imshow("depth",point_d2);
	//	cv::waitKey(0);

	int count2 = 0;
	for (int h = 0; h < 480; h++) {
		for (int w = 0; w < 640; w++) {
			if ((point_d2.at<uchar>(h, w) < 50) || ((point_c2.at<Vec3b>(h, w)[2] == 255) && (point_c2.at<Vec3b>(h, w)[1] == 255) && (point_c2.at<Vec3b>(h, w)[0] == 255)) || (h < 30) || (460 < h) || (w < 30) || (580 < w)) {
				continue;
			}
			else {
				count2++;
			}
		}
	}

	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZRGBA>);
	cloud_target->width = count2;
	cloud_target->height = 1;
	cloud_target->points.resize(cloud_target->width * cloud_target->height);
	cloud_target->is_dense = false;

	i = 0;
	for (int h = 0; h < 480; h++) {
		for (int w = 0; w < 640; w++) {
			if (remove_flag) {
				//depth ga toretenai mataha object no ryouiki janai tennha mushi
				if ((point_d2.at<uchar>(h, w) < 50) || ((point_c2.at<Vec3b>(h, w)[2] == 255) && (point_c2.at<Vec3b>(h, w)[1] == 255) && (point_c2.at<Vec3b>(h, w)[0] == 255)) || (h < 30) || (460 < h) || (w < 30) || (580 < w)) {
					continue;
				}
			}
			//				pcl::PointXYZRGBA &point = cloud_target->points[w + h * cloud_target->width];
			pcl::PointXYZRGBA &point = cloud_target->points[i++];
			float p[3];

			deproject_pixel_to_point(p, intr, w, h, (255-point_d2.at<uchar>(h, w))*4);
			point.x = p[0];
			point.y = -p[1];
			point.z = -p[2];
			point.r = point_c2.at<Vec3b>(h, w)[2];
			point.g = point_c2.at<Vec3b>(h, w)[1];
			point.b = point_c2.at<Vec3b>(h, w)[0];
			point.a = 0;
		}
	}
//	std::vector<int> mapping2;
//	pcl::removeNaNFromPointCloud(*cloud_target, *cloud_target, mapping2);

	//ICP用の対応点のみのクラウド
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_target_match(new pcl::PointCloud<pcl::PointXYZRGBA>);
	cloud_target_match->width = match_size;
	cloud_target_match->height = 1;
	cloud_target_match->points.resize(cloud_target_match->width * cloud_target_match->height);
	cloud_target_match->is_dense = false;

	for (int i = 0; i < match_size; i++) {

		pcl::PointXYZRGBA &point = cloud_target_match->points[i];
		float p[3];
		deproject_pixel_to_point(p, intr, match_points[i][3], match_points[i][2], (255-point_d2.at<uchar>(match_points[i][3], match_points[i][2]))*4);

		point.x = p[1];
		point.y = -p[0];
		point.z = -p[2];
		point.r = point_c2.at<Vec3b>(match_points[i][3], match_points[i][2])[2];
		point.g = point_c2.at<Vec3b>(match_points[i][3], match_points[i][2])[1];
		point.b = point_c2.at<Vec3b>(match_points[i][3], match_points[i][2])[0];
		point.a = 0;

//		cout << "target_match point[" << i << "] = (" <<   p[1] << ", " << -p[0] << ", " << -p[2] << ")" << endl;

	}

	std::vector<int> mapping_target_match;
	pcl::removeNaNFromPointCloud(*cloud_target_match, *cloud_target_match, mapping_target_match);

	std::cout << "Debug Position3" << std::endl;	//debug

	pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;

	//ICPの初期値
	//	icp.setMaxCorrespondenceDistance(0.05);
  	icp.setMaximumIterations(20);					//最大繰り返し数、デフォで10
	//	icp.setUseReciprocalCorrespondences(true);	//相互で最近傍の点だけ使う？デフォでfalse
	//	icp.setTransformationEpsilon(1e-8);			//transfomationの差が閾値以下で終了
	//	icp.setEuclideanFitnessEpsilon(1);			//二つの点群のユークリッド二乗誤差が閾値以下で終了
	std::cout << "criteria_1:" << *icp.getConvergeCriteria() << std::endl;

	//maybe-later:点群の数の違いが影響しているなら直す
	std::cout << count1 << " and " << count2 << std::endl;
	//	std::cout << decrease_points(cloud_source, cloud_target, new_cloud, count1, count2) << std::endl;

	bool use_match = false;
	if (use_match) {
		icp.setInputSource(cloud_source_match);
		icp.setInputTarget(cloud_target_match);
	}
	else {
		icp.setInputSource(cloud_source);
		icp.setInputTarget(cloud_target);
	}

	std::cout << "Debug Position4" << std::endl;



	pcl::PointCloud<pcl::PointXYZRGBA> Final;
	//	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Final(new pcl::PointCloud<pcl::PointXYZRGBA>);
	//	pcl::PointCloud<pcl::PointXYZ> Final;

	icp.align(Final);
	std::cout << "criteria_2:" << *icp.getConvergeCriteria() << std::endl;
	std::cout << "Debug Position5" << std::endl;

	pcl::io::savePCDFileBinary("icp_result.pcd", Final);

	std::cout << "has converged:" << icp.hasConverged() << " score: " <<
		icp.getFitnessScore() << std::endl;
	std::cout << icp.getFinalTransformation() << std::endl;
	//FitnessScoreは呼び出されてから計算してるから時間かかる？

	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	transformation_matrix = icp.getFinalTransformation().cast<double>();
	print4x4Matrix(transformation_matrix);

	std::cout << "Debug Position6" << std::endl;

	pcl::transformPointCloud(*cloud_source, *cloud_source, transformation_matrix);

	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr F_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>(Final));
	//	pcl::PointCloud<pcl::PointXYZ>::Ptr Final_cloud(new pcl::PointCloud<pcl::PointXYZ>(Final));

	*F_cloud += *cloud_target;

	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_icped = cloud_source;
	*cloud_icped += *cloud_target;

	std::cout << "Debug Position7" << std::endl;
		// ビューワーの作成
	pcl::visualization::CloudViewer viewer("PointCloudViewer");
	if (use_match) {
		viewer.showCloud(cloud_icped);
	}
	else {
		viewer.showCloud(F_cloud);
	}


	// ビューワー起動時の一回だけ呼ばれる関数をセット
	viewer.runOnVisualizationThreadOnce(viewerOneOff);

	// ビューワー起動中の毎フレーム実行される関数をセット
//	viewer.runOnVisualizationThread(viewerPsycho);

	// ビューワー視聴用ループ
	while (!viewer.wasStopped())
	{

	}
	return 0;
}
```


#関数
###点群を書き込む
```C++
pcl::io::savePCDFileBinary(const std::string &file_name, const pcl::PointCloud< PointT > &cloud);
//ヘッダはテキスト形式で、データ部分はバイナリ形式で保存
//使用例：pcl::io::savePCDFileBinary("cloud_icp_034_and_050.pcd", cloud);

pcl::io::savePCDFileASCII(const std::string &file_name, const pcl::PointCloud< PointT > &cloud);
//全部テキスト形式で保存

pcl::io::savePCDFileBinaryCompressed(const std::string &file_name, const pcl::PointCloud< PointT > &cloud);
//ヘッダはテキスト形式で、データ部分はLZF形式で圧縮保存
```
###点群を読み込む
```C++
pcl::io::loadPCDFile("cloud_icp_034_and_050.pcd", cloud);
```

###MeshLab等で読み込むために.plyで保存する場合
```C++
std::string writePath = "your/path";
pcl::io::savePLYFileBinary(writePath, *sceneCloudPtr);
```

#ICPのアルゴリズム解説

###学習するのに参考にしたサイト

[かみやんの技術者ブログ - ICP(Iterative Closest Point)のアルゴリズム](https://kamiyan.hatenablog.jp/entry/20081107)
[Qiita - PCLでICPアルゴリズムを使ってみる。@nnn12358](https://qiita.com/nnn112358/items/05c370bb455c5b8ac054)

###アルゴリズム（簡易）
1. Selection :    1つまたは2つのメッシュからいくつかの点を選択する
2. Matching :     1つの点が他方のメッシュのどこに対応するか求める
3. Weighting:     重み付け
4. Rejecting :    不要なペア（点対）を除外する
5. Error Metric : 誤差量を求める
6. Minimizing :   誤差量を最小化する

###公式コメントの翻訳（意訳）

IterativeClosestPoint は　ICPアルゴリズムの基本操作である。
変換はSVD(Singular Value Decomposition)に基づいて推定される。

###終了条件
1. ユーザーが設定した最大繰り返し数に到達したとき（setMaximumIterations）
2. 最新の変換とそのひとつ前の変換の差が、ユーザーが設定したε未満になったとき
　（setTransformationEpsilon）
3. 二つの点群のユークリッド二乗誤差の和が閾値未満のとき（setEuclideanFitnessEpsilon）

`setMaximumIterations (int nr_iterations) { max_iterations_ = nr_iterations; }`となる。
`max_iterations_`の初期値は10


###使用例

```c++
     IterativeClosestPoint<PointXYZ, PointXYZ> icp;
     // Set the input source and target
     icp.setInputCloud (cloud_source);
     icp.setInputTarget (cloud_target);
  
     // Set the max corresphitoondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
     icp.setMaxCorrespondenceDistance (0.05);
     // Set the maximum number of iterations (criterion 1)
     icp.setMaximumIterations (50);
     // Set the transformation epsilon (criterion 2)
     icp.setTransformationEpsilon (1e-8);
     // Set the euclidean distance difference epsilon (criterion 3)
     icp.setEuclideanFitnessEpsilon (1);
    
     // Perform the alignment
     icp.align (cloud_source_registered);
    
     // Obtain the transformation that aligned cloud_source to cloud_source_registered
     Eigen::Matrix4f transformation = icp.getFinalTransformation ();
 ```


>IterativeClosestPointクラスによって使用されるDefaultConvergenceCriteriaへのポインターを返します。
これにより、align（）メソッドの後に収束状態を確認したり、設定したりできます。
align（）メソッドが呼び出される前は、ICP APIを介して利用できないDefaultConvergenceCriteriaのパラメーター。 alignメソッドはmax_iterations_、euclidean_fitness_epsilon_、transformation_epsilon_を設定するため、DefaultConvergenceCriteriaインスタンスのデフォルト/設定値をオーバーライドすることに注意してください。
IterativeClosestPointのDefaultConvergenceCriteriaへのポインターをreturnする。

[pcl::registration::CorrespondenceRejectorReciprocal Class Reference](http://docs.pointclouds.org/1.0.0/classpcl_1_1registration_1_1_correspondence_rejector_reciprocal.html)

CorrespondenceRejectorReciprocal implements a reciprocal correspondence rejection method for ICP-like registration algorithms.

>ターゲットとソース間の対応（相互検索）が与えられると、ターゲットポイント（一致インデックス）がソース内のクエリポイント（クエリインデックス）とは異なる対応ポイントを持ち、距離の差が特定のしきい値を超えるすべての対応を削除します。