菜鳥専門:Eigen--Sophus--CV--3種類の異なるライブラリ対マトリクスの操作方法

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 
	#include 
	#include 

2. OpenCV

	// opencv -- Mat
	#include 
	
	 * 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 
	#include 

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<
	// 	std::cout<

	/**********************************************************************
	 *  pose.txt      (    )+     ，      vector
	 * 
	 **********************************************************************/	
		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 
"<<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();
	}