diff --git a/docs/en/source/4_application_guide/coordinate_systems.md b/docs/en/source/4_application_guide/coordinate_systems.md index 93fcd27c..5fe4f076 100644 --- a/docs/en/source/4_application_guide/coordinate_systems.md +++ b/docs/en/source/4_application_guide/coordinate_systems.md @@ -11,3 +11,143 @@ * All data published in our wrapper topics is optical data taken directly from our camera sensors. * static and dynamic TF topics publish optical CS and ROS CS to give the user the ability to move from one CS to other CS. +### Using ROS2 TF Tools + +#### Viewing the TF Tree Structure + +You can use the following ROS2 commands to print and visualize the TF tree published by the camera package: + +**Print all TF relationships:** + +```bash +ros2 run tf2_tools view_frames +``` + +This command generates a `frames.pdf` file that displays the hierarchical relationships between all frames. + +![image-20251027111351870](../image/application_guide/image4.png) + +**View all currently published TF information:** + +```bash +ros2 topic echo /tf_static +``` + +#### Visualizing TF Tree with rviz2 + +You can use rviz2 to visualize the TF tree structure and relative positions of coordinate systems in real-time: + +```bash +rviz2 +``` + +In rviz2: + +- Add the `TF` display plugin +- Configure the Fixed Frame to `camera_link` or `camera_depth_optical_frame` +- Select the TF frames to display + +![image-20251027140652727](../image/application_guide/image5.png) + +### Camera TF Calculation and Publishing Mechanism + +#### Core Function: `OBCameraNode::calcAndPublishStaticTransform()` + +The camera node calculates and publishes static transformation relationships between all sensors through this function. Below is a detailed explanation of the code: + +#### Quaternion Initialization and Coordinate System Transformation + +```cpp +tf2::Quaternion quaternion_optical, zero_rot; +zero_rot.setRPY(0.0, 0.0, 0.0); +quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); +``` + +**Explanation:** + +- `quaternion_optical`: Defines the rotation transformation from the optical coordinate system to the ROS standard coordinate system (90-degree rotation) +- This rotation converts the camera optical coordinate system (X right, Y down, Z forward) to the ROS standard coordinate system (X forward, Y left, Z up) + +#### Obtaining Device Information and Base Stream + +```cpp +auto base_stream_profile = stream_profile_[base_stream_]; +auto device_info = device_->getDeviceInfo(); +// The base stream is typically the DEPTH stream +``` + +**Explanation:** + +- A base stream (typically the depth stream) is selected, and all other sensor transformations are calculated relative to this base stream + +#### Iterating Through All Streams and Calculating Relative Transformations + +```cpp +for (const auto &item : stream_profile_) { + auto stream_index = item.first; + auto stream_profile = item.second; + + // Get the extrinsics of this stream relative to the base stream + OBExtrinsic ex; + ex = stream_profile->getExtrinsicTo(base_stream_profile); + + // Convert rotation matrix to quaternion + auto Q = rotationMatrixToQuaternion(ex.rot); + + // Apply optical coordinate system transformation: Q_new = quaternion_optical * Q * quaternion_optical.inverse() + Q = quaternion_optical * Q * quaternion_optical.inverse(); + + tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]); +``` + +**Explanation:** + +- `OBExtrinsic` contains the rotation matrix (`rot`) and translation vector (`trans`) between two sensors +- Quaternion multiplication applies the optical coordinate system transformation to each sensor's rotation relationship +- This transformation converts the camera's native optical coordinate system to the ROS standard coordinate system + +#### Publishing TF Transformations + +```cpp +// Publish the transformation from sensor to base stream (in ROS coordinate system) +publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]); + +// Publish the transformation from physical frame to its optical frame +publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index], + optical_frame_id_[stream_index]); +``` + +**Explanation:** + +- First `publishStaticTF`: Publishes the transformation from the base stream to the current sensor (translation + rotation) +- Second `publishStaticTF`: Publishes the transformation from physical frame to optical frame (pure rotation, no translation) +- `frame_id_[stream_index]`: Physical coordinate system frame name (e.g., `camera_depth_frame`) +- `optical_frame_id_[stream_index]`: Optical coordinate system frame name (e.g., `camera_depth_optical_frame`) + +#### Special Handling for Left and Right IR Cameras + +```cpp +if (stream_index.first == OB_STREAM_IR_RIGHT && base_stream_.first == OB_STREAM_DEPTH) { + trans[0] = std::abs(trans[0]); +} +``` + +**Explanation:** + +- Left and right IR cameras are symmetric about the center plane in the device coordinate system +- Using `abs()` ensures the X-axis offset is positive, maintaining geometric consistency + +#### Publishing Extrinsics from Depth to Other Sensors + +```cpp +if (enable_stream_[DEPTH] && enable_stream_[COLOR] && enable_publish_extrinsic_) { + OBExtrinsic ex = base_stream_profile->getExtrinsicTo(stream_profile_[COLOR]); + auto ex_msg = obExtrinsicsToMsg(ex, "depth_to_color_extrinsics"); + depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg); +} +``` + +**Explanation:** + +- In addition to publishing transformation relationships through TF, raw extrinsic parameters are also published through custom topics +- This allows users to directly access the camera's intrinsic and extrinsic parameters for high-precision point cloud alignment and depth-color registration diff --git a/docs/en/source/image/application_guide/image4.png b/docs/en/source/image/application_guide/image4.png new file mode 100644 index 00000000..2ea5a88b Binary files /dev/null and b/docs/en/source/image/application_guide/image4.png differ diff --git a/docs/en/source/image/application_guide/image5.png b/docs/en/source/image/application_guide/image5.png new file mode 100644 index 00000000..42e2129c Binary files /dev/null and b/docs/en/source/image/application_guide/image5.png differ diff --git a/docs/zh/source/4_application_guide/coordinate_systems.md b/docs/zh/source/4_application_guide/coordinate_systems.md index 01a487c0..fe2efc71 100644 --- a/docs/zh/source/4_application_guide/coordinate_systems.md +++ b/docs/zh/source/4_application_guide/coordinate_systems.md @@ -10,3 +10,143 @@ * 相机光学坐标系:(X: 向右,Y: 向下,Z: 向前) * 我们封装器话题中发布的所有数据都是直接从相机传感器获取的光学数据。 * 静态和动态TF话题发布光学坐标系和ROS坐标系,使用户能够在两个坐标系之间转换。 + +### ROS2 TF工具的使用 + +#### 查看TF树结构 + +可以使用以下ROS2命令来打印和可视化相机包发布的TF树: + +**打印所有TF关系:** + +```bash +ros2 run tf2_tools view_frames +``` + +这个命令会生成一个 `frames.pdf`文件,展示所有frame之间的层级关系。 + +![image-20251027111351870](../image/application_guide/image4.png) + +**查看所有正在发布的TF信息:** + +```bash +ros2 topic echo /tf_static +``` + +#### 使用rviz2可视化TF树 + +在rviz2中可以实时可视化TF树结构和坐标系的相对位置: + +```bash +rviz2 +``` + +在rviz2中: + +- 添加 `TF`显示插件 +- 配置固定框架(Fixed Frame)为 `camera_link`或 `camera_depth_optical_frame` +- 选择显示的TF框架树 + +![image-20251027140652727](../image/application_guide/image5.png) + +### 相机TF计算和发布机制 + +#### 核心函数:`OBCameraNode::calcAndPublishStaticTransform()` + +相机节点通过此函数计算和发布所有传感器之间的静态转换关系。下面是代码的详细解释: + +#### 四元数初始化与坐标系变换 + +```cpp +tf2::Quaternion quaternion_optical, zero_rot; +zero_rot.setRPY(0.0, 0.0, 0.0); +quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); +``` + +**说明:** + +- `quaternion_optical`:定义光学坐标系到ROS标准坐标系的旋转变换(90度旋转) +- 这个旋转将相机光学坐标系(X右、Y下、Z前)转换为ROS标准坐标系(X前、Y左、Z上) + +#### 获取设备信息与基准流 + +```cpp +auto base_stream_profile = stream_profile_[base_stream_]; +auto device_info = device_->getDeviceInfo(); +// 通常基准流是深度流(DEPTH) +``` + +**说明:** + +- 选择一个基准流(通常是深度流),所有其他传感器的变换都相对于这个基准流进行计算 + +#### 遍历所有流并计算相对变换 + +```cpp +for (const auto &item : stream_profile_) { + auto stream_index = item.first; + auto stream_profile = item.second; + + // 获取该流相对于基准流的外参 + OBExtrinsic ex; + ex = stream_profile->getExtrinsicTo(base_stream_profile); + + // 将旋转矩阵转换为四元数 + auto Q = rotationMatrixToQuaternion(ex.rot); + + // 应用光学坐标系变换:Q_new = quaternion_optical * Q * quaternion_optical.inverse() + Q = quaternion_optical * Q * quaternion_optical.inverse(); + + tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]); +``` + +**说明:** + +- `OBExtrinsic`包含了两个传感器之间的旋转矩阵(`rot`)和平移向量(`trans`) +- 通过四元数乘法将光学坐标系变换应用到每个传感器的旋转关系中 +- 这个变换将相机原生的光学坐标系转换为ROS标准坐标系 + +#### 发布TF变换 + +```cpp +// 发布传感器到基准流的变换(在ROS坐标系中) +publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]); + +// 发布传感器到其光学frame的变换 +publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index], + optical_frame_id_[stream_index]); +``` + +**说明:** + +- 第一个 `publishStaticTF`:发布从基准流到当前传感器的变换(平移+旋转) +- 第二个 `publishStaticTF`:发布从物理frame到光学frame的变换(纯旋转,无平移) +- `frame_id_[stream_index]`:物理坐标系frame名称(如 `camera_depth_frame`) +- `optical_frame_id_[stream_index]`:光学坐标系frame名称(如 `camera_depth_optical_frame`) + +#### 特殊处理左右红外摄像头 + +```cpp +if (stream_index.first == OB_STREAM_IR_RIGHT && base_stream_.first == OB_STREAM_DEPTH) { + trans[0] = std::abs(trans[0]); +} +``` + +**说明:** + +- 左右红外摄像头在设备坐标系中关于中心平面对称 +- 通过 `abs()`确保X轴偏移为正值,保持几何一致性 + +#### 发布深度到其他传感器的外参 + +```cpp +if (enable_stream_[DEPTH] && enable_stream_[COLOR] && enable_publish_extrinsic_) { + OBExtrinsic ex = base_stream_profile->getExtrinsicTo(stream_profile_[COLOR]); + auto ex_msg = obExtrinsicsToMsg(ex, "depth_to_color_extrinsics"); + depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg); +} +``` + +**说明:** + +- 通过TF发布变换关系 diff --git a/docs/zh/source/image/application_guide/image4.png b/docs/zh/source/image/application_guide/image4.png new file mode 100644 index 00000000..2ea5a88b Binary files /dev/null and b/docs/zh/source/image/application_guide/image4.png differ diff --git a/docs/zh/source/image/application_guide/image5.png b/docs/zh/source/image/application_guide/image5.png new file mode 100644 index 00000000..42e2129c Binary files /dev/null and b/docs/zh/source/image/application_guide/image5.png differ