ORB-SLAM坐标系到ROS坐标系的转换

Step0：整体流程

ros::Publisher CamPose_Pub;
geometry_msgs::PoseStamped Cam_Pose;

cv::Mat Camera_Pose;
tf::Transform slam_tf;

CamPose_Pub = nh.advertise<geometry_msgs::PoseStamped>("/Camera_Pose",1);

Camera_Pose = SLAM.TrackRGBD(imRGB,imD,tframe);

Pub_CamPose(Camera_Pose); 

void Pub_CamPose(cv::Mat &pose)
{
    cv::Mat Rwc(3,3,CV_32F);
	cv::Mat twc(3,1,CV_32F);
	Eigen::Matrix<double,3,3> rotationMat;

	if(pose.dims < 2 || pose.rows < 3)
	{
        Rwc = Rwc;
		twc = twc;
	}
	else
	{
		Rwc = pose.rowRange(0,3).colRange(0,3).t();//pose is Tcw, so Rwc need .t()
		twc = -Rwc*pose.rowRange(0,3).col(3);
		
		rotationMat << Rwc.at<float>(0,0), Rwc.at<float>(0,1), Rwc.at<float>(0,2),
					   Rwc.at<float>(1,0), Rwc.at<float>(1,1), Rwc.at<float>(1,2),
					   Rwc.at<float>(2,0), Rwc.at<float>(2,1), Rwc.at<float>(2,2);
		Eigen::Quaterniond Q(rotationMat);

		// slam's trans. x is twc.at<float>(0), y is twc.at<float>(1), z is twc.at<float>(2)
		// ros's x <-- slam's Z; ros's y <-- slam's -x; ros's z <-- slam's -y
        // note:SLAM和ROS中的坐标系方向是不一致的，因此需要进行坐标转换
		lam_tf.setOrigin(tf::Vector3(twc.at<float>(2), -twc.at<float>(0), -twc.at<float>(1)));
		slam_tf.setRotation(tf::Quaternion(Q.z(), -Q.x(), -Q.y(), Q.w()));

		Cam_Pose.header.stamp = ros::Time::now();
		Cam_Pose.header.frame_id = "/map";
		tf::pointTFToMsg(slam_tf.getOrigin(), Cam_Pose.pose.position);
		tf::quaternionTFToMsg(slam_tf.getRotation(), Cam_Pose.pose.orientation);

		CamPose_Pub.publish(Cam_Pose);
}

Step1: 获取SLAM当前帧的位姿（变换矩阵T）

cv::Mat Camera_Pose;

Camera_Pose = SLAM.TrackRGBD(imRGB,imD,tframe);

Step2：将变换矩阵T转化为旋转矩阵R和平移向量t，并将旋转矩阵转化为Eigen库的四元数表示。

cv::Mat Rwc(3,3,CV_32F);

cv::Mat twc(3,1,CV_32F);

Eigen::Matrix<double,3,3> rotationMat;



Rwc = pose.rowRange(0,3).colRange(0,3).t();//pose is Tcw, so Rwc need .t()

twc = -Rwc*pose.rowRange(0,3).col(3);

rotationMat << Rwc.at<float>(0,0), Rwc.at<float>(0,1), Rwc.at<float>(0,2),

Rwc.at<float>(1,0), Rwc.at<float>(1,1), Rwc.at<float>(1,2),

Rwc.at<float>(2,0), Rwc.at<float>(2,1), Rwc.at<float>(2,2);

Eigen::Quaterniond Q(rotationMat);

Step3：利用TF坐标变换将SLAM坐标系下的位移t和四元数Q转换到ROS坐标系下

// slam's trans. x is twc.at<float>(0), y is twc.at<float>(1), z is twc.at<float>(2)
// ros's x <-- slam's Z; ros's y <-- slam's -x; ros's z <-- slam's -y
// note:SLAM和ROS中的坐标系方向是不一致的，因此需要进行坐标转换
tf::Transform slam_tf;

slam_tf.setOrigin(tf::Vector3(twc.at<float>(2), -twc.at<float>(0), -twc.at<float>(1)));
slam_tf.setRotation(tf::Quaternion(Q.z(), -Q.x(), -Q.y(), Q.w()));

Step4：将TF坐标转化为ROS消息格式

geometry_msgs::PoseStamped Cam_Pose;

Cam_Pose.header.stamp = ros::Time::now();
Cam_Pose.header.frame_id = "/map";
tf::pointTFToMsg(slam_tf.getOrigin(), Cam_Pose.pose.position);
tf::quaternionTFToMsg(slam_tf.getRotation(), Cam_Pose.pose.orientation);

Step5：将坐标消息发布至话题

ros::Publisher CamPose_Pub;

CamPose_Pub = nh.advertise<geometry_msgs::PoseStamped>("/Camera_Pose",1);

CamPose_Pub.publish(Cam_Pose);