T265 SDK的set_pose函数使用示例

以下是使用T265 SDK的set_pose函数的示例代码：

#include <iostream>
#include <librealsense2/rs.hpp>

int main()
{
    // Create a pipeline
    rs2::pipeline pipe;
    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);

    // Start the pipeline
    rs2::pipeline_profile profile = pipe.start(cfg);

    // Get the T265 device
    rs2::device device = profile.get_device();
    rs2::pose_sensor pose_sensor = device.first<rs2::pose_sensor>();

    // Set the initial pose of the device
    rs2_pose initial_pose;
    initial_pose.translation.x = 0.0f;
    initial_pose.translation.y = 0.0f;
    initial_pose.translation.z = 1.0f;
    initial_pose.rotation.x = 0.0f;
    initial_pose.rotation.y = 0.0f;
    initial_pose.rotation.z = 0.0f;
    initial_pose.rotation.w = 1.0f;
    pose_sensor.set_pose(initial_pose);

    // Wait for the device to stabilize
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    while (true)
    {
        // Get the latest pose data
        rs2::frameset frames = pipe.wait_for_frames();
        rs2::pose_frame pose_frame = frames.get_pose_frame();
        rs2_pose pose_data = pose_frame.get_pose_data();

        std::cout << "Pose data:" << std::endl;
        std::cout << "Translation: (" << pose_data.translation.x << ", " << pose_data.translation.y << ", " << pose_data.translation.z << ")" << std::endl;
        std::cout << "Rotation: (" << pose_data.rotation.x << ", " << pose_data.rotation.y << ", " << pose_data.rotation.z << ", " << pose_data.rotation.w << ")" << std::endl;

        // Set the new pose of the device
        rs2_pose new_pose;
        new_pose.translation.x = pose_data.translation.x + 0.1f;
        new_pose.translation.y = pose_data.translation.y + 0.1f;
        new_pose.translation.z = pose_data.translation.z + 0.1f;
        new_pose.rotation.x = pose_data.rotation.x;
        new_pose.rotation.y = pose_data.rotation.y;
        new_pose.rotation.z = pose_data.rotation.z;
        new_pose.rotation.w = pose_data.rotation.w;
        pose_sensor.set_pose(new_pose);
    }

    return 0;
}

在此示例中，我们首先创建了一个管道，并启用了T265设备的姿态流。然后，我们获取了T265设备的姿态传感器，并使用set_pose函数设置了设备的初始姿态。接下来，我们等待设备稳定，并在一个循环中获取最新的姿态数据，并使用set_pose函数设置设备的新姿态。在此示例中，我们只是将设备的平移坐标增加了0.1单位，而保持其旋转不变。