点云配准算法——JRMPC算法介绍(附c++源码)

最新推荐文章于 2024-05-21 10:41:44 发布

A晨的博客

最新推荐文章于 2024-05-21 10:41:44 发布

阅读量1.3k

点赞数 3

分类专栏：随记专栏文章标签：算法

本文链接：https://blog.csdn.net/qq_37764141/article/details/118634060

版权

随记专栏专栏收录该内容

7 篇文章 0 订阅

订阅专栏

点云配准算法——JRMPC算法介绍

对于给定的两个点集v1,v2，分别对他们进行坐标变换到同一个空间坐标系下，形成一个新的点集Θ。

在这里插入图片描述
V1_tran = $R1\times V1$ + t1
V2_tran = $R2\times V2$ + t2
Θ = { V1_tran ，V2_tran }
最终目的就是求解点集V1和V2正确配准时的变换参数R1,R2,t1,t2。
如下图所示，JRMPC算法随机初始化了一系列点，一般设置为数据大小维度的网格点。以这些点为中心建立K维高斯混合模型GMM，其中K为初始化的点数量。（高斯混合模型的参数可以预先给定初值，后续会进行迭代更新）
在这里插入图片描述

利用高斯模型计算点集中的每个点Θ(i)被划分在第k个高斯模型的值p_ik，
f(Θ)= ∑_ik▒〖p(x)〗
通过EM算法求解max f(Θ)。即可知道第i个点被分配到第k个高斯模型时可以获得最大f(Θ)。（获得了分类结果）
利用以下公式计算点集的变换参数R1,R2,t1,t2：
在这里插入图片描述

同时根据点集的分类结果可以更新高斯混合模型的参数：

利用新的变换参数生成的点集和高斯混合模型利用EM算法重新求解，进行迭代，直到指定的迭代次数为止。

[1]Evangelidis G.D. et al.,. A Generative Model for the Joint Registration of Multiple Point Sets. 2014. Cham: Springer International Publishing.

JRMPC_CU.cuh

#pragma once

#ifndef _JRMPC_CU_CUH_
#define _JRMPC_CU_CUH_
#define EIGEN_USE_MKL_ALL//Using MKL for Eigen

#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <algorithm>
#include <Eigen/Core>

#include "cuda_runtime.h"
#include "device_launch_parameters.h"
#include "..//ReaderWriterIO//point_RW.cpp"

using namespace Eigen;

class JRMPC
{
public:
	void setInputPoint(struct Result *Data_1, struct Result *Data_2);
	void edgeArea_extraction(int Edge, int Flag);
	//void preProcess();预处理后续再加
	MatrixXf jrmpcMethod();

public:
	Result PointData_1;//Storge point Datas
	Result PointData_2;
	MatrixXf Tans;

private:
	static const int M_Nano = 3;//读取点云单个点的信息维度
	static const int K = 300;//聚类中心点数量
	const int maxNumIter = 15;//迭代次数

};

__global__ static void MatrixEularDis(float *P_1, float *C, float *A_1, int N, int K);


#endif

JRMPC_CU.cu

#include "JRMPC_CU.cuh"

using namespace std;
using namespace Eigen;


__global__ static void MatrixEularDis(float *P, float *C, float *A, int N, int K)
{
	int i = threadIdx.x + blockIdx.x * blockDim.x;
	for (int j = 0; j < K; j++) {
		if (i < N) {
			A[j*N + i] = abs(P[i] + C[j] - 2 * A[j*N+i]);
		}
	}

}


void JRMPC::setInputPoint(struct Result *Data_1, struct Result *Data_2)
{
	PointData_1 = *Data_1;
	PointData_2 = *Data_2;
}

void JRMPC::edgeArea_extraction(int Edge, int Flag)
{
	int Num_1 = 0;
	int Num_2 = 0;
	float Max_1 = 0;
	float Min_2 = INT_MAX;
	float(*point_1)[M_Nano] = (float(*)[M_Nano]) PointData_1.pointData;
	float(*point_2)[M_Nano] = (float(*)[M_Nano]) PointData_2.pointData;

	if (Flag == 1) {
		for (int i = 0; i < PointData_1.Num; i++)//find the Max or Min Value
			if (point_1[i][0] > Max_1) Max_1 = point_1[i][0];
		for (int i = 0; i < PointData_2.Num; i++)
			if (point_2[i][0] < Min_2) Min_2 = point_2[i][0];
		for (int i = 0; i < PointData_1.Num; i++) {//Count the edgeArea points nums
			if (point_1[i][0] > Max_1 - Edge) Num_1++;
		}
		for (int i = 0; i < PointData_2.Num; i++) {//Count the edgeArea points nums
			if (point_2[i][0] < Min_2 + Edge) Num_2++;
		}
		int i_1 = 0;
		float *pointEdge_1 = new float[Num_1*M_Nano];
		for (int i = 0; i < PointData_1.Num; i++) {//Get the edgeArea points
			if (point_1[i][0] > Max_1 - Edge) {
				for (int j = 0; j < M_Nano; j++) {
					pointEdge_1[i_1] = point_1[i][j];
					i_1++;
				}
			}
		}
		delete[] PointData_1.pointData;

		int i_2 = 0;
		float *pointEdge_2 = new float[Num_2*M_Nano];
		for (int i = 0; i < PointData_2.Num; i++) {//Get the edgeArea points
			if (point_2[i][0] < Min_2 + Edge) {
				for (int j = 0; j < M_Nano; j++) {
					pointEdge_2[i_2] = point_2[i][j];
					i_2++;
				}
			}
		}
		delete[] PointData_2.pointData;

		PointData_1.Num = Num_1;
		PointData_1.pointData = pointEdge_1;
		PointData_2.Num = Num_2;
		PointData_2.pointData = pointEdge_2;

	}
	else if (Flag == 2) {
		for (int i = 0; i < PointData_1.Num; i++)//find the Max or Min Value
			if (point_1[i][1] > Max_1) Max_1 = point_1[i][1];
		for (int i = 0; i < PointData_2.Num; i++)
			if (point_2[i][1] < Min_2) Min_2 = point_2[i][1];
		for (int i = 0; i < PointData_1.Num; i++) {//Count the edgeArea points nums
			if (point_1[i][1] > Max_1 - Edge) Num_1++;
		}
		for (int i = 0; i < PointData_2.Num; i++) {//Count the edgeArea points nums
			if (point_2[i][1] < Min_2 + Edge) Num_2++;
		}
		int i_1 = 0;
		float *pointEdge_1 = new float[Num_1*M_Nano];
		for (int i = 0; i < PointData_1.Num; i++) {//Get the edgeArea points
			if (point_1[i][1] > Max_1 - Edge) {
				for (int j = 0; j < M_Nano; j++) {
					pointEdge_1[i_1] = point_1[i][j];
					i_1++;
				}
			}
		}
		delete[] PointData_1.pointData;

		int i_2 = 0;
		float *pointEdge_2 = new float[Num_2*M_Nano];
		for (int i = 0; i < PointData_2.Num; i++) {//Get the edgeArea points
			if (point_2[i][1] < Min_2 + Edge) {
				for (int j = 0; j < M_Nano; j++) {
					pointEdge_2[i_2] = point_2[i][j];
					i_2++;
				}
			}
		}
		delete[] PointData_2.pointData;

		PointData_1.Num = Num_1;
		PointData_1.pointData = pointEdge_1;
		PointData_2.Num = Num_2;
		PointData_2.pointData = pointEdge_2;
	}
	else {
		cout << "Set a correct Flag Value";
		exit(0);
	}

};

/*
void JRMPC::preProcess()
{

};*/

MatrixXf JRMPC::jrmpcMethod()
{

	//Setting Paraments for JRMPC
	const int K = this->K;//聚类中心点数量
	const int maxNumIter = this->maxNumIter;//迭代次数
	double epsilon = pow(10.0,-5);
	float gamma = 0.0500;//(1 / K)
	Matrix<float, K, 1> pk;//K个点集权重+噪声点集权重
	MatrixXf moveDistanceXY(1, 2);
	//*******pk = 0*pk.array() + (1 / (K*(gamma + 1)));
	pk = 0 * pk.array() + 0.003200;

	//vector<pair<float, float>> T(2);//位移量
	Matrix<float, 2, 2> T;
	T.fill(0);
	//MatrixXf T = MatrixXf::Zero(2,2);//位移量

	//Create Data Matrix
	Matrix<float, Dynamic, Dynamic, RowMajor> imageRaw_1;
	imageRaw_1.resize(PointData_1.Num, 2);
	Matrix<float, Dynamic, Dynamic, RowMajor> imageRaw_2;
	imageRaw_2.resize(PointData_2.Num, 2);
	Matrix<float, K, 2, RowMajor> K_CenterPoint;
	Matrix<float, K, 1> az;

	float(*point_1)[M_Nano] = (float(*)[M_Nano]) PointData_1.pointData;
	for (int i = 0; i < PointData_1.Num; i++) {
		imageRaw_1(i, 0) = point_1[i][0];
		imageRaw_1(i, 1) = point_1[i][1];
	}

	float(*point_2)[M_Nano] = (float(*)[M_Nano]) PointData_2.pointData;
	for (int i = 0; i < PointData_2.Num; i++) {
		imageRaw_2(i, 0) = point_2[i][0];
		imageRaw_2(i, 1) = point_2[i][1];
	}

	Matrix<float, K, 1> vec;
	for (int cc = 0; cc < 300; cc++) {
		vec(cc) = cc + 1;
	}
	K_CenterPoint.col(0) = vec;
	K_CenterPoint.col(1) = vec;

	Matrix<float, Dynamic, 2> image_1;
	Matrix<float, Dynamic, 2> image_2;
	image_1 = imageRaw_1.rowwise() + T.col(0).transpose();
	image_2 = imageRaw_2.rowwise() + T.col(1).transpose();
	
	Matrix<float, K, 1> Q;
	Q = 0 * Q.array() + 0.50197*pow(10, -6);

	//h
	float h = 2 / Q.mean();
	float beta = 1.195200*pow(10,-8);

	//IterBegin
	for (int i = 0; i < maxNumIter; i++) {
		//两矩阵欧拉距离计算
		static const int Nu_1 = PointData_1.Num;
		Matrix<float, Dynamic, K, RowMajor> a_1,a_2;
		a_1.resize(PointData_1.Num, K);
		a_2.resize(PointData_2.Num, K);

		//计算两向量的欧拉距离
		a_1 = image_1 * K_CenterPoint.transpose();
		a_2 = image_2 * K_CenterPoint.transpose();
		MatrixXf image_1_sqare = image_1.rowwise().squaredNorm();
		MatrixXf image_2_sqare = image_2.rowwise().squaredNorm();
		MatrixXf K_CenterPoint_sqare = K_CenterPoint.rowwise().squaredNorm();
		float *P_1, *C, *A_1;
		cudaMalloc((void**)&P_1, PointData_1.Num*sizeof(float));
		cudaMalloc((void**)&C, K*sizeof(float));
		cudaMalloc((void**)&A_1, PointData_1.Num*K*sizeof(float));
		cudaMemcpy(P_1, image_1_sqare.data(), PointData_1.Num * sizeof(float), cudaMemcpyHostToDevice);
		cudaMemcpy(C, K_CenterPoint_sqare.data(), K * sizeof(float), cudaMemcpyHostToDevice);
		cudaMemcpy(A_1, a_1.data(), PointData_1.Num*K * sizeof(float), cudaMemcpyHostToDevice);
		dim3 Gridsize_1(PointData_1.Num/1024,1);
		dim3 Blocksize_1(1024);
		MatrixEularDis << <Gridsize_1, Blocksize_1 >> > (P_1, C, A_1, PointData_1.Num, K);
		float *P_2, *A_2;
		cudaMalloc((void**)&P_2, PointData_2.Num * sizeof(float));
		cudaMalloc((void**)&A_2, PointData_2.Num*K * sizeof(float));
		cudaMemcpy(P_2, image_2_sqare.data(), PointData_2.Num * sizeof(float), cudaMemcpyHostToDevice);
		cudaMemcpy(A_2, a_2.data(), PointData_2.Num*K * sizeof(float), cudaMemcpyHostToDevice);
		dim3 Gridsize_2(PointData_2.Num / 1024, 1);
		dim3 Blocksize_2(1024);
		MatrixEularDis << <Gridsize_2, Blocksize_2 >> > (P_2, C, A_2, PointData_2.Num, K);
		cudaMemcpy(a_1.data(), A_1, PointData_1.Num*K * sizeof(float), cudaMemcpyDeviceToHost);
		cudaMemcpy(a_2.data(), A_2, PointData_2.Num*K * sizeof(float), cudaMemcpyDeviceToHost);

		//pk*S^-1.5*exp(-.5/S^2*||.||) and normalize
		for (int t = 0; t < PointData_1.Num; t++) {
			a_1.row(t) = (a_1.row(t).array()*Q.transpose().array().pow(2)*(-0.5)).array().exp();
			a_1.row(t) = a_1.row(t).array()*pk.transpose().array()*(Q.transpose().array().pow(1.5));
			a_1.row(t) = a_1.row(t) / (a_1.row(t).sum()+beta);
		}
		for (int t = 0; t < PointData_2.Num; t++) {
			a_2.row(t) = (a_2.row(t).array()*Q.transpose().array().pow(2)*(-0.5)).array().exp();
			a_2.row(t) = a_2.row(t).array()*pk.transpose().array()*(Q.transpose().array().pow(1.5));
			a_2.row(t) = a_2.row(t) / (a_2.row(t).sum() + beta);
		}

		Matrix<float, K, 2>lambda;
		Matrix<float, 2, K>W_1;
		Matrix<float, 2, K>W_2;
		Matrix<float, K, 2>b;
		Matrix<float, 2, 2>mW;
		Matrix<float, 2, 2>mX;
		Matrix<float, 1, 2>sumOfWeight;
		lambda.col(0) = a_1.colwise().sum();
		lambda.col(1) = a_2.colwise().sum();
		W_1.row(0) = (imageRaw_1.transpose()*a_1).row(0).cwiseProduct(Q.transpose());
		W_1.row(1) = (imageRaw_1.transpose()*a_1).row(1).cwiseProduct(Q.transpose());
		W_2.row(0) = (imageRaw_2.transpose()*a_2).row(0).cwiseProduct(Q.transpose());
		W_2.row(1) = (imageRaw_2.transpose()*a_2).row(1).cwiseProduct(Q.transpose());

		b.col(0) = lambda.col(0).cwiseProduct(Q);
		b.col(1) = lambda.col(1).cwiseProduct(Q);
		mW.col(0) = W_1.rowwise().sum();
		mW.col(1) = W_2.rowwise().sum();
		mX.col(0) = K_CenterPoint.transpose()*b.col(0);
		mX.col(1) = K_CenterPoint.transpose()*b.col(1);
		sumOfWeight(0, 0) = lambda.col(0).dot(Q);
		sumOfWeight(0, 1) = lambda.col(1).dot(Q);

		T.col(0) = (mX.col(0) - mW.col(0))/sumOfWeight(0,0);
		T.col(1) = (mX.col(1) - mW.col(1))/sumOfWeight(0,1);

		image_1 = imageRaw_1.rowwise() + T.col(0).transpose();
		image_2 = imageRaw_2.rowwise() + T.col(1).transpose();

		Matrix<float, 1, K>den;
		den = lambda.rowwise().sum().transpose();
		K_CenterPoint = a_1.transpose()*image_1 + a_2.transpose()*image_2;
		K_CenterPoint.col(0) = K_CenterPoint.col(0).cwiseQuotient(den.transpose());
		K_CenterPoint.col(1) = K_CenterPoint.col(1).cwiseQuotient(den.transpose());

		Matrix<float, K, 2>wnormes;
		Matrix<float, Dynamic, K> temp_a_1, temp_a_2;
		temp_a_1.resize(PointData_1.Num, K);
		temp_a_2.resize(PointData_2.Num, K);

		//计算两向量的欧拉距离
		temp_a_1 = image_1 * K_CenterPoint.transpose();
		temp_a_2 = image_2 * K_CenterPoint.transpose();
		image_1_sqare = image_1.rowwise().squaredNorm();
		image_2_sqare = image_2.rowwise().squaredNorm();
		K_CenterPoint_sqare = K_CenterPoint.rowwise().squaredNorm();
		cudaMemcpy(C, K_CenterPoint_sqare.data(), K * sizeof(float), cudaMemcpyHostToDevice);
		cudaMemcpy(P_1, image_1_sqare.data(), PointData_1.Num * sizeof(float), cudaMemcpyHostToDevice);
		cudaMemcpy(A_1, temp_a_1.data(), PointData_1.Num*K * sizeof(float), cudaMemcpyHostToDevice);
		MatrixEularDis << <Gridsize_1, Blocksize_1 >> > (P_1, C, A_1, PointData_1.Num, K);
		cudaMemcpy(P_2, image_2_sqare.data(), PointData_2.Num * sizeof(float), cudaMemcpyHostToDevice);
		cudaMemcpy(A_2, temp_a_2.data(), PointData_2.Num*K * sizeof(float), cudaMemcpyHostToDevice);
		MatrixEularDis << <Gridsize_2, Blocksize_2 >> > (P_2, C, A_2, PointData_2.Num, K);
		cudaMemcpy(temp_a_1.data(), A_1, PointData_1.Num*K * sizeof(float), cudaMemcpyDeviceToHost);
		cudaMemcpy(temp_a_2.data(), A_2, PointData_2.Num*K * sizeof(float), cudaMemcpyDeviceToHost);
		cudaFree(P_1);
		cudaFree(P_2);
		cudaFree(C);
		cudaFree(A_1);
		cudaFree(A_2);

		wnormes.col(0) = (a_1.cwiseProduct(temp_a_1)).colwise().sum();
		wnormes.col(1) = (a_2.cwiseProduct(temp_a_2)).colwise().sum();

		Q = 3*den.transpose().cwiseQuotient(wnormes.rowwise().sum() + 3 * epsilon * den.transpose());

	}

	moveDistanceXY(0, 0) = -(T(0, 1) - T(0, 0));
	moveDistanceXY(0, 1) = -(T(1, 1) - T(1, 0));
	Tans = moveDistanceXY;
	return moveDistanceXY;
};

A晨的博客

关注

3
点赞
踩
8

收藏

觉得还不错? 一键收藏
3
评论
点云配准算法——JRMPC算法介绍(附c++源码)

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档点云配准算法——JRMPC算法介绍前言一、pandas是什么？二、使用步骤1.引入库2.读入数据总结前言一、pandas是什么？二、使用步骤1.引入库2.读入数据总结前言提示：这里可以添加本文要记录的大概内容：例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。提示：以下是本篇文章正文内容，下面案例可供参考一、pandas是什么？示例：pandas 是基于N
复制链接

扫一扫