《A Novel Rock-Mass Point Cloud Registration Method Based on Feature Line Extraction and Feature Poin

《A Novel Rock-Mass Point Cloud Registration Method Based on Feature Line Extraction and Feature Point Matching》读后感

问题1：III Methodology — A Feature Line Extraction 里面有这么一共公式
原文对图片的描述为

At the same time, if the following Equation (3) is satisfied, the number of supervoxels is increased by 1.

但是文中超体素是融合过程，并不是分裂，为什么能够在P,Q的基础上使number of supervoxel 加1呢？

#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件 
#include <pcl/registration/icp.h>
#include"box.h"
#include <pcl/features/boundary.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include<pcl\filters\radius_outlier_removal.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>
#include<pcl/features/principal_curvatures.h>
#include <pcl/registration/ia_fpcs.h>
#include<cmath>
struct supervoxel {
	//std::vector<double> represent;
	std::vector<std::vector<double>> nonrepresent;
};
void initializeMatrix(std::vector<std::vector<double>>& A, int size) {
	std::vector<double> a = { 0 };
	a.resize(size);
	A.resize(size);
	for (int i = 0; i < size; i++) {
		A[i].resize(a.size());
	}
}
class vectorMatrixCal
{
public:
	void LU(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& L, std::vector<std::vector<double>>& U);
	double vectorT_vector(std::vector<double> vec1, std::vector<double> vec2);
	void vector_vectorT(std::vector<double> vec1, std::vector<double> vec2, Eigen::Matrix4f& T);
	void vector_vectorT(std::vector<double> vec1, std::vector<double> vec2, std::vector<std::vector<double>>& M);
	void vectorConst(double& c, std::vector<double>& Ai, std::vector<double>& c_Ai);
	void vectorConst_d(std::vector<double>& A, double& c, std::vector<double>& M);
	void vectorPlusVector(std::vector<double>& v1, std::vector<double>& v2, std::vector<double>& v);
	void matrix_vector(std::vector<std::vector<double>>& B_Inv, std::vector<double>& c, std::vector<double>& v);
	void matrix_matrix(Eigen::Matrix4f& A, Eigen::Matrix4f& B, Eigen::Matrix4f& T);
	void matrix_matrix(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& B, std::vector<std::vector<double>>& M);
	void matrixInverse(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& M);
	void matrixConst(Eigen::Matrix4f& A, double c, Eigen::Matrix4f& T);
	void matrixConst(std::vector<std::vector<double>>& A, double c, std::vector<std::vector<double>>& M);
	void matrixConst_d(std::vector<std::vector<double>>& A, double& c, std::vector<std::vector<double>>& M);
	void matrixPlusMatrix(Eigen::Matrix4f& A, Eigen::Matrix4f& B, Eigen::Matrix4f& T);
	void matrixPlusMatrix(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& B, std::vector<std::vector<double>>& T);
	void matrixOne(Eigen::Matrix4f& A, Eigen::Matrix4f& T);
	void matrixOne(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& T);
	void matrixTranspo(Eigen::Matrix4f& A, Eigen::Matrix4f& T);
	void matrixTranspo(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& T);
	void vectorCrossProduct(std::vector<double>& a, std::vector<double>& b, std::vector<double>& c);
};
double vectorMatrixCal::vectorT_vector(std::vector<double> vec1, std::vector<double> vec2)
{
	double c;
	c = vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2];
	return(c);
}
void vectorMatrixCal::vector_vectorT(std::vector<double> vec1, std::vector<double> vec2, Eigen::Matrix4f& T)
{
	T = Eigen::Matrix4f::Identity();
	T(0, 0) = vec1[0] * vec2[0]; T(0, 1) = vec1[0] * vec2[1]; T(0, 2) = vec1[0] * vec2[2];
	T(1, 0) = vec1[1] * vec2[0]; T(1, 1) = vec1[1] * vec2[1]; T(1, 2) = vec1[1] * vec2[2];
	T(2, 0) = vec1[2] * vec2[0]; T(2, 1) = vec1[2] * vec2[1]; T(2, 2) = vec1[2] * vec2[2];
}
void vectorMatrixCal::vector_vectorT(std::vector<double> vec1, std::vector<double> vec2, std::vector<std::vector<double>>& M)
{
	for (int i = 0; i < vec1.size(); i++) {
		for (int j = 0; j < vec2.size(); j++) {
			M[i][j] = vec1[i] * vec2[j];
		}
	}
}
void vectorMatrixCal::vectorConst(double& c, std::vector<double>& Ai, std::vector<double>& c_Ai) {
	for (int i = 0; i < Ai.size(); i++) {
		c_Ai[i] = c * Ai[i];
	}
}
void vectorMatrixCal::vectorPlusVector(std::vector<double>& v1, std::vector<double>& v2, std::vector<double>& v) {
	for (int i = 0; i < v1.size(); i++) {
		v[i] = v1[i] + v2[i];
	}
}
void vectorMatrixCal::matrix_vector(std::vector<std::vector<double>>& B_Inv, std::vector<double>& c, std::vector<double>& v) {
	for (int i = 0; i < c.size(); i++) {
		for (int j = 0; j < c.size(); j++) {
			v[i] += B_Inv[i][j] * c[j];
		}
	}
}
void vectorMatrixCal::matrix_matrix(Eigen::Matrix4f& A, Eigen::Matrix4f& B, Eigen::Matrix4f& T)
{
	T = Eigen::Matrix4f::Identity();
	T(0, 0) = A(0, 0) * B(0, 0) + A(0, 1) * B(1, 0) + A(0, 2) * B(2, 0);
	T(0, 1) = A(0, 0) * B(0, 1) + A(0, 1) * B(1, 1) + A(0, 2) * B(2, 1);
	T(0, 2) = A(0, 0) * B(0, 2) + A(0, 1) * B(1, 2) + A(0, 2) * B(2, 2);

	T(1, 0) = A(1, 0) * B(0, 0) + A(1, 1) * B(1, 0) + A(1, 2) * B(2, 0);
	T(1, 1) = A(1, 0) * B(0, 1) + A(1, 1) * B(1, 1) + A(1, 2) * B(2, 1);
	T(1, 2) = A(1, 0) * B(0, 2) + A(1, 1) * B(1, 2) + A(1, 2) * B(2, 2);

	T(2, 0) = A(2, 0) * B(0, 0) + A(2, 1) * B(1, 0) + A(2, 2) * B(2, 0);
	T(2, 1) = A(2, 0) * B(0, 1) + A(2, 1) * B(1, 1) + A(2, 2) * B(2, 1);
	T(2, 2) = A(2, 0) * B(0, 2) + A(2, 1) * B(1, 2) + A(2, 2) * B(2, 2);

}
void vectorMatrixCal::matrix_matrix(std::vector<std::vector<double>>& V, std::vector<std::vector<double>>& B, std::vector<std::vector<double>>& M) {
	M.clear();
	initializeMatrix(M, V.size());
	for (int i = 0; i < V.size(); i++) {
		for (int j = 0; j < V[0].size(); j++) {
			for (int k = 0; k < V.size(); k++) {
				M[i][j] += V[i][k] * B[k][j];
			}
		}
	}
}
void vectorMatrixCal::matrixInverse(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& X)
{
	std::vector<std::vector<double>> L;
	std::vector<std::vector<double>> U;
	LU(A, L, U);
	//正向和逆向替换
	std::vector<double> y; y.resize(U[0].size());
	std::vector<std::vector<double>> E; initializeMatrix(E, U.size()); for (int a = 0; a < E.size(); a++) { E[a][a] = 1; }
	initializeMatrix(X, U.size());
	double item = 0; double item2 = 0;
	for (int h = 0; h < X.size(); h++) {
		for (int i = 0; i < L.size(); i++) {
			if (i == 0) {
				y[i] = E[i][h];
			}
			else {
				for (int j = 0; j < i; j++) {
					item += L[i][j] * y[j];
				}
			}
			y[i] = E[i][h] - item;
			item = 0;
		}
		for (int i = y.size() - 1; i >= 0; i--) {
			if (i == y.size() - 1) {
				X[i][h] = y[i] / U[i][i];
			}
			else {
				for (int j = i + 1; j < y.size(); j++) {
					item2 += U[i][j] * X[j][h];
				}
				X[i][h] = (y[i] - item2) / U[i][i];
				item2 = 0;
			}
		}
		y.clear(); y.resize(U[0].size());
	}
}
void vectorMatrixCal::matrixConst(Eigen::Matrix4f& A, double c, Eigen::Matrix4f& T)
{
	T = Eigen::Matrix4f::Identity();
	T(0, 0) = c * A(0, 0);
	T(0, 1) = c * A(0, 1);
	T(0, 2) = c * A(0, 2);

	T(1, 0) = c * A(1, 0);
	T(1, 1) = c * A(1, 1);
	T(1, 2) = c * A(1, 2);

	T(2, 0) = c * A(2, 0);
	T(2, 1) = c * A(2, 1);
	T(2, 2) = c * A(2, 2);
}
void vectorMatrixCal::matrixConst(std::vector<std::vector<double>>& A, double c, std::vector<std::vector<double>>& M)
{
	for (int i = 0; i < A.size(); i++) {
		for (int j = 0; j < A[0].size(); j++) {
			M[i][j] = c * A[i][j];
		}
	}
}
void vectorMatrixCal::matrixConst_d(std::vector<std::vector<double>>& A, double& c, std::vector<std::vector<double>>& M)
{
	for (int i = 0; i < A.size(); i++) {
		for (int j = 0; j < A[0].size(); j++) {
			M[i][j] = A[i][j] / c;
		}
	}
}
void vectorMatrixCal::vectorConst_d(std::vector<double>& A, double& c, std::vector<double>& M)
{
	for (int i = 0; i < A.size(); i++) {
		M[i] = A[i] / c;
	}
}
void vectorMatrixCal::matrixPlusMatrix(Eigen::Matrix4f& A, Eigen::Matrix4f& B, Eigen::Matrix4f& T)
{
	T = Eigen::Matrix4f::Identity();
	T(0, 0) = A(0, 0) + B(0, 0);
	T(0, 1) = A(0, 1) + B(0, 1);
	T(0, 2) = A(0, 2) + B(0, 2);

	T(1, 0) = A(1, 0) + B(1, 0);
	T(1, 1) = A(1, 1) + B(1, 1);
	T(1, 2) = A(1, 2) + B(1, 2);

	T(2, 0) = A(2, 0) + B(2, 0);
	T(2, 1) = A(2, 1) + B(2, 1);
	T(2, 2) = A(2, 2) + B(2, 2);

}
void vectorMatrixCal::matrixPlusMatrix(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& B, std::vector<std::vector<double>>& M)
{
	if (M.size() == 0) {
		initializeMatrix(M, A.size());
	}
	for (int i = 0; i < A.size(); i++) {
		for (int j = 0; j < A[0].size(); j++) {
			M[i][j] = A[i][j] + B[i][j];
		}
	}
}
void vectorMatrixCal::matrixOne(Eigen::Matrix4f& A, Eigen::Matrix4f& T) {
	double D = A(0, 0) * (A(1, 1) * A(2, 2) - A(1, 2) * A(2, 1))
		- A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0))
		+ A(0, 2) * (A(1, 0) * A(2, 2) - A(1, 1) * A(2, 0));
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			T(i, j) = A(i, j) / D;
		}
	}
}
void vectorMatrixCal::matrixOne(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& T) {
	double D = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1])
		- A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0])
		+ A[0][2] * (A[1][0] * A[2][2] - A[1][1] * A[2][0]);
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			T[i][j] = A[i][j] / D;
		}
	}
}
void vectorMatrixCal::matrixTranspo(Eigen::Matrix4f& A, Eigen::Matrix4f& T) {
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			T(i, j) = A(j, i);
		}
	}
}
void vectorMatrixCal::matrixTranspo(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& T) {
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			T[i][j] = A[j][i];
		}
	}
}
void vectorMatrixCal::vectorCrossProduct(std::vector<double>& a, std::vector<double>& b, std::vector<double>& c) {

	double x1, x2, y1, y2, z1, z2, h, m, n;
	x1 = a[0]; y1 = a[1]; z1 = a[2];
	x2 = b[0]; y2 = b[1]; z2 = b[2];
	h = y1 * z2 - z1 * y2;  //计算三阶行列式
	m = z1 * x2 - x1 * z2;
	n = x1 * y2 - y1 * x2;
	c[0] = h; c[1] = m; c[2] = n;
	//cout << "( " << h << " " << m << " " << n << " )" << endl;
}
void vectorMatrixCal::LU(std::vector<std::vector<double>>& A, std::vector<std::vector<double>>& L, std::vector<std::vector<double>>& U)
//不需要初始化LU
{
	std::vector<std::vector<double>> vw;
	std::vector<std::vector<double>> A_;
	initializeMatrix(L, A.size());
	initializeMatrix(U, A.size());
	int i = 0;
	vectorMatrixCal C;
	std::vector<double> v, w;
	do {
		U[i][i] = A[0][0];
		L[i][i] = 1;
		for (int j = 1; j < A.size(); j++) {
			v.push_back(A[j][0]);
			w.push_back(A[0][j]);
		}
		for (int j = i + 1; j < L.size(); j++) {
			L[j][i] = A[j - i][0] / A[0][0];
			U[i][j] = A[0][j - i];
		}
		initializeMatrix(vw, v.size());
		initializeMatrix(A_, vw.size());
		C.vector_vectorT(v, w, vw);
		double a00 = -A[0][0];
		C.matrixConst_d(vw, a00, vw);
		for (int k = 0; k < (A.size() - 1); k++) {
			for (int l = 0; l < (A.size() - 1); l++) {
				A_[k][l] = A[k + 1][l + 1];
			}
		}
		A.clear();
		initializeMatrix(A, A_.size());
		C.matrixPlusMatrix(A_, vw, A);
		vw.clear();
		v.clear();
		w.clear();
		A_.clear();
		i++;
	} while (A.size() > 1);
	L[L.size() - 1][L.size() - 1] = 1;
	U[L.size() - 1][L.size() - 1] = A[0][0];
	A.clear();
}
void readtxt(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	std::ifstream file(file_path.c_str());//c_str()：生成一个const char*指针，指向以空字符终止的数组。
	std::string line;
	pcl::PointXYZ point;
	while (getline(file, line)) {
		std::stringstream ss(line);
		ss >> point.x;
		ss >> point.y;
		ss >> point.z;
		cloud->push_back(point);
	}
	file.close();
}
void showAcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr Acloud, pcl::PointCloud<pcl::PointXYZ>::Ptr Bcloud)
{
	//TODO::添加两个点云cloudA,cloudB并显示在窗口中，id为Acloud，颜色为random_green，另一个id为Bcloud,颜色为random_red,点的大小设置为2
	pcl::visualization::PCLVisualizer viewer("look!");
	viewer.setBackgroundColor(1, 1, 1);
	viewer.initCameraParameters();
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> random_green(Acloud, 100, 200, 50);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> random_red(Bcloud, 200, 50, 200);
	viewer.addPointCloud(Acloud, random_green, "Acloud");
	viewer.addPointCloud(Bcloud, random_red, "Bcloud");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "Acloud");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "Bcloud");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
	}
	return;
}
#define R 0.008//resolution
void Dissimilarity(int q, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<int> neighbor, pcl::PointCloud<pcl::Normal>::Ptr normal, std::vector<double>& D) {
	vectorMatrixCal cal;
	/*pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr c(new pcl::PointCloud<pcl::PointXYZ>);
	c->push_back(cloud->points[q]);
	for (int a = 0; a < neighbor.size(); a++) {
		neighbors->push_back(cloud->points[neighbor[a]]);
	}
	showAcloud(c, neighbors);*/
	D.clear();
	std::vector<double> n_pi = { normal->points[q].normal_x,normal->points[q].normal_y,normal->points[q].normal_z };
	for (int w = 0; w < neighbor.size(); w++) {
		std::vector<double> n_pj = { normal->points[neighbor[w]].normal_x,normal->points[neighbor[w]].normal_y,normal->points[neighbor[w]].normal_z };// ,neighbor[i].y,neighbor[i].z };
		double n_ij = cal.vectorT_vector(n_pi, n_pj);
		double dij = sqrt(abs((cloud->points[q].x - cloud->points[neighbor[w]].x) * (cloud->points[q].x - cloud->points[neighbor[w]].x)
			+ (cloud->points[q].y - cloud->points[neighbor[w]].y) * (cloud->points[q].y - cloud->points[neighbor[w]].y)
			+ (cloud->points[q].z - cloud->points[neighbor[w]].z) * (cloud->points[q].z - cloud->points[neighbor[w]].z)));
		double Di = 1 - n_ij + 0.4 * dij / R;
		D.push_back(Di);
	}

}
double Dissimilarity(pcl::PointXYZ p1, pcl::PointXYZ p2, pcl::Normal normal1, pcl::Normal normal2) {
	vectorMatrixCal cal;
	std::vector<double> n_pi = { normal1.normal_x,normal1.normal_y,normal1.normal_z };
	std::vector<double> n_pj = { normal2.normal_x,normal2.normal_y,normal2.normal_z };// ,neighbor[i].y,neighbor[i].z };
	double n_ij = cal.vectorT_vector(n_pi, n_pj);
	double dij = sqrt(abs((p1.x - p2.x) * (p1.x - p2.x)
		+ (p1.y - p2.y) * (p1.y - p2.y)
		+ (p1.z - p2.z) * (p1.z - p2.z)));
	return(1 - n_ij + 0.4 * dij / R);

}
pcl::visualization::PCLVisualizer::Ptr rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
	// --------------------------------------------
	// -----Open 3D viewer and add point cloud-----
	// --------------------------------------------
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(1, 1, 1);
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "sample cloud");
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
	}
	return (viewer);
}
void giveColor(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr color) {
	pcl::PointXYZRGB point;
	for (int i = 0; i < cloud->size(); i++) {
		point.x = cloud->points[i].x; point.y = cloud->points[i].y; point.z = cloud->points[i].z;
		point.r = rand() % 256;
		point.g = rand() % 256;
		point.b = rand() % 256;
		color->push_back(point);
	}
}
//void fusionColor(pcl::PointXYZRGB point, pcl::PointCloud<pcl::PointXYZRGB>::Ptr color) {
//}
void minimizationFusion(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int K, supervoxel& sv) {
	
	
	//module1 - get normals
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst; //定义一个法线估计的对象
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); //保存法线估计的结果
	normEst.setInputCloud(cloud);
	normEst.setKSearch(10);//表示计算点云法向量时，搜索的点云个数//normEst.setRadiusSearch(reforn); 设置法线估计的半径
	normEst.compute(*normals); //将法线估计结果保存至normals
	
							   
	//module2 - get neighbors and dissimilarity metric D
	std::vector<int> pointIndex;//cloud中近邻4点的索引
	std::vector<float> pointSquareDis;//cloud中近邻4点de对应点距离
	std::vector<std::vector<int>> neighbor_index; neighbor_index.clear();
	double ppooww = pow(cloud->size(), 0.3333333);//pow里面的数都是取整，1/3直接=0
	double lloogg = log2(ppooww);
	int resolution = ceil(lloogg);
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	neighbor_index.clear();
	neighbor_index.resize(cloud->size());
	std::vector<double> D_median; D_median.clear();
	for (int i = 0; i < cloud->size(); i++) {
		octree.nearestKSearch(cloud->points[i], 20, pointIndex, pointSquareDis);
		for (int j = 2; j < pointIndex.size(); j++) {
			neighbor_index[i].push_back(pointIndex[j]);//neighbor[i].push_back(cloud->points[pointIndex[j]]);
		}
		std::vector<double> D;
		Dissimilarity(i, cloud, neighbor_index[i], normals, D);
		double D_min = 10000;
		for (int j = 0; j < D.size(); j++) {
			if (D_min > D[j]) {
				D_min = D[j];
			}
		}
		if (D_median.size() < 1) {
			D_median.push_back(D_min);
		}
		else if (D_median.size() == 1) {
			if (D_median[0] <= D_min) {
				D_median.push_back(D_min);
			}
			else {
				D_median.emplace(D_median.begin(), D_min);
			}
		}
		else{
			if (D_median[0] > D_min) {
				D_median.emplace(D_median.begin(), D_min);
			}
			else if (D_median.back() < D_min) {
				D_median.push_back(D_min);
			}
			else{
				for (int k = 0; k < D_median.size(); k++) {
					if ((D_median[k] <= D_min) & (D_min < D_median[k+1])) {
						D_median.emplace(D_median.begin() + k + 1, D_min);
						break;
					}
				}
			}
		}
	}

	//module3 - get lambda0
	double lambda0 = 0;
	if (D_median.size() % 2 != 0) {
		lambda0 = D_median[(D_median.size()-1) / 2];
	}
	else {
		int k = D_median.size() / 2;
		lambda0 = (D_median[k-1]+D_median[k])*0.5000;
	}


	//module4 - define initial variances
	double lambda = lambda0;
	/*pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_index(new pcl::PointCloud<pcl::PointXYZI>);
	cloud_index->clear();
	for (int i = 0; i < cloud->size(); i++) {
		pcl::PointXYZI p_n;
		p_n.x = cloud->points[i].x;
		p_n.y = cloud->points[i].y;
		p_n.z = cloud->points[i].z;//存储坐标值
		p_n.intensity = i;//存储点索引，用于索引邻域
		cloud_index->push_back(p_n);
	}*/
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr Represent(new pcl::PointCloud<pcl::PointXYZRGB>);

	giveColor(cloud, Represent);
	//rgbVis(Represent);

	int ci = 1;
	double Ri = 0;
	sv.nonrepresent.clear();
	sv.nonrepresent.resize(cloud->size());

	//module5 - Repeat
	std::vector<int> neighbor;
	do {
		for (int i = 0; i < cloud->size(); i++) {//i---a point on cloud index
			if (cloud->points[i].x != NULL) {//非NULL---使该点是上一步骤中represent点
				pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree2(resolution);
				octree2.setInputCloud(cloud);
				octree2.addPointsFromInputCloud();
				octree.nearestKSearch(cloud->points[i], 10, pointIndex, pointSquareDis);
				neighbor.clear();
				for (int n = 1; n < pointIndex.size(); n++) {//当前represent点的邻域点
					neighbor.push_back(pointIndex[n]);
				}
				for (int j = 0; j < neighbor.size(); j++) {//j---index of cloud->points[i]'s neighbor
					int n_ij = neighbor[j];
					if (cloud->points[n_ij].x != NULL) {//如果邻域点是有意义的点
						double DDD = Dissimilarity(cloud->points[i], cloud->points[n_ij],
							normals->points[i], normals->points[n_ij]);
						if ((lambda - ci * DDD) > 0) {//判断两个点之间的距离，是否满足融合条件
							cout << lambda << "-" << ci << "*" << DDD << "=" << lambda - ci * DDD << endl;
							//给Represent颜色点云融合，可以用于显示与分类
							//邻域合并，注意去重
							Represent->points[n_ij].r = Represent->points[i].r;//这里i索引点是representative point，所以把j索引点改成和i一样的颜色，并在cloud中把j点置NULL
							Represent->points[n_ij].g = Represent->points[i].g;
							Represent->points[n_ij].b = Represent->points[i].b;
							cloud->points[n_ij].x = NULL;
							cloud->points[n_ij].y = NULL;
							cloud->points[n_ij].z = NULL;

							for (int sun = 0; sun < sv.nonrepresent[n_ij].size(); sun++) {

								for (int jing = 0; jing < sv.nonrepresent[i].size(); jing++) {
									if (sv.nonrepresent[i][jing] == sv.nonrepresent[n_ij][sun]) {
										break;
									}
								}
								sv.nonrepresent[i].push_back(sv.nonrepresent[n_ij][sun]);
								Represent->points[sv.nonrepresent[n_ij][sun]].r = Represent->points[i].r;
								Represent->points[sv.nonrepresent[n_ij][sun]].g = Represent->points[i].g;
								Represent->points[sv.nonrepresent[n_ij][sun]].b = Represent->points[i].b;

							}

							sv.nonrepresent[n_ij].clear();
							sv.nonrepresent[i].push_back(n_ij);
							cout << "sv.nonrepresent[i].push_back(cloud->points[" << n_ij << "]" << endl;
							//

						}
					}
					/*for (int m = 0; m < neighbor_index[n_ij].size(); m++) {
						if (cloud_index->points[neighbor_index[n_ij][m]].x == NULL) {
							neighbor_index[n_ij].erase(neighbor_index[n_ij].begin() + m);
						}
					}*/
				}
			}
		}
		lambda = 2 * lambda;
		ci++; Ri = 0;
		for (int m = 0; m < cloud->size(); m++) {
			if (cloud->points[m].x != NULL) {
				Ri++;
				//cloud_index->erase(cloud_index->begin()+m);
			}
		}
		cout << "Ri = " << Ri << endl;
		int number = 0;
		int number2 = 0;
		for (int i = 0; i < sv.nonrepresent.size(); i++) {
			number = number + sv.nonrepresent[i].size();
			if (sv.nonrepresent[i].size() != 0) {
				number2++;
			}
		}
		cout << number << endl;
		cout << number2 << endl;
		//rgbVis(Represent);
		//showAcloud(cloud, cloud);
		rgbVis(Represent);
	} while (Ri> K);
}
void main() {
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud18000(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud600(new pcl::PointCloud<pcl::PointXYZ>);
	readtxt("blade0.txt", cloud18000);
	for (int i = 1; i < 7200; i++) {
		cloud600->push_back(cloud18000->points[i]);
	}
	supervoxel supervoxel;
	//minimizationFusion(cloud600, 400, supervoxel);
	minimizationFusion(cloud600, 210, supervoxel);

}