C++ 根据一个（法）向量旋转前后的夹角，求旋转矩阵（3D）

关于如何求解两个向量间旋转矩阵的理论很多，

例如：https://blog.csdn.net/zhazhiqiang/article/details/52441170

           https://www.cnblogs.com/meteoric_cry/p/7987548.html等

这里不再赘述，直接上代码。

#include <iostream>
#include <math.h>
#include <algorithm>


#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件


#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

 int main(int argc, char** argv)
{
    Vector3f vecbefore;
	vecbefore << 0, -1, 0;
	Vector3f vecafter;
	vecafter << 0.707, -0.707, 0;

	//【1】求两个向量间的旋转角angle(点积)
	double tem = vecbefore.dot(vecafter);//分子
	double tep = sqrt( vecbefore.dot(vecbefore) * vecafter.dot(vecafter) );//分母
	double angle = acos(tem / tep);
	if (isnan(angle))//acos取值范围[-1,1],若超出范围则越界，输出-1.#IND00
	{
		angle = acos(tep / tem);
	}
	std::cout << "角度： " << angle << std::endl;

	//【2】求旋转轴（叉积）
	Vector3f axis1 = vecbefore.cross(vecafter);
	std::cout << "求旋转轴： " << axis1 << std::endl;
	Vector3f axis2 = vecafter.cross(vecbefore);
	std::cout << "求旋转轴： " << axis2 << std::endl;
	std::cout << "求旋转轴(归一化)： " << axis2.normalized() << std::endl;
	//【3】求旋转矩阵
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	// Define a translation of 2.5 meters on the x axis.
	transform_2.translation() << 0, 0, 0;
	// The same rotation matrix as before; theta radians arround Z axis
	transform_2.rotate(Eigen::AngleAxisf(angle, axis2.normalized()));
	// Print the transformation
	std::cout << transform_2.matrix() << std::endl;
}