neckprotecter

在宇树机器狗上用相机sdk打开相机并使用Apriltag

发表于 更新于 分类于 ， ， Disqus：

解决了如何让 Apriltag 功能包成功识别宇树机器狗相机 sdk 获得的图像的问题

问题发现：

在进行实验室实习项目时，需要使用 Apeiltag 功能包，但宇树机器狗只提供了相机 sdk 来让使用者调用相机，传统的 usb_cam 功能包方法需要额外配置参数，这里我选择仍使用 sdk 来打开相机。

实现过程：

首先研究与 Apriltag 功能包有关的话题

apriltag话题

  • 可以知道 apriltag 功能包需要接收 /camera/image_rect 和 /camera/camera_info 两个话题。
  • 而调用相机 sdk 之后，得到的图像就可以作为 image_rect 话题的输入，那么问题就到了 camera_info 这个话题上，有两个方面需要解决：这个话题是什么，这个话题怎么给。
  • 首先，这个话题是关于相机的一些参数信息，根据博客：https://blog.csdn.net/wangmj_hdu/article/details/115002514 中的分享，我们可以得知这些参数的含义：
    • header：标准消息头
    • seq：序列 ID，连续递增的 ID 号
    • stamp：两个时间戳
    • frame_id：与此数据相关联的帧 ID
    • height：图像尺寸，height 代表高度，（height*width）相机的分辨率，以像素为单位
    • width：图像尺寸，width 代表宽度，（height*width）相机分辨率，以像素为单位
    • distortion_model：指定了相机畸变模型，对于大多数相机，”plumb_bob”简单的径向和切向畸变模型就足够了
    • D：畸变参数，取决于畸变模型，(k1, k2, t1, t2, k3)，（我的这个 usb 相机号称是无畸变相机，但通过标定结果可以看出来还是存在畸变的，是不是被商家坑了，哈哈哈）
    • K：相机内参矩阵，使用焦距(fx, fy)和主点坐标(cx, cy)，单位为像素，内参矩阵可以将相机坐标中的 3D 点投影到 2D 像素坐标
      Alt text
    • R：旋转矩阵，将相机坐标系统对准理想的立体图像平面，使两张立体图像中的极线平行
    • P：投影矩阵
      Alt text
      左边 3*3 矩阵是相机的内参矩阵，将相机坐标中的 3D 点投影到 2D 像素坐标，可能与相机内参 K 不同。对于单目相机 Tx = Ty = 0。对于双目相机，Tx 和 Ty 有所不同。
    • binning_x：图像下采样参数，水平方向
    • binning_y：图像下采样参数，竖直方向
    • (width / binning_x) x (height / binning_y)
    • 下采样：binning_x = binning_y ＞ 1。缩小图像，生成对应图像的缩略图，使得图像符合显示区域的大小。
    • roi：感兴趣区域定义，即完整图像上的一个矩形子窗口
  • 接下来就是关于这个话题的输入方式，因为宇树有给出相机的标定参数： Alt text因此直接通过这些参数计算出 camera_info 需要的参数即可：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
// 相机信息处理
sensor_msgs::CameraInfo getCameraInfo(void)
{                                     // extract cameraInfo.
    sensor_msgs::CameraInfo cam_info; // 信息对象

    // 相机参数赋值，K矩阵，D畸变参数等都根据宇树提供的相机标定参数计算得出
    vector<double> D{
        -2.0874507966557976e-01, 2.6922545819193944e-02,
        1.4887812678896397e-03, 3.5422189471526382e-03};

    boost::array<double, 9> K =
        {
            1.9086173551086370e+02, 5.6239632677513474e-01, 2.2299573347226581e+02,
            0., 1.9608536122280060e+02, 1.9553241319375684e+02,
            0., 0., 1.};

    boost::array<double, 12> P =
        {
            192.85, 0, 222.99, 0,
            0, 196.08, 195.53, 0,
            0, 0, 1, 0};

    boost::array<double, 9> r =
        {
            -9.9992248085182622e-01, 1.1878079705669157e-02, -3.7341009138761533e-03,
            -1.1878162517594585e-02, -9.9992945213910256e-01, 0.,
            -3.7338374810443040e-03, 4.4354257512119405e-05, 9.9999302822087976e-01};

    cam_info.height = 480;
    cam_info.width = 640;
    cam_info.distortion_model = "plumb_bob";
    cam_info.D = D;
    cam_info.K = K;
    cam_info.P = P;
    cam_info.R = r;
    cam_info.binning_x = 0;
    cam_info.binning_y = 0;
    cam_info.header.frame_id = "camera"; // frame_id为camera，也就是相机名字

    return cam_info;
}

正常使用 apriltag

现在我们提供了 Apriltag 功能包需要的话题，就能够正常使用 apriltag 了，只需要发布话题出去，然后通过 apriltag 功能包给出的使用方法就能够实现让 Apriltag 功能包成功识别宇树机器狗相机 sdk 获得的图像。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
while (cam.isOpened())
   {
       // 同时获取左右两个相机画面，但后续只用到左侧
       cv::Mat left, right;
       if (!cam.getRectStereoFrame(left, right)) ///< get rectify left,right frame
       {
           usleep(1000);
           continue;
       }
       cv::flip(left, left, -1);         // 原图像为翻转图像，使用该函数翻转为正图像
       cv::imshow("Longlat_Rect", left); // 将图像显示在屏幕上

       // ---------------------------------------------------------------------------------------------------------------

       // 信息整合发布
       // 图像信息需要信息头header
       std_msgs::Header image_header;
       image_header.frame_id = "camera";
       image_header.stamp = ros::Time::now();

       // 相机信息赋值，函数见最上方
       camera_info_msg = getCameraInfo();
       camera_info_msg.header.stamp = image_header.stamp; // 相机信息和图像信息的时间戳要相同

       sensor_msgs::ImagePtr image_msg = cv_bridge::CvImage(image_header, "bgr8", left).toImageMsg(); // 将图像信息的信息头header和图像left整合
       pub_image.publish(image_msg);                                                                  // 图像信息发布
       pub_info.publish(camera_info_msg);                                                             // 相机信息发布

       // ---------------------------------------------------------------------------------------------------------------

       char key = cv::waitKey(10);
       if (key == 27) // esc键退出
           break;
   }

完整代码见：