通过两组匹配好的3D点,计算相对位姿变换
采用的方法为ICP,求解方式分为两种:SVD或者非线性优化方法
本文采用SVD的方法求解
构建最小二乘的代价函数,求得使误差平方和达到最小的Rt
首先定义两组点的质心
代入代价函数
上面三项中最后一项求和为零,因此代价函数变为
式中第一项只与R有关,因此可以先求得一个R使得第一项最小然后再求t
对第一项展开得到
第一项和第二项都与R无关,因此最后优化目标函数变为:
最后通过SVD方法求得使得上述代价函数最小的R,先定义矩阵
对其进行SVD分解
当W满秩时,R为:
得到R后即可求得t
/*给两组已经匹配好的3D点,来计算相对位姿变换,写代码*/
#include<AccCtrl.h>
#include <iostream>
#include<Eigen/Geometry>
#include <vector>
#include <math.h>
#include "Eigen/Geometry"
using namespace std;
#define PI 3.1415926
int main()
{
/*生成旋转矩阵*/
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
Eigen::AngleAxisd rotationVector(PI / 4, Eigen::Vector3d(0, 0, 1));
R = rotationVector.toRotationMatrix();
cout << R << endl;
//生成数据点
Eigen::Vector3d p1 = Eigen::Vector3d(1, 2, 3);
Eigen::Vector3d p2 = Eigen::Vector3d(6, 5, 4);
Eigen::Vector3d p3 = Eigen::Vector3d(8, 7, 9);
vector<Eigen::Vector3d> pA = { p1, p2, p3 };
//生成旋转后的点
vector<Eigen::Vector3d> pB;
for (auto p:pA)
{
pB.emplace_back(R*p);
}
/*求两个点云的中心*/
Eigen::Vector3d qA = Eigen::Vector3d(0, 0, 0), qB = Eigen::Vector3d(0, 0, 0);
for (int i = 0; i < pA.size(); i++)
{
for (int j = 0; j < 3; j++)
{
qA[j] += pA[i][j];
qB[j] += pB[i][j];
}
}
qA = qA / pA.size();
qB = qB / pB.size();
/*求去中心的点云*/
for (int i = 0; i < pA.size(); i++)
{
pA[i] = pA[i] - qA;
pB[i] = pB[i] - qB;
}
/*求矩阵W*/
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i < pA.size(); i++)
{
W += pA[i] * pB[i].transpose();
}
/*SVD分解*/
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
Eigen::Matrix3d Rr = U * V.transpose();
cout << Rr << endl;
}