菜鸟专学：Eigen--Sophus--CV--三种不同库对矩阵的操作方式

RobotSlamApplication(4): SLAM中对向量和矩阵的多种不同表达方式

SLAM 中对点、向量以及矩阵存在着多种不同的表达方式，甚至在使用过程中涉及到多种不同表达方式转换（如，OpenCV、Eigen, Sophus等库之间的转换），因此，本文针对常用几种不同矩阵、向量的表达式进行了讨论和相互转换分析。

/*
	 * 旋转向量可通过罗德里格斯公式将 3*1向量--revc 转化为 3*3矩阵--R
	 * 		cv::Rodrigues( result.rvec, R );
	 *	
	 *旋转矩阵:								旋转向量: 第一个参数为旋转角度，第二个参数哪个为1就绕哪轴旋转
	 * 		3*3 Eigen::Matrix3d (double)			3*1 Eigen::AngleAxisd(M_PI, Vector3d(0,0,1))
	 * 	
	 *四元数：								欧拉角：角度顺序（ZYX）, yaw, pitch roll  
	 * 		4*1 Eigen::Quaterniond 					3*1 Vector3d
	 * 	
	 *欧式变换： 表达式是3d, 实际上是4*4矩阵		放射变换	表达式是3d, 实际上是4*4矩阵
	 * 		4*4 Eigen::Isometry3d					4*4 Eigen::Affine3d
*/

1. Eigen3

	// Eigen -- Matrix
	#include <Eigen/Core>
	#include <Eigen/Dense>
	#include <Eigen/Geometry>

2. OpenCV

	// opencv -- Mat
	#include <opencv2/opencv.hpp>
	
	 * OpenCV 中有模板类 Vec,可以表示一个向量
		typedef Vec<uchar, 2> Vec2b;
		typedef Vec<uchar, 3> Vec3b;
		typedef Vec<uchar, 4> Vec4b;
		typedef Vec<short, 2> Vec2s;
		typedef Vec<short, 3> Vec3s;
		typedef Vec<short, 4> Vec4s;
		typedef Vec<int, 2> Vec2i;
		typedef Vec<int, 3> Vec3i;
		typedef Vec<int, 4> Vec4i;
		typedef Vec<float, 2> Vec2f;
		typedef Vec<float, 3> Vec3f;
		typedef Vec<float, 4> Vec4f;
		typedef Vec<float, 6> Vec6f;
		typedef Vec<double, 2> Vec2d;
		typedef Vec<double, 3> Vec3d;
		typedef Vec<double, 4> Vec4d;
		typedef Vec<double, 6> Vec6d;

3. Sophus

	// Sophus -- SO3 SE3d
	#include <sophus/se3.hpp>
	#include <sophus/so3.hpp>

4.代码分别测试

	int main(int argc, char **argv)
	{	
	
		Eigen::Matrix4d R_0;
		Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();  // z-axis rotation
	
		cv::Vec3b recv = {0,0,1};
	
	   //Eigen::Vector3d revc = {0,0,1};
	/****************************************************************
	 * cv::Mat R;     Eigen::Matrix3d
	 * cv::Vec3b;	  Eigen::Vector3d
	 * opencv中将向量转为旋转矩阵R
	   cv::Rodrigues( revc, R );
	******************************************************************/	
		Eigen::Isometry3d T = Eigen::Isometry3d::Identity();  // T.matrix 4*4
		Eigen::Quaterniond q(R); 
	
	// 	std::cout<<"Eigen::Test \n"<<T.matrix()<<std::endl;
	// 	std::cout<<"Eigen::Test \n"<<q.coeffs()<<std::endl;

	/**********************************************************************
	 *  pose.txt 一般以位置(三维向量)+四元数组成， 假设其为 vector<float>
	 * 
	 **********************************************************************/	
		float data[7]= {0};
		Eigen::Vector3d trans = {0,1,2};
	
		Eigen::Quaterniond q_d(data[6], data[3], data[4], data[5]);
	// 	Eigen::Quaterniond q_d(R);
		Eigen::Isometry3d T_d(q_d); // Change into 4*4 Matrix
		T_d.pretranslate(trans);   // get the Final Transformation Matrix
	
		std::cout<<"Eigen::Test \n"<<T_d.matrix()<<std::endl;
	/***********************************************************************
	 * SE(3)  SO(3) == Sophus
	 * SE3 SO3 与 Eigen中的矩阵可相关交叉变换；
	 * 
	 * 		原型					李代数
	 * SE3--(4*4)=="T"  	  --> se3  (6*1)
	 * 
	 * SO3--(3*3)=="R"		  --> so3 (3*3)
	 ***********************************************************************/
		Sophus::SE3d SE3R_4(R,trans);

		Sophus::SO3d SO3R_3(R);
	
		Eigen::Vector3d so3 = SO3R_3.log();
	
		// 李代数se(3) 是一个六维向量，方便起见先typedef一下
	    typedef Eigen::Matrix<double,6,1> Vector6d;
	    Vector6d se3 = SE3R_4.log();
	}