罗德里格斯（Rodrigues）变换

opencv C++实现

#include 
#include 
#include 

int main()
{
	double r_vec[3] = { -2.100418,-2.167796,0.273330 };
	cv::Mat pr_vec(1, 3, CV_64FC1, r_vec);
	std::cout << "pr_vec = " << pr_vec << std::endl;
	cv::Mat pR_matrix;
	cv::Rodrigues(pr_vec, pR_matrix);
	std::cout << "pR_matrix = " << pR_matrix << std::endl;
	system("pause");
	return 0;
}

罗德里格斯公式（Rodriguez formula）是计算机视觉中的一大经典公式，在描述相机位姿的过程中很常用。公式：

                                                            

在三维空间中，旋转矩阵可以对坐标系（基向量组）进行刚性的旋转变换。

                                                          

通常为了方便计算，基向量组中的向量是相互正交的且都为单位向量，那么就是一个标准正交矩阵。两个重要性质：

• 
• 

假设原坐标系基向量矩阵为，旋转后的坐标系基向量矩阵为。

                                                         

                                                                         

                                                         

其变换过程如图所示：

 

根据线性代数的定义，旋转矩阵就是从基向量矩阵到基向量矩阵的过渡矩阵。由于旋转矩阵是标准3阶正交矩阵，故旋转矩阵的自由度为3，这说明最少可以用三个变量来表示旋转矩阵，这就是罗德里格斯公式（Rodriguez formula）存在的基础。

罗德里斯公式（Rodriguez formula）首先要确定一个三维的单位向量（两个自由度）和一个标量（一个自由度）。