写在前面
1、本文内容
使用Eigen计算两个向量之间的刚体变换;
当两个向量是点云平面法向量时,也就知道了这两个平面点云之间的刚体变换
2、平台
windows, linux
3、转载请注明出处:
https://blog.csdn.net/qq_41102371/article/details/130582783
原理
假设有两个空间向量a,b,认为b可由a通过空间变换得到,其旋转轴axis,垂直于a,b,旋转角度可同过向量夹角计算方式得到:
cos θ = a ⃗ ⋅ b ⃗ ∣ a ⃗ ∣ ∣ b ⃗ ∣ \cos\theta = \frac{\vec{a}\cdot \vec{b}} {| \vec{a}| |\vec{b}|} cosθ=∣a∣∣b∣a⋅b
θ = arccos a ⃗ ⋅ b ⃗ ∣ a ⃗ ∣ ∣ b ⃗ ∣ \theta = \arccos{\frac{\vec{a}\cdot \vec{b}} {| \vec{a}| |\vec{b}|}} θ=arccos∣a∣∣b∣a⋅b
代码
1、手动计算
Eigen::Matrix3d ComputeMatrixFromTwoVector(const Eigen::Vector3d a, const Eigen::Vector3d b){
std::cout << "\nComputeMatrixFromTwoVector\n"
<< std::endl;
std::cout << "a: " << a.transpose() << std::endl;
std::cout << "b: " << b.transpose() << std::endl;
// 叉乘求旋转轴
auto axis = a.cross(b);
// 向量夹角
auto cos_theta = a.dot(b)/(a.norm() * b.norm());
auto theta = acos(cos_theta);
std::cout << "axis:\n"
<< axis.transpose() << std::endl;
std::cout << "theta: " << theta << std::endl;
// 使用轴角获取旋转矩阵
Eigen::Matrix3d mat_rot = Eigen::AngleAxisd(theta, axis).matrix();
return mat_rot;
}
2、利用Eigen函数
// v1 v2是输入的两个向量
Eigen::Matrix3d mat = Eigen::Quaterniond().FromTwoVectors(v1, v2).matrix();
经测试,两个结果一致
参考
https://blog.csdn.net/newbeixue/article/details/124731785
http://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html#acdb1eb44eb733b24749bc7892badde64
https://my.oschina.net/u/4290001/blog/3457663
完
主要做激光/影像三维重建,配准、分割等常用点云算法,技术交流、咨询可私信