相机坐标系与四元数和欧拉角的转换

今天，项目中利用aruco来识别二维码来确定相机姿态，我就详细研究了一下相机坐标系。

（一）相机坐标系

（二）如何在ROS中进行四元数和欧拉角转化

1. 将geometry_msgs::Quaternion转化为tf::Quaternion类型

	tf::Quaternion quat;
	tf::quaternionMsgToTF(bucket_mark_poses->orientation,quat);

2. 通过tf::Quaternion,将四元数转化为欧拉角

  double roll, pitch, yaw;
  tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

  mark_rpy_.x = roll;     //绕x轴
  mark_rpy_.y = pitch;    //绕y轴
  mark_rpy_.z = yaw;      //绕z轴
  //发布欧拉角的主题，消息类型为<geometry_msgs::Vector3>
  rpy_pub_.publish(mark_rpy_);  
```javascript
3. 进行tf::Transform变换

tf::Vector3 dock2getPonint = get_point_ - point_dock_;
double angle = dock2getPonint.angle(tf::Vector3(1, 0, 0 ));
tf::Vector3 angle_sign = dock2getPonint.cross(tf::Vector3(1, 0, 0));
angle = angle_sign.z() >= 0 ? -angle : angle;
tf::Quaternion Rotation;
Rotation.setRotation(tf::Vector3(1,0,0),angle);
tf::Transform Dock;
Dock.setOrigin(point_dock_);
Dock.setRotation(Rotation);

tf::Transform dock2robot = pose.inverse() * Dock;

* 参考
[TF库说明](http://mirror.umd.edu/roswiki/doc/diamondback/api/tf/html/c++/namespacetf.html#a146c64492ebe3407729ea6f7246529f6%20tf_api%E8%AF%B4%E6%98%8E)