3点对求旋转矩阵和平移矩阵代码(SVD)

侯增涛

已于 2025-03-20 15:59:13 修改

阅读量62

点赞数

分类专栏： Realsense 文章标签： svd 代码 3点icp实例

于 2019-12-06 16:24:26 首次发布

本文链接：https://blog.youkuaiyun.com/weixin_44345862/article/details/103424038

版权

Realsense 专栏收录该内容

10 篇文章

订阅专栏

通过VC+Eigen用svd方法实现:
用的realsense D415相机，自己做了一个三个球的标志杆，扫描了两幅截图(自己写的代码实现的)，然后需要解算出旋转矩阵和平移矩阵，借鉴了视觉slam十四讲从理论到实践第7节的内容。用SVD分解奇异矩阵。一定注意不用SVD，直接用公式推导求旋转矩阵是求不出来的，即使代了具体的值，得出来的也肯定不对（我测试过了），原因是奇异矩阵的关系。

验证直接用公式不行的见下面的matlab代码:

clear all;
close all;
syms x1 y1 z1 x2 y2 z2 x3 y3 z3;
syms xo1 yo1 zo1 xo2 yo2 zo2 xo3 yo3 zo3;
x1=43;y1=7;z1=404;
x2=-38;y2=6;z2=384;
x3=-52;y3=2;z3=382;
xo1=78;yo1=6;zo1=416;
xo2=6;yo2=7;zo2=372;
xo3=-7;yo3=2;zo3=367;
%M为刚开始的相机坐标系（此状态定位为我们的世界坐）标下的标记点坐标
M=[x2-x1,x3-x1,x3-x2;
y2-y1,y3-y1,y3-y2;
z2-z1,z3-z1,z3-z2];
%Mo为相机旋转后的得到的相机坐标系下的标记点坐标
Mo=[xo2-xo1,xo3-xo1,xo3-xo2;
yo2-yo1,yo3-yo1,yo3-yo2;
zo2-zo1,zo3-zo1,zo3-zo2];
%M=RMo是原公式
Mn=inv(Mo);
R=MMn
%%R单位化
% [m,n]=size®;
% for i=1:n
% A(1,i)=norm(R(:,i));
% end
% A=repmat(A,m,1);
% Y=R./A%Y为单位矩阵
%%
% Rn=inv®;%Rn是R的逆矩阵
% %%RN单位化
% [m,n]=size(Rn);
% for i=1:n
% A(1,i)=norm(Rn(:,i));
% end
% A=repmat(A,m,1);
% Yn=Rn./A%Y为单位矩阵
%inv®
%factor(Rn)
%collect(Rn)

通过叉乘法验证直接用公式不行的matlab代码：

clear all;
close all;
syms x1 y1 z1 x2 y2 z2 x3 y3 z3;
syms xo1 yo1 zo1 xo2 yo2 zo2 xo3 yo3 zo3;

% x1=43;y1=7;z1=404;
% x2=-38;y2=6;z2=384;
% x3=-52;y3=2;z3=382;
% xo1=78;yo1=6;zo1=416;
% xo2=6;yo2=7;zo2=372;
% xo3=-7;yo3=2;zo3=367;

a1=[x2-x1,y2-y1,z2-z1];
a2=[x3-x1,y3-y1,z3-z1];
a3=cross(a1,a2);
b1=[xo2-xo1,yo2-yo1,zo2-zo1];
b2=[xo3-xo1,yo3-yo1,zo3-zo1];
b3=cross(b1,b2);
A=[a1’ a2’ a3’];
B=[b1’ b2’ b3’];
%B=RA, Rn=ABn
% An=inv(A);
% R=BAn;
% inv®
Bn=inv(B);
Rn=ABn

别人的教材和我的实践经验证明对于3点对的ICP，只能通过SVD分解来求旋转矩阵和平移矩阵，新建控制台空应用程序，下载Eigen放到该工程下，在项目设置的C++中添加Eigen文件夹路径，如下图，当然opencv的也要加进去，我的以前一直都有，所以不用管。
在这里插入图片描述
运行界面如下：

成功的vc代码如下：

#include
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include “Eigen/Core”
#include “Eigen/Geometry”
#include “Eigen/SVD”

using namespace std;
using namespace cv;
void pose_estimation_3d3d(
const vector& pts1,
const vector& pts2,
Mat& R, Mat& t
);

int main(int argc, char** argv)
{
vector pts1, pts2;

float x1 = 43, y1 = 7, z1 = 404;
float x2 = -38, y2 = 6, z2 = 384;
float x3 = -52, y3 = 2, z3 = 382;
float xo1 = 78, yo1 = 6, zo1 = 416;
float xo2 = 6, yo2 = 7, zo2 = 372;
float xo3 = -7, yo3 = 2, zo3 = 367;
pts1.push_back(Point3f(x1, y1, z1));
pts2.push_back(Point3f(xo1, yo1, zo1));
pts1.push_back(Point3f(x2, y2, z2));
pts2.push_back(Point3f(xo2, yo2, zo2));
pts1.push_back(Point3f(x3, y3, z3));
pts2.push_back(Point3f(xo3, yo3, zo3));
cout << "3d-3d pairs: " << pts1.size() << endl;
Mat R, t;
pose_estimation_3d3d(pts1, pts2, R, t);
cout << "ICP via SVD results: " << endl;
cout << "R = " << R << endl;
cout << "t = " << t << endl;
cout << "R_inv = " << R.t() << endl;
cout << "t_inv = " << -R.t() *t << endl;

cout << "calling bundle adjustment" << endl;


// verify p1 = R*p2 + t
for (int i = 0; i<3; i++)
{
	cout << "p1 = " << pts1[i] << endl;
	cout << "p2 = " << pts2[i] << endl;
	cout << "(R*p2+t) = " <<
		R * (Mat_<double>(3, 1) << pts2[i].x, pts2[i].y, pts2[i].z) + t
		<< endl;
	cout << endl;
}
while(1)//没有while不行，控制台运行一次立马退出了，来不及看到输出的值
{
	char c = getchar();//输入q 时 退出
	if(c='q')
	return 1;
}

}

void pose_estimation_3d3d(
const vector& pts1,
const vector& pts2,
Mat& R, Mat& t
)
{
Point3f p1, p2; // center of mass
int N = pts1.size();
for (int i = 0; i<N; i++)
{
p1 += pts1[i];
p2 += pts2[i];
}
p1 = Point3f(Vec3f(p1) / N);
p2 = Point3f(Vec3f(p2) / N);
vector q1(N), q2(N); // remove the center
for (int i = 0; i<N; i++)
{
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}

// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i<N; i++)
{
	W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
}
cout << "W=" << W << endl;

// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();

if (U.determinant() * V.determinant() < 0)
{
	for (int x = 0; x < 3; ++x)
	{
		U(x, 2) *= -1;
	}
}

cout << "U=" << U << endl;
cout << "V=" << V << endl;

Eigen::Matrix3d R_ = U* (V.transpose());
Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

// convert to cv::Mat
R = (Mat_<double>(3, 3) <<
	R_(0, 0), R_(0, 1), R_(0, 2),
	R_(1, 0), R_(1, 1), R_(1, 2),
	R_(2, 0), R_(2, 1), R_(2, 2)
	);
t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));

}
结果如下：
在这里插入图片描述
3d-3d pairs: 3
W=4696.33 123 2763
183 13 103
1112 26 656
U= 0.972376 0.0456656 -0.228909
0.0375012 0.937374 0.346299
0.230388 -0.345317 0.909768
V= 0.861662 0.204539 -0.464438
0.022494 0.898887 0.437604
0.506984 -0.387513 0.769935
ICP via SVD results:
R = [0.9535138403822582, -0.03725076845750107, 0.2990380184003431;
0.06320891770961798, 0.9949784347989045, -0.07760507075636958;
-0.2946455309714655, 0.09289937854715749, 0.9510802892206818]
t = [-155.0835715233195;
28.28069788618139;
30.93216019823876]
R_inv = [0.9535138403822582, 0.06320891770961798, -0.2946455309714655;
-0.03725076845750107, 0.9949784347989045, 0.09289937854715749;
0.2990380184003431, -0.07760507075636958, 0.9510802892206818]
t_inv = [155.2007623236433;
-36.78924519172842;
19.15164160771658]
calling bundle adjustment
p1 = [43, 7, 404]
p2 = [78, 6, 416]
(R*p2+t) = [43.46681907029429;
6.897154641675272;
404.156605369551]

p1 = [-38, 6, 384]
p2 = [6, 7, 372]
(R*p2+t) = [-38.38110101530089;
6.755714114661941;
383.6164502523337]

p1 = [-52, 2, 382]
p2 = [-7, 2, 367]
(R*p2+t) = [-52.08571718998444;
1.347131364224236;
382.2269438161235]
另外Eigen文件夹内容如下图：
在这里插入图片描述
百度云盘下载直接解压即可用（无提取码）：https://pan.baidu.com/s/15iSTw7HSdZfHXRUUT-skoA

实际应用中需要用到矩阵和数组的转化，上面的例子没有转化，以下是我实际应用的含转化的例子代码：
vector pts1, pts2;
Mat R = (Mat_(3, 3) << 0, 0, 0, 0, 0, 0, 0, 0, 0), t = (Mat_(3, 1) << 0, 0, 0);
…
if (counter >1 && counterN>1 && counterQ>1)//当有第二帧时再执行
{
if (posecomputer_enable_flag)//main函数初始化中为true
{
pts1.push_back(Point3f(x1, y1, z1));
pts2.push_back(Point3f(xo1, yo1, zo1));
pts1.push_back(Point3f(x2, y2, z2));
pts2.push_back(Point3f(xo2, yo2, zo2));
pts1.push_back(Point3f(x3, y3, z3));
pts2.push_back(Point3f(xo3, yo3, zo3));
pose_estimation_3d3d(pts1, pts2, R, t);//这句的计算量很大，只用一次就够所以引入了posecomputer_enable_flag
posecomputer_enable_flag = false;//往后不再计算,否则很慢
}
}
…
xout = (1000 * x - xo1)*R.at(0, 0) + (1000 * y - yo1)*R.at(0, 1) + (1000 * z - zo1)*R.at(0, 2) + x1;
yout = (1000 * x - xo1)*R.at(1, 0) + (1000 * y - yo1)*R.at(1, 1) + (1000 * z - zo1)*R.at(1, 2) + y1;
zout = (1000 * x - xo1)*R.at(2, 0) + (1000 * y - yo1)*R.at(2, 1) + (1000 * z - zo1)*R.at(2, 2) + z1;