ORB SLAM2 实践 (三)发布TF (相机轨迹与坐标变换)

Share:
## 目标 实现实时发布SLAM 的TF 使用TUM Dataset 来测试 使用Realsense Camera 进行测试 查看Realsense 相机的TF 通过调节Realsense相机参数,让其发布自身的TF 通过TF节点 来查阅TF树: rosrun tf view_frames ![img](http://qiniu.yubaoliu.cn/1.png) 其中对我们比较有用的是这几个。 ## 相机: 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在跟着动。 ![img](http://qiniu.yubaoliu.cn/3.png) 由图可知,它们都是相机坐标系。在建图的时候要转换成ROS中的map/world坐标系。形成 camera_frame -> odom -> map 这样的TF树。 相关知识 ## ROS 坐标系,机器人坐标系 ![img](http://qiniu.yubaoliu.cn/9KgUe.png) 右手做出手枪的姿势,前方即是X轴方向 ,朝上为Z轴,左方为Y轴。 ![image-20200427212847492](http://qiniu.yubaoliu.cn/image-20200427212847492.png) (注:这不是教材,参考书上的那种判定方式。) ## 相机坐标系 伸出左手,大姆指指向右方(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坐标系

No comments