Kinect v2保存图像和深度图序列

上班后的端午节就意味着多一天的假期！！！

本工作的主要出发点是录制数据集，用来供后续的建图和bug重现。

软硬件配置

环境配置如下：

系统：Ubuntu 16.04 LTS  64位

CPU: Intel® Core™ i7-7700 CPU @ 3.60GHz × 8 

内存：16G

工作内容

1）保存的工作主要是在ROS环境下，所以没有ROS环境的话，可以先配置一下ROS环境；

2）Kinect v2的驱动应该是已经配置好了的，包括libfreenet2和catkin工作环境下的iai_kinect2；

3）如果对Kinect v2保存不要求帧率，可以参考这篇博客（感谢这位博主提供的教程）中的保存图像序列。在这个工作中，能够保存对应的彩色图，深度图，彩色深度图以及点云文件pcd，但帧率很慢，我在10s内通过这种方法保存了28张图片，所以帧率大约是在2~3fps之间。

4）为了能够使图像之间更加连续，关联性更大，发现代码保存是每一个循环都会调用一个imwrite，如果一张1920x1080图片0.8M，按照Kinect v2官方给出的fps为30，那么你的磁盘写入速率应该达到24M/s，如果考虑到深度图还要写入，那么应该是48M/s，我觉得可能是写这块太耗时，而且我也用不着每一帧的点云文件和彩色深度图，所以在saveCloudAndImages函数里注释掉这两行

OUT_INFO("saving cloud: " << cloudName);
writer.writeBinary(cloudName, *cloud);

OUT_INFO("saving depth: " << depthColoredName);
cv::imwrite(depthColoredName, depthColored, params);

然后重新在catkin_make  , source devel/set_up.bash后，保存速度可以达到5fps。仍然很慢。

5）查看ROS读取Kinect v2的平均速度是在10~20fps之间跳动，所以达不到30fps（目前我猜测可能是ROS读取的原因），但5fps也太慢了，我决定不要求每一次读入图片都立即保存，先将其存在内存中，等结束时再统一写入磁盘，这样带来的问题是很吃内存，所以不能保存太长时间。经过这样的修改方案，保存结果是8fps，比之前有所提升，但还是比较慢。修改如下：

注：这个修改是基于上面提出那篇博客的基础之上修改的，当然也没有保存pcd点云文件和彩色深度图。

在Receiver类中添加私有变量 

bool save_stop;

std::vector<cv::Mat> vcolor;
std::vector<cv::Mat> vdepth;

在Receiver的构造函数中添加

save_stop(false);

我这次主要是在imageviewer这个函数里修改，void imageViewer()，而不是下面的Cloudviewer,读者有兴趣可以对比着原来的代码看看，我主要是将读取键盘的指令移入到for循环当中了。

void imageViewer()
  {
    cv::Mat color, depth, depthDisp, combined;
    std::chrono::time_point<std::chrono::high_resolution_clock> start, now;
    double fps = 0;
    size_t frameCount = 0;
    std::ostringstream oss;
    const cv::Point pos(5, 15);
    const cv::Scalar colorText = CV_RGB(255, 255, 255);
    const double sizeText = 0.5;
    const int lineText = 1;
    const int font = cv::FONT_HERSHEY_SIMPLEX;

    cv::namedWindow("Image Viewer");
    oss << "starting...";

    start = std::chrono::high_resolution_clock::now();
    for(; running && ros::ok();)
    {
      if(updateImage)
      {
        lock.lock();
        color = this->color;
        depth = this->depth;
        updateImage = false;
        lock.unlock();

        ++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)";
          //std::cerr << "fps: " << fps << " ( " << elapsed / frameCount * 1000.0 << " ms)"<< std::endl;
          start = now;
          frameCount = 0;
        }

        dispDepth(depth, depthDisp, 12000.0f);
        combine(color, depthDisp, combined);
        //combined = color;

        cv::putText(combined, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
        cv::imshow("Image Viewer", combined);
        int key = cv::waitKey(1);
            switch(key & 0xFF)
            {
                case 27:
                case 'q':
                running = false;
                break;
                case 'b': save_seq = true; save = true; break;
                case 'e': save_seq = false; save = false; save_stop = true; break;

                case ' ':
                case 's':
                if (save_seq) break;
            }
        if (save_seq) {
            save = false;
            cv::Mat depthDisp;
            dispDepth(depth, depthDisp, 12000.0f);
            vcolor.push_back(color.clone());
            vdepth.push_back(depthDisp.clone());  
        }
        if(!save_seq && save_stop){
            std::cerr << "starting saving ..."<<std::endl;
            for(size_t i = 0; i < vcolor.size(); i ++){
                oss.str("");
                oss << "./" << std::setfill('0') << std::setw(4) << i;
                const std::string baseName = oss.str();
                const std::string colorName = baseName + "_color.jpg";
                const std::string depthName = baseName + "_depth.png";

                OUT_INFO("saving color: " << colorName);
                cv::imwrite(colorName, color, params);
                OUT_INFO("saving depth: " << depthName);
                cv::imwrite(depthName, depth, params);
            
            }
            OUT_INFO("saving complete!");
            save_stop = false;
            //save_seq =  false;
            vcolor.clear();
            vdepth.clear();
        }
      }

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

保存后再次在~/catkin_ws目录下运行catkin_make，应该还有一些后缀，读者自己找一下吧，然后source一下devel/setup.bash，

运行原来的命令就好，但是这条命令最后的cloud应该改为image,

rosrun kinect2_viewer ve_seq hd image

这样就可以保存1920x1080的图片序列和深度图序列了。

后续可能可以修改的点，我觉得8fps还是太慢，我后面还要用Kinect v2的话，应该会考虑不在ROS下运行，然后保存。我看到过一篇博文，博主说能保存到15fps，不过看他是在windows下实现的，没有细看，有兴趣的朋友可以找找看。