Eigen计算三维向量的夹角以及空间变换矩阵

写在前面

1、本文内容
使用Eigen计算两个向量之间的刚体变换；
当两个向量是点云平面法向量时，也就知道了这两个平面点云之间的刚体变换

2、平台
windows, linux
3、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/130582783

原理

假设有两个空间向量a,b，认为b可由a通过空间变换得到，其旋转轴axis，垂直于a,b，旋转角度可同过向量夹角计算方式得到：
$$\cos \theta =\frac{\vec{a}\cdot \vec{b}}{|\vec{a}||\vec{b}|}$$
$$\theta =\arccos \frac{\vec{a}\cdot \vec{b}}{|\vec{a}||\vec{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.normalized()).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

完

主要做激光/影像三维重建，配准、分割等常用点云算法，技术交流、咨询可私信