Eigen calculates the angle between three-dimensional vectors and the spatial transformation matrix

write in front

1. Content of this article
Use Eigen to calculate the rigid body transformation between two vectors;
when the two vectors are point cloud plane normal vectors, the rigid body transformation between the two plane point clouds is also known.

2. Platform
windows, linux
3. Please indicate the source for reprinting:
https://blog.csdn.net/qq_41102371/article/details/130582783

principle

Suppose there are two space vectors a and b. It is considered that b can be obtained from a through space transformation. Its rotation axis is perpendicular to a and b. The rotation angle can be obtained by calculating the angle between the vectors: cos ⁡ θ = a ⃗ ⋅
$$\cos i=\frac{\vec{a}\cdot \vec{b}}{|\vec{a}||\vec{b}|}$$
$$i=\arccos \frac{\vec{a}\cdot \vec{b}}{|\vec{a}||\vec{b}|}$$

code

1. Manual calculation

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. Use Eigen function

// v1 v2是输入的两个向量
Eigen::Matrix3d mat = Eigen::Quaterniond().FromTwoVectors(v1, v2).matrix();

After testing, the two results are consistent

reference

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

over

Mainly engaged in laser/image three-dimensional reconstruction, registration, segmentation and other commonly used point cloud algorithms. For technical exchanges and consultations, please send a private message