相机坐标下数据获取（2019.8.31）
平移矩阵和欧拉角
- 1.旋转矩阵和欧拉角
- 旋转矩阵
- 欧拉角
- 2旋转矩阵与欧拉角之间的转换
- 欧拉角转旋转矩阵 ：
- 旋转矩阵转欧拉角
- 代码
- 3我自己的坐标信息获取
- 首先，先看转换部分的程序（这一部分是直接复制师兄的）
- 现在，启动图像发布和接收节点和虚拟环境
1.旋转矩阵和欧拉角
旋转矩阵
描述两个坐标系间的相对姿态有多种方式，比如：旋转矩阵、欧拉角、四元数、罗德里格参数等。
常用旋转矩阵和平移矩阵来描述相机坐标与世界坐标之间的联系。
欧拉角
欧拉角具有直观、易于理解的特点，能让我们清楚的看到两个坐标系间是怎么转动过去的，虽然欧拉角会碰到万向锁的问题，但它仍然是我们在描述刚体运动学过程中的重要参数。
学过相关内容的同学应该知道，根据旋转顺序不同，描述两个坐标系间转换的欧拉角共有12组。这里只介绍一种比较常见的转换方式，ZYX顺序转动，也就是航空航天领域常用“偏航-俯仰-滚转”(yaw-pitch-roll)方式：
绕Z轴旋转，得到偏航角ψ ；
绕旋转之后的Y轴旋转，得到俯仰角θ；
绕旋转之后的X轴旋转，得到俯仰角ϕ；
以卫星或无人机为例，这里的“偏航-俯仰-滚转”指的是本体系b相对于参考坐标系i（世界坐标系）的转动，转换为姿态矩阵表示为参考坐标系到本体系的转换矩阵。这一点在仿真过程中极为重要，有时感觉理论上好像懂了，但是如果顺序搞反了在仿真中会差一个转置，得不到正确的结果，而且不太好发现。
经过ZYX顺序三次旋转得到的旋转矩阵为：
2旋转矩阵与欧拉角之间的转换
欧拉角转旋转矩阵 ：
欧拉角通过将刚体绕过原点的轴(i,j,k)旋转θ，分解成三步，如下图（蓝色是起始坐标系，而红色的是旋转之后的坐标系）
如果将每一个角度用旋转矩阵表示如下：
所以，容易得到，欧拉角转旋转矩阵如下：
所以，可得旋转欧拉角转旋转矩阵如下：
旋转矩阵转欧拉角
将旋转矩阵表示如下：
则欧拉角如下：
代码
/**
欧拉角计算对应的旋转矩阵
**/
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// 计算旋转矩阵的X分量
Mat R_x = (Mat_(3,3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);
// 计算旋转矩阵的Y分量
Mat R_y = (Mat_(3,3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);
// 计算旋转矩阵的Z分量
Mat R_z = (Mat_(3,3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1);
// 合并
Mat R = R_z * R_y * R_x;
return R;
}
/***
旋转矩阵转为欧拉角
***/
/*** 功能： 检查是否是旋转矩阵**/
bool isRotationMatrix(Mat &R)
{
Mat Rt;
transpose(R, Rt);
Mat shouldBeIdentity = Rt * R;
Mat I = Mat::eye(3,3, shouldBeIdentity.type());
return norm(I, shouldBeIdentity) < 1e-6;
}
/**
* 功能： 通过给定的旋转矩阵计算对应的欧拉角**/
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
assert(isRotationMatrix(R));
float sy = sqrt(R.at(0,0) * R.at(0,0) + R.at(1,0) * R.at(1,0) );
bool singular = sy < 1e-6; // If
float x, y, z;
if (!singular) {
x = atan2(R.at(2,1) , R.at(2,2));
y = atan2(-R.at(2,0), sy);
z = atan2(R.at(1,0), R.at(0,0));
} else {
x = atan2(-R.at(1,2), R.at(1,1));
y = atan2(-R.at(2,0), sy);
z = 0;
}
return Vec3f(x, y, z);
}
3我自己的坐标信息获取
由于我是在ROS环境中使用旋转矩阵与欧拉角的使用，所以我用TF坐标转换来获取相机坐标下二维码的平移矩阵和欧拉角的信息
首先，先看转换部分的程序（这一部分是直接复制师兄的）
void sendMarkerTf(vector& marker_trans, vector& marker_rot, vector& ids)
{
Mat rot(3, 3, CV_64FC1);
Mat rot_to_ros(3, 3, CV_64FC1);
rot_to_ros.at(0,0) = -1.0;
rot_to_ros.at(0,1) = 0.0;
rot_to_ros.at(0,2) = 0.0;
rot_to_ros.at(1,0) = 0.0;
rot_to_ros.at(1,1) = 0.0;
rot_to_ros.at(1,2) = 1.0;
rot_to_ros.at(2,0) = 0.0;
rot_to_ros.at(2,1) = 1.0;
rot_to_ros.at(2,2) = 0.0;
static tf::TransformBroadcaster marker_position_broadcaster;
for(int i = 0; i < ids.size(); i++)
{
//tf::Transform transform;
//transform.setOrigin(tf::Vector3(marker_trans[i][0], marker_trans[i][1], marker_trans[i][2]));
//tf::Quaternion q;
cv::Rodrigues(marker_rot[i], rot);
rot.convertTo(rot, CV_64FC1);
/***
double pitch = -atan2(rot_mat.at(2,0), rot_mat.at(2,1));
double yaw = acos(rot_mat.at(2,2));
double roll = -atan2(rot_mat.at(0,2), rot_mat.at(1,2));
//q.setRPY(marker_rot[i][0], marker_rot[i][1], marker_rot[i][2]);
q.setRPY(roll, pitch, yaw);
//q = q.inverse();
transform.setRotation(q);
***/
//rot = rot*rot_to_ros.t();
tf::Matrix3x3 tf_rot(rot.at(0,0), rot.at(0,1), rot.at(0,2),
rot.at(1,0), rot.at(1,1), rot.at(1,2),
rot.at(2,0), rot.at(2,1), rot.at(2,2));
tf::Vector3 tf_trans(marker_trans[i][0], marker_trans[i][1], marker_trans[i][2]);
tf::Transform transform(tf_rot, tf_trans);
ostringstream oss;
oss << "marker_" << ids[i];
marker_position_broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_base", oss.str()));
}
}
现在，启动图像发布和接收节点和虚拟环境
运行tf查看信息
rosrun tf tf_echo camera_base marker_23
