sensor_msgs::Image转换成深度图，或直接获取距离

1.使用cv_bridge

#include <cv_bridge/cv_bridge.h>
void CB(const sensor_msgs::ImageConstPtr& depth) {
  cv_bridge::CvImageConstPtr depth_ptr;
  depth_ptr =
      cv_bridge::toCvShare(depth, sensor_msgs::image_encodings::TYPE_32FC1);
  cv::imshow("res", depth_ptr->image);
  cv::waitKey(1);
}

• 显示的图像不对，试了好几个image_encodings都不行

2.直接获取距离

• 属性
  • height: 480
  • width: 640
  • encoding: “16UC1”
  • is_bigendian: 0

typedef union USHORT_UNION {
  ushort num;
  char c[2];
} ushort_union;

ushort Little(char c0, char c1) {
  ushort_union tmp;
  tmp.c[0] = c0;
  tmp.c[1] = c1;
  return tmp.num;
}

void cb(const sensor_msgs::Image::ConstPtr& depth) {
  int x = 320, y = 240;
  int index = y * depth->width + x;
  ushort tmp = Little(depth->data[index*2], depth->data[index*2+1]);//单位mm
  // 也可以转换成深度图
  cv::Mat dep_im = cv::Mat::zeros(depth->height, depth->width, CV_8UC3);
  for (int i = 0; i < depth->height; ++i) {
    for (int j = 0; j < depth->width; j++) {
      int index = i * depth->width + j;
      ushort tmp = Little(depth->data[index * 2], depth->data[index * 2 + 1]);
      if (tmp > 4000)
        tmp = 4000;
      dep_im.at<cv::Vec3b>(i, j)[0] = tmp * 255 / 4000;
      dep_im.at<cv::Vec3b>(i, j)[1] = tmp * 255 / 4000;
      dep_im.at<cv::Vec3b>(i, j)[2] = tmp * 255 / 4000;
    }
  }
}