## 旋转矩阵到四元数的转换
```cpp
std::vector toQuaternion(const cv::Mat& M) {
Eigen::Matrix eigMat = toMatrix3d(M);
Eigen::Quaterniond q(eigMat);
std::vector v(4); v[0] = q.x();
v[1] = q.y(); v[2] = q.z(); v[3] = q.w();
return v;
}
```
## OpenCV矩阵到Eigen矩阵的类型转换
```cpp
#include Eigen::Matrix toMatrix3d(const cv::Mat& cvMat3) {
Eigen::Matrix M; M << cvMat3.at(0, 0), cvMat3.at(0, 1), cvMat3.at(0, 2), cvMat3.at(1, 0), cvMat3.at(1, 1), cvMat3.at(1, 2), cvMat3.at(2, 0), cvMat3.at(2, 1), cvMat3.at(2, 2);
return M;
}
```
## 遗留问题
发布
建图的时候要转换成ROS中的map/world坐标系
ORB SLAM2 实践 （三）发布TF （相机轨迹与坐标变换）
## 目标
实现实时发布SLAM 的TF
使用TUM Dataset 来测试
使用Realsense Camera 进行测试
查看Realsense 相机的TF
通过调节Realsense相机参数，让其发布自身的TF
通过TF节点 来查阅TF树： rosrun tf view_frames
其中对我们比较有用的是这几个。
## 相机：
camera_depth_optical_frame
camera_color_optical_frame
camera_link
## IMU：
camera_accel_optical_frame
camera_gyro_optical_frame
在传感器整合时会用到IMU, 现在暂不考虑。
我们的目标是要发布 camera_link 到 world (SLAM 中的世界坐标系，为了与ROS进行区分，我将其命名为odom) 的TF.
但问题是，我们通过SLAM获得的是从camera_color_optical_frame到odom的TF. 那么如何发布camera_link (l)到odom的TF？
注，上标应该是写在左上角的，但打字比较麻烦，就放到右上角了。
我们要得到的是：
事实上一般情况下我们只需要用 就可以了。如果是要将相机与机器人间的相对坐标关系考虑进来的话，发布是比较准确的。因为我们可能需要手动测量相机与机器人底坐之间的坐标变换。
如果只发布的话，可以禁止Realsense 节点发布自身的TF. 如果不禁用的话，会出现两棵TF 树。
## ORB SLAM2 发布TF
坐标变换
```cpp
cv::Mat Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cv_ptrRGB->header.stamp.toSec());
cv::Mat Rcw = Tcw.rowRange(0, 3).colRange(0, 3);
cv::Mat tcw = Tcw.rowRange(0, 3).col(3);
cv::Mat Rwc = Rcw.t();
cv::Mat twc = -Rwc * tcw;
```
Rwc， twc 即, : 相机相对于世界坐标系下的坐标旋转与平移。
换言之，其为相机坐标到世界坐标系的变换。
可以利用 tf2_ros::TransformBroadcaster 来发布TF.
可视化
将Fixed_frame设置为 odom
现在移动相机，就可以看到相机Frame在跟着动。
由图可知，它们都是相机坐标系。在建图的时候要转换成ROS中的map/world坐标系。形成
camera_frame -> odom -> map 这样的TF树。
相关知识
## ROS 坐标系，机器人坐标系
右手做出手枪的姿势，前方即是X轴方向 ，朝上为Z轴，左方为Y轴。
（注：这不是教材，参考书上的那种判定方式。）
## 相机坐标系
伸出左手，大姆指指向右方（X轴）， 前方为Z轴，下方为Y轴。
## 旋转矩阵到四元数的转换
```cpp
std::vector toQuaternion(const cv::Mat& M) {
Eigen::Matrix eigMat = toMatrix3d(M);
Eigen::Quaterniond q(eigMat);
std::vector v(4); v[0] = q.x();
v[1] = q.y(); v[2] = q.z(); v[3] = q.w();
return v;
}
```
## OpenCV矩阵到Eigen矩阵的类型转换
```cpp
#include Eigen::Matrix toMatrix3d(const cv::Mat& cvMat3) {
Eigen::Matrix M; M << cvMat3.at(0, 0), cvMat3.at(0, 1), cvMat3.at(0, 2), cvMat3.at(1, 0), cvMat3.at(1, 1), cvMat3.at(1, 2), cvMat3.at(2, 0), cvMat3.at(2, 1), cvMat3.at(2, 2);
return M;
}
```
## 遗留问题
发布
建图的时候要转换成ROS中的map/world坐标系
## 旋转矩阵到四元数的转换
```cpp
std::vector toQuaternion(const cv::Mat& M) {
Eigen::Matrix eigMat = toMatrix3d(M);
Eigen::Quaterniond q(eigMat);
std::vector v(4); v[0] = q.x();
v[1] = q.y(); v[2] = q.z(); v[3] = q.w();
return v;
}
```
## OpenCV矩阵到Eigen矩阵的类型转换
```cpp
#include Eigen::Matrix toMatrix3d(const cv::Mat& cvMat3) {
Eigen::Matrix M; M << cvMat3.at(0, 0), cvMat3.at(0, 1), cvMat3.at(0, 2), cvMat3.at(1, 0), cvMat3.at(1, 1), cvMat3.at(1, 2), cvMat3.at(2, 0), cvMat3.at(2, 1), cvMat3.at(2, 2);
return M;
}
```
## 遗留问题
发布
建图的时候要转换成ROS中的map/world坐标系
No comments