标签：cos 配准 Eigen SVD C++ fid sin inputPointCloud1 MatrixXd
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();
}
标签：cos,配准,Eigen,SVD,C++,fid,sin,inputPointCloud1,MatrixXd 来源： https://blog.csdn.net/qq_41188371/article/details/113258438
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。