Kinect v2.0实现对采集到的图像实时解算距离和坐标信息，并分析源码及其背后的算法原理---ROS下实现的，用鼠标控制【附github链接，linuxc++源码】

Jack Ju

已于 2023-10-07 16:24:35 修改

阅读量4.7k

点赞数 10

分类专栏：自动驾驶算法文章标签：计算机视觉传感器

于 2020-07-24 11:54:55 首次发布

本文链接：https://blog.csdn.net/qq_40464599/article/details/107557651

版权

自动驾驶算法专栏收录该内容

73 篇文章 51 订阅

订阅专栏

Kinect2 v2.0 是一款深度相机，笔者想用它来测量距离信息，是在ROS系统下进行的，用鼠标去点击图片就能显示该位置的物体的距离和坐标相对于相机来说。

首先要知道相机坐标系，Kinect使用以Kinect的IR传感器为中心的直角坐标系。Y轴正指向上方，Z轴正指向Kinect指向的位置，X轴正向左侧。此坐标系中的一个单位等于一米。

， Jack Ju https://img-blog.csdnimg.cn/20200724114331678.png

为了实现这个功能，资料很少，并且没有给出源码链接，为了避免后面的人走弯路。所以我这里给出了github链接，直接使用就可以。kinect v2详细参数和原理介绍_别人论文里和自己整理出来的一些内容_kinect v2参数-CSDN博客

https://github.com/JackJu-HIT/kinect-v2.0

参考了博客ROS下Kinect2的驱动安装及简单应用_ros2 kinect-CSDN博客

运行效果如下图所示：

Jack Ju https://img-blog.csdnimg.cn/20200724114950928.png

因为我也刚接触视觉和PCL点云，我只说明输出部分，我最终要用他的输出数据进行导航。

下面的就是输出的数据要显示在屏幕上。

double distance=sqrt(ptMsg.point.x*ptMsg.point.x+ptMsg.point.y*ptMsg.point.y+ptMsg.point.z*ptMsg.point.z);
      cv::putText(color, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
      ossXYZ.str("");
      ossXYZ <<"Distance"<<distance<< "m cooridates"<<"( " << ptMsg.point.x << ", " << ptMsg.point.y
                                  << ", " << ptMsg.point.z << " )";
      //ossXYZ<<"Distance"<<distance;
      cv::putText(color, ossXYZ.str(), cv::Point(img_x, img_y), font, 1, colorText, 3, CV_AA);
      // cv::circle(color, cv::Point(mouseX, mouseY), 5, cv::Scalar(0, 0, 255), -1);
      cv::imshow(window_name, color);
      // cv::imshow(window_name, depth);
      cv::waitKey(1);

20200724

----------------------------------------------------------------------------分割线-------------------------------------------------------------------------------

今天重新分析了其中的源码，大概说说我对他的理解。如果有问题请指出。

因为这里ROS订阅了RGB图像话题和深度话题，要满足实时性，你需要让这两个话题同步的去接收这样才能对其，当你点击RGB图像会调用深度图像解算出位置信息。那么如何通过深度信息解算出信息？

一、如何通过深度信息解算出信息？

所谓深度图像（depth image)也被称为距离影像（range image），是指将从图像采集器到场景中各点的距离（深度）作为像素值的图像，就是把像素点的像素值改成深度信息.

像素坐标到世界坐标系转换：

像素坐标：[u,v,d]------>世界坐标[x,y,z]

// 相机内参
const double camera_factor=1000;
const double cx=325.5;
const double cy=253.5;
const double fx=518.0;
const double fy=519.0;

$x=(u-c_x)*d/f_x$

$y=(v-c_y)*d/f_y$

z=d/camera_factor

camera_factor通常为1000。

那我们来看看viewer.cpp相关代码

void createLookup(size_t width, size_t height)这里的求求x和y没有乘上d，严格意义上式x一部分，下一段才是真正求解x和y

void createLookup(size_t width, size_t height)
  {
    /**内参长成这样
     * 
     * ＃[fx 0 cx]＃K
         [0 fy cy]
         [0 0 1]
     * 
     * *****/

    const float fx = 1.0f / cameraMatrixColor.at<double>(0, 0);
    const float fy = 1.0f / cameraMatrixColor.at<double>(1, 1);
    const float cx = cameraMatrixColor.at<double>(0, 2);
    const float cy = cameraMatrixColor.at<double>(1, 2);
    float *it;

    lookupY = cv::Mat(1, height, CV_32F);
    it = lookupY.ptr<float>();
    for(size_t r = 0; r < height; ++r, ++it)
    {
      *it = (r - cy) * fy;
    }

    lookupX = cv::Mat(1, width, CV_32F);
    it = lookupX.ptr<float>();
    for(size_t c = 0; c < width; ++c, ++it)
    {
      *it = (c - cx) * fx;
    }
  }
};

这一段，就是真正的求解xy坐标的，你会发现，它把数值存到点云中了，后面也乘上了depthValue。

itP->z = depthValue;
itP->x = *itX * depthValue;//妙啊
itP->y = y * depthValue;
itP->b = itC->val[0];
itP->g = itC->val[1];
itP->r = itC->val[2];
itP->a = 255;

void createCloud(const cv::Mat &depth, const cv::Mat &color, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud) const
  {
    const float badPoint = std::numeric_limits<float>::quiet_NaN();

    #pragma omp parallel for
    for(int r = 0; r < depth.rows; ++r)
    {
      pcl::PointXYZRGBA *itP = &cloud->points[r * depth.cols];
      const uint16_t *itD = depth.ptr<uint16_t>(r);
      const cv::Vec3b *itC = color.ptr<cv::Vec3b>(r);
      const float y = lookupY.at<float>(0, r);
      const float *itX = lookupX.ptr<float>();

      for(size_t c = 0; c < (size_t)depth.cols; ++c, ++itP, ++itD, ++itC, ++itX)
      {
        register const float depthValue = *itD / 1000.0f;
        // Check for invalid measurements
        if(*itD == 0)
        {
          // not valid
          itP->x = itP->y = itP->z = badPoint;
          itP->rgba = 0;
          continue;
        }
        itP->z = depthValue;
        itP->x = *itX * depthValue;//妙啊
        itP->y = y * depthValue;
        itP->b = itC->val[0];
        itP->g = itC->val[1];
        itP->r = itC->val[2];
        itP->a = 255;
      }
    }
  }

我们实际上是根据像素来匹配点云，进而得到世界坐标系的xyz，点云的xyz信息。具体代码看如下：

最关键的部分，你通过（img_x,img_y）匹配points，进而得到坐标。 const pcl::PointXYZRGBA& pt = cloud->points[img_y * depth.cols + img_x];//创建点云并赋值
ptMsg.point.x = pt.x;
ptMsg.point.y = pt.y;
ptMsg.point.z = pt.z;

基于此原理，我们可以写一个在你的屏幕中间提取一条竖线的障碍物距离信息并返回最小距离，代码如下

/***实现屏幕中央一条竖线返回最小距离值***/
double min_distance=1000000000;
double middle_distance;
for(int i=0;i<217;i++)
{
const pcl::PointXYZRGBA& gt = cloud->points[905 * depth.cols + i];//创建点云并赋值
middle_distance=sqrt(gt.x*gt.x+gt.y*gt.y+gt.z*gt.z);
if(middle_distance<min_distance)
min_distance=middle_distance;
}
cout<<"相机正前方最短距离为："<<min_distance<<endl;
if(min_distance<1)
cout<<"相机正前方的物体小于1米，请避开障碍物！"<<endl;

void cloudViewer() {
  cv::Mat color, depth;
  std::chrono::time_point<std::chrono::high_resolution_clock> start, now;
  double fps = 0;
  size_t frameCount = 0;
  std::ostringstream oss;
  std::ostringstream ossXYZ; // 新增一个string流
  const cv::Point pos(5, 15);
   CV_RGB(255, 0, 0);
  const double sizeText = 0.5;
  const int lineText = 1;
  const int font = cv::FONT_HERSHEY_SIMPLEX;
  // 从全局变量获取当前鼠标坐标
  int img_x = mouseX;
  int img_y = mouseY;
  geometry_msgs::PointStamped ptMsg;
  ptMsg.header.frame_id = "kinect_link";

  lock.lock();
  color = this->color;
  depth = this->depth;
  updateCloud = false;
  lock.unlock();

  const std::string window_name = "color viewer";
  cv::namedWindow(window_name);
  // 注册鼠标回调函数, 第三个参数是C++11中的关键字, 若不支持C++11, 替换成NULL
  cv::setMouseCallback(window_name, onMouse, nullptr);
  createCloud(depth, color, cloud);

  for(; running && ros::ok();) {
    if(updateCloud) {
      lock.lock();
      color = this->color;
      depth = this->depth;
      updateCloud = false;
      lock.unlock();

      createCloud(depth, color, cloud);
      img_x = mouseX;
      img_y = mouseY;
      const pcl::PointXYZRGBA& pt = cloud->points[img_y * depth.cols + img_x];//创建点云并赋值
      ptMsg.point.x = pt.x;
      ptMsg.point.y = pt.y;
      ptMsg.point.z = pt.z;
      //leftBtnPointPub.publish(ptMsg);//发布
      //cout<<
      // 根据鼠标左键压下或右键压下, 分别发布三维坐标到不同的话题上去
      switch (mouseBtnType) {
      case cv::EVENT_LBUTTONUP:
          ptMsg.header.stamp = ros::Time::now();
          leftBtnPointPub.publish(ptMsg);
          ros::spinOnce();
          break;
      case cv::EVENT_RBUTTONUP:
          ptMsg.header.stamp = ros::Time::now();
          rightBtnPointPub.publish(ptMsg);
          ros::spinOnce();
          break;
      default:
          break;
      }
      mouseBtnType = cv::EVENT_MOUSEMOVE;

      ++frameCount;
      now = std::chrono::high_resolution_clock::now();
      double elapsed =
          std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() / 1000.0;
      if(elapsed >= 1.0) {
        fps = frameCount / elapsed;
        oss.str("");
        oss << "fps: " << fps << " ( " << elapsed / frameCount * 1000.0 << " ms)";
        start = now;
        frameCount = 0;
      }
      double distance=sqrt(ptMsg.point.x*ptMsg.point.x+ptMsg.point.y*ptMsg.point.y+ptMsg.point.z*ptMsg.point.z);
      cv::putText(color, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
      ossXYZ.str("");
      ossXYZ <<"Distance"<<distance<< "m cooridates"<<"( " << ptMsg.point.x << ", " << ptMsg.point.y
                                  << ", " << ptMsg.point.z << " )";


      /***实现屏幕中央一条竖线返回最小距离值***/
      double min_distance=1000000000;
      double middle_distance;
      for(int i=0;i<217;i++)
      {
       const pcl::PointXYZRGBA& gt = cloud->points[905 * depth.cols + i];//创建点云并赋值
       middle_distance=sqrt(gt.x*gt.x+gt.y*gt.y+gt.z*gt.z);
       if(middle_distance<min_distance)
            min_distance=middle_distance;
      }
      cout<<"相机正前方最短距离为："<<min_distance<<endl;
     if(min_distance<1)
         cout<<"相机正前方的物体小于1米，请避开障碍物！"<<endl;
      //ossXYZ<<"Distance"<<distance;
      cv::putText(color, ossXYZ.str(), cv::Point(img_x, img_y), font, 1, colorText, 3, CV_AA);
      // cv::circle(color, cv::Point(mouseX, mouseY), 5, cv::Scalar(0, 0, 255), -1);
      cv::imshow(window_name, color);
      // cv::imshow(window_name, depth);
      cv::waitKey(1);
    }

  }
  cv::destroyAllWindows();
  cv::waitKey(100);
}

这是代码的关键部分，主要解释了如何实现坐标的提取，其中还有很多的同步时间订阅（meaasge_filters）、传感器信息预处理等等。

如果觉得文章还不错，可以关注下我的微信公众号：
在这里插入图片描述 20200728

鞠春宇于研究室

Jack Ju

关注

10
点赞
踩
74

收藏

觉得还不错? 一键收藏
打赏
5
评论
Kinect v2.0实现对采集到的图像实时解算距离和坐标信息，并分析源码及其背后的算法原理---ROS下实现的，用鼠标控制【附github链接，linuxc++源码】

Kinect2 v2.0 是一款深度相机，笔者想用它来测量距离信息，是在ROS系统下进行的，用鼠标去点击图片就能显示该位置的物体的距离和坐标相对于相机来说。首先要知道相机坐标系，Kinect使用以Kinect的IR传感器为中心的直角坐标系。Y轴正指向上方，Z轴正指向Kinect指向的位置，X轴正向左侧。此坐标系中的一个单位等于一米。，为了实现这个功能，资料很少，并且没有给出源码链接，为了避免后面的人走弯路。所以我这里给出了github链接，直接使用就可以。http...
复制链接

扫一扫