SVD计算点集配准（C++、matlab代码）

一、原理

二、matlab实现

clear all;
close all;
clc;
%%
%生成原始点集
X=[];Y=[];Z=[];
for i=-180:2:180
    for j=-90:2:90
        x = i * pi / 180.0;
        y = j * pi / 180.0;   
        X =[X,cos(y) * cos(x)];
        Y =[Y,sin(y) * cos(x)];
        Z =[Z,sin(x)]; 
    end
end
P=[X(1:3000)' Y(1:3000)' Z(1:3000)'];
%%
%生成变换后点集
i=0.5;j=0.3;k=0.7;
Rx=[1 0 0;0 cos(i) -sin(i); 0 sin(i) cos(i)];
Ry=[cos(j) 0 sin(j);0 1 0;-sin(j) 0 cos(j)];
Rz=[cos(k) -sin(k) 0;sin(k) cos(k) 0;0 0 1];
R=Rx*Ry*Rz;
X=P*R + [0.2,0.3,0.4];
%%
%输出点云便于C++测试
fid = fopen('data1.txt', 'wt');
mat = P;
for i = 1:size(mat, 1)
    fprintf(fid, '%f\t', mat(i,:));
    fprintf(fid, '\n');
end
fclose(fid);
fid = fopen('data2.txt', 'wt');
mat = X;
for i = 1:size(mat, 1)
    fprintf(fid, '%f\t', mat(i,:));
    fprintf(fid, '\n');
end
fclose(fid);

%%
%画出点云
plot3(P(:,1),P(:,2),P(:,3),'b.');
hold on;
plot3(X(:,1),X(:,2),X(:,3),'r.');
%%
%计算点集均值
up = mean(P);
ux = mean(X);
P1=P-up;
X1=X-ux;
%计算点集协方差
sigma=P1'*X1/(length(X1));

[u s v] = svd(sigma);
RR=u*v';

%计算平移向量
qr=ux-up*RR;
%%
%验证旋转矩阵与平移向量正确性
Pre = P*RR+qr;
%画图
figure;
plot3(P(:,1),P(:,2),P(:,3),'b.');
hold on;
plot3(X(:,1),X(:,2),X(:,3),'r.');
plot3(Pre(:,1),Pre(:,2),Pre(:,3),'go');

三、C++代码

#include <Eigen/Dense>
#include <iostream>
#include <string.h>
#include <fstream>
#include <vector>

struct Point
{
	double x, y, z;
};

//功能：读取文件txt、asc->MatrixXd
void CreateCloudMatrix(const std::string& file_path, Eigen::MatrixXd &inPutPointCloud)
{
	std::ifstream file(file_path.c_str());//c_str()：生成一个const char*指针，指向以空字符终止的数组。
	std::string line;
	Point point;
	std::vector<Point> cloud;
	while (getline(file, line))           //用到x，y,z
	{
		std::stringstream ss(line);
		ss >> point.x;
		ss >> point.y;
		ss >> point.z;
		cloud.push_back(point);
	}
	Eigen::MatrixXd pointCloud = Eigen::MatrixXd::Zero(cloud.size(), 3);
	for (int i = 0; i < cloud.size(); i++)
	{
		pointCloud(i, 0) = cloud[i].x;
		pointCloud(i, 1) = cloud[i].y;
		pointCloud(i, 2) = cloud[i].z;
	}
	inPutPointCloud = pointCloud;
	file.close();

}

//功能：SVD计算点集配准
//介绍：输入两个对应点云，输出旋转矩阵、平移矩阵。
void PointCloudRegistrationSVD(Eigen::MatrixXd inputPointCloud1, Eigen::MatrixXd inputPointCloud2, Eigen::Matrix3d &rotationMat, Eigen::Vector3d &translationMat)
{
	Eigen::RowVector3d meanVector1 = inputPointCloud1.colwise().mean();  //每列求均值
	Eigen::RowVector3d meanVector2 = inputPointCloud2.colwise().mean();
	inputPointCloud1.rowwise() -= meanVector1;                        //去质心坐标
	inputPointCloud2.rowwise() -= meanVector2;

	Eigen::MatrixXd covMat;
	if (inputPointCloud1.rows() == 1)
		covMat = (inputPointCloud1.transpose()*inputPointCloud2) / double(inputPointCloud1.rows());
	else
		covMat = (inputPointCloud1.transpose()*inputPointCloud2) / double(inputPointCloud1.rows() - 1);

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(covMat, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d S = U.inverse() * covMat * V.transpose().inverse();

	rotationMat = U * V.transpose();
	translationMat = meanVector2 - meanVector1 * rotationMat;

}

void test()
{
	Eigen::MatrixXd inPutPointCloud1;
	Eigen::MatrixXd inPutPointCloud2;
	Eigen::Matrix3d rotationMat;
	Eigen::Vector3d translationMat;
	CreateCloudMatrix("data1.txt", inPutPointCloud1);
	CreateCloudMatrix("data2.txt", inPutPointCloud2);
	PointCloudRegistrationSVD(inPutPointCloud1, inPutPointCloud2, rotationMat, translationMat);

	std::cout << rotationMat(0,0) << " , " << rotationMat(0,1) << " , " << rotationMat(0,2) << std::endl;
	std::cout << rotationMat(1,0) << " , " << rotationMat(1,1) << " , " << rotationMat(1,2) << std::endl;
	std::cout << rotationMat(2,0) << " , " << rotationMat(2,1) << " , " << rotationMat(2,2) << std::endl;
}

int main()
{
	test();
}