奥比中光 astra 乐视三合一体感摄像头采集深度图彩色图并保存

本文参考的文章：

ROS下开发

前提是需要提前配置好相机在ROS中的运行环境Ubuntu18.04+ROS+ 乐视三合一深度相机配置使用，然后通过程序对终端中发布的深度图节点和彩色图节点进行采集并保存。

运行ROS节点

配置好环境后，通过CTRL+ALT+T打开一个终端，并运行相机节点：

roslaunch astra_camera astrapro.launch

查看相机的话题及画面

通过CTRL+ALT+T打开一个终端,输入：

rqt_image_view

从图中可查看深度相机、彩色相机以及红外相机的节点，为接下来采集和保存深度和彩色图片作准备。

订阅画面并保存

相机话题启动，运行下方的程序，可实现自动采集深度图和彩色图，需要提前建立好image和depth文件夹，以防找不到地址。

#!/usr/bin/env python3   #不同的python环境，可修改编辑器地址
# -*- coding: utf-8 -*- 

import rospy
from geometry_msgs.msg import PointStamped
from nav_msgs.msg import Path
from sensor_msgs.msg import Image
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge

class save_image():
    def __init__(self):
        self.count = 0
        self.cvbridge = CvBridge()
    
    def message(self, data):
        print(data.encoding)

    
    def save_image(self, data):
        image = self.cvbridge.imgmsg_to_cv2(data,  desired_encoding='rgb8') #彩色图编码格式：rgb8
        image = cv.cvtColor(image,cv.COLOR_BGR2RGB)
        image = image[144:336]

        if self.count < 10:
            name = '00000{}'.format(self.count)      
        elif self.count < 100 and self.count >= 10:
            name = '0000{}'.format(self.count)      
        elif self.count < 1000 and self.count >= 100:
            name = '00{}'.format(self.count)      
        elif self.count < 10000 and self.count >= 1000:
            name = '0{}'.format(self.count)      
        elif self.count < 100000 and self.count >= 10000:
            name = '{}'.format(self.count)    
        else:
            name = '0000000000000'
        
        cv.imwrite('./scpict/image/{}.jpg'.format(name), image) #更改为自己彩色图的保存图片路径
        
        print('image:  {}'.format(name))

        self.count += 1

    
    def save_depth(self, data):
        depth = self.cvbridge.imgmsg_to_cv2(data,  desired_encoding='32FC1')#深度图编码格式：16UC1或32FC1
        depth = depth[144:336]

        if self.count < 10:
            name = '00000{}'.format(self.count)      
        elif self.count < 100 and self.count >= 10:
            name = '0000{}'.format(self.count)      
        elif self.count < 1000 and self.count >= 100:
            name = '000{}'.format(self.count)      
        elif self.count < 10000 and self.count >= 1000:
            name = '00{}'.format(self.count)      
        elif self.count < 100000 and self.count >= 10000:
            name = '0{}'.format(self.count)    
        else:
            name = '0000000000000'
        
        cv.imwrite('./scpict/depth/{}.png'.format(name), depth)#更改为自己深度图的保存图片路径

        print('depth:  {}'.format(name))


'''-------------define main----------------'''
if __name__ == '__main__':
    try:
        a = save_image()
        rospy.init_node('save_image', anonymous = True)
        rospy.Subscriber("/camera/rgb/image_raw",  Image,  a.save_image)      #上边查看的节点彩色图的订阅话题
        rospy.Subscriber("/camera/depth/image_raw", Image,  a.save_depth)  #上边查看的节点深度图的订阅话题
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

Python环境下开发

首先需要对linux系统下的环境进行配置，可参考Ubuntu下使用Python调用乐视三合一摄像头,需要安装opencv，numpy，openni2三个python的第三方库，用于程序的调用。

调用摄像头并保存采集画面

通过调用时间函数time，对采集到的画面进行分文件夹保存。

from openni import openni2
import numpy as np
import cv2, os, time
from imageio import imsave


def mousecallback(event,x,y,flags,param):
     if event==cv2.EVENT_LBUTTONDBLCLK:
         print(y, x, dpt[y,x])


if __name__ == "__main__": 

    openni2.initialize()

    dev = openni2.Device.open_any()
    print(dev.get_device_info())
    dev.set_depth_color_sync_enabled(True)
    # 按照日期创建文件夹
    save_path = os.path.join(os.getcwd(), "realsense/realsense/out", time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime()))
    os.mkdir(save_path)

    depth_stream = dev.create_depth_stream()
    depth_stream.start()

    cap = cv2.VideoCapture(1)
    cv2.namedWindow('depth')
    cv2.setMouseCallback('depth',mousecallback)

    saved_count = 0

    while True:

        frame = depth_stream.read_frame()
        dframe_data = np.array(frame.get_buffer_as_triplet()).reshape([480, 640, 2])
        dpt1 = np.asarray(dframe_data[:, :, 0], dtype='float32')
        dpt2 = np.asarray(dframe_data[:, :, 1], dtype='float32')
        
        dpt2 *= 255
        dpt = dpt1 + dpt2
        dpt = dpt[:, ::-1]
        
        cv2.imshow('depth', dpt)

        ret,frame = cap.read()
        cv2.imshow('color', frame)

        key = cv2.waitKey(30)
        if key & 0xFF == ord('s'):
            imsave(os.path.join((save_path), "{:04d}r.png".format(saved_count)), frame)  # 保存RGB为png文件
            imsave(os.path.join((save_path), "{:04d}d.tiff".format(saved_count)), dpt)  # 保存深度图为tiff文件
            saved_count+=1
            cv2.imshow('save_dpt', dpt)
            cv2.imshow('save_color', frame)
            print("save ok " + "{:04d}r.".format(saved_count))
        if int(key) == ord('q'):
            break

    depth_stream.stop()
    dev.close()

程序运行后，鼠标放在图片上，每按一下s键，就自动保存一组深度和彩色图。

C++环境下开发

自己做实验中发现乐视三和一体感摄像头在ROS和Python开发过程中，并没有对深度相机和彩色相机进行配准，导致在采集到的画面中，深度图和彩色图子像素点是不匹配的。并且发现这款相机由于硬件原因，在python代码中，无法实现深度图和彩色图的配准。

C++环境配置

首先需要在自己环境中安装opencv，cmke的环境依赖，可参考全网最详细 Opencv + OpenNi + 奥比中光(Orbbec) Astra Pro /乐视三合一体感摄像头LeTMC-520 + linux 环境搭建来对现有的环境进行配置。

代码编译

本文在C++部分参考奥比中光 Astra Pro 一代（MX400）RGBD 摄像头 彩色RGB及深度采集，并在此文源代码进行修改，通过键盘按一下来保存一组深度、彩色和融合图像，并对保存的深度图和彩色图不匹配进行调整。同时，本文程序可直接输出深度图，不再通过生成xml文件再进行转化。

采集和保存程序如下所示：

#include <opencv2/opencv.hpp>
#include <sstream>
#include <fstream>
#include <stdlib.h>
#include <iostream>
#include <string>
#include "/home/lts/OpenNI-Linux-x64-2.3.0.66/Include/OpenNI.h"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include<sys/stat.h>
#include<sys/types.h>

using namespace std;
using namespace cv;
using namespace openni;
 
void writeMatToXML(const std::string xmlName, const cv::Mat & mat)
{
    FileStorage fs(xmlName, FileStorage::WRITE);
    fs << "Mat" << mat;
    fs.release();
};

void CheckOpenNIError( Status result, string status )
{ 
	if( result != STATUS_OK ) 
		cerr << status << " Error: " << OpenNI::getExtendedError() << endl;
};
 
class ScenceNumber
{
    // use a file(number.txt) to recoder different scence
    public:

        int scNum;
        string fileName = "./data/number.txt";
        ScenceNumber()
        {
            ifstream f;
            f.open(this->fileName);
            if(f.good())
            {
                stringstream cvStr;
                string tmpStr;
                getline(f, tmpStr);
                cvStr << tmpStr;
                cvStr >> this->scNum;
                f.close();
            }
            else
            {
                ofstream f(this->fileName);
                f << "0" << endl;
                this->scNum = 0;
                f.close();
            }
        }
        string getNum()
        {
            ofstream f(this->fileName);
            stringstream cvStr;
            string tmpStr;
            this->scNum ++;
            cvStr << this->scNum;
            cvStr >> tmpStr;
            f << tmpStr << endl;
            f.close();
            cvStr >> tmpStr;
            return tmpStr;

        }
};

int main( int argc, char** argv )
{
	Status result = STATUS_OK;  
    ScenceNumber ScN;
    string baseFilePath = "./data";
    string filePath;
	VideoFrameRef oniDepthImg;
 
	//OpenCV image
	cv::Mat cvDepthImg;
	cv::Mat cvBGRImg;
	cv::Mat cvFusionImg;
	
	cv::namedWindow("depth");
	cv::namedWindow("image");
	cv::namedWindow("fusion");
	char key=0;

	// initialize OpenNI2
    result = OpenNI::initialize();
	CheckOpenNIError( result, "initialize context" );  
 
	// open device  
	Device device;
    result = device.open( openni::ANY_DEVICE );

	// create depth stream 
    VideoStream oniDepthStream;
    result = oniDepthStream.create( device, openni::SENSOR_DEPTH );

	// set depth video mode
    VideoMode modeDepth;
    modeDepth.setResolution( 640, 480 );
    modeDepth.setFps( 30 );
    modeDepth.setPixelFormat( PIXEL_FORMAT_DEPTH_1_MM );
    oniDepthStream.setVideoMode(modeDepth);
	// start depth stream
    result = oniDepthStream.start();
 
	//create color stream
    VideoCapture capture;
    capture.open(2);
    capture.set(3, 640); //set the rgb size
    capture.set(4, 480);

	//set depth and color imge registration mode
	if( device.isImageRegistrationModeSupported(IMAGE_REGISTRATION_DEPTH_TO_COLOR ) )
	{
	device.setImageRegistrationMode( IMAGE_REGISTRATION_DEPTH_TO_COLOR );
	}

    long numInSc;
    numInSc = 0;
    filePath = baseFilePath + "/count" + ScN.getNum();
	while( key!=27 ) 
	{
		// read frame
		if( oniDepthStream.readFrame( &oniDepthImg ) == STATUS_OK )
		{
            capture >> cvBGRImg;
			cv::Mat cvRawImg16U( oniDepthImg.getHeight(), oniDepthImg.getWidth(), CV_16UC1, (void*)oniDepthImg.getData() );
			cvRawImg16U.convertTo(cvDepthImg, CV_8U, 255.0/(oniDepthStream.getMaxPixelValue()));
            cvRawImg16U.convertTo(cvRawImg16U, CV_32F);
            cv::flip(cvDepthImg, cvDepthImg, 1);
            cv::flip(cvRawImg16U, cvRawImg16U,1);

			// convert depth image GRAY to BGR
			cv::cvtColor(cvDepthImg,cvFusionImg,COLOR_GRAY2BGR);
			cv::imshow( "depth", cvDepthImg );
            cv::imshow( "image", cvBGRImg );

            cv::addWeighted(cvBGRImg,0.5,cvFusionImg,0.5,0,cvFusionImg);
		    cv::imshow( "fusion", cvFusionImg );

            if (key == 's')
            {  
                mkdir(filePath.c_str(), 0777);
                stringstream cvt;
                string SNumInSc;
                cvt << numInSc;
                cvt >> SNumInSc;
                cv::imwrite(filePath + "/" + SNumInSc + ".tiff", cvRawImg16U);
                cv::imwrite(filePath + "/" + SNumInSc + ".png", cvBGRImg);
                cv::imwrite(filePath + "/" + SNumInSc + ".jpg", cvFusionImg);
                cout << numInSc << "  saved" << endl;
                cout << filePath << endl;  
                numInSc ++;
            } 
		}
		key = cv::waitKey(100);
	}
    //cv destroy
	cv::destroyWindow("depth");
	cv::destroyWindow("image");
	cv::destroyWindow("fusion");

    //OpenNI2 destroy
    oniDepthStream.destroy();
    capture.release();
    device.close();
    OpenNI::shutdown();

	return 0;
}

CMake文件如下：

# cmake needs this line
cmake_minimum_required(VERSION 3.1)
 
# Enable C++11
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED TRUE)
 
# Define project name
project(data_capture)
 
# Find OpenCV, you may need to set OpenCV_DIR variable
# to the absolute path to the directory containing OpenCVConfig.cmake file
# via the command line or GUI
INCLUDE_DIRECTORIES($ENV{OPENNI2_INCLUDE})
link_directories($ENV{OPENNI2_REDIST})
find_package(OpenCV REQUIRED)
 
# If the package has been found, several variables will
# be set, you can find the full list with descriptions
# in the OpenCVConfig.cmake file.
# Print some message showing some of them
message(STATUS "OpenCV library status:")
message(STATUS "    config: ${OpenCV_DIR}")
message(STATUS "    version: ${OpenCV_VERSION}")
message(STATUS "    libraries: ${OpenCV_LIBS}")
message(STATUS "    include path: ${OpenCV_INCLUDE_DIRS}")
message(STATUS "    include path: $ENV{OPENNI2_INCLUDE}")
 
# Declare the executable target built from your sources
add_executable(data_capture test.cpp)
 
# Link your application with OpenCV libraries
target_link_libraries(data_capture LINK_PRIVATE ${OpenCV_LIBS} libOpenNI2.so)

上面程序中device.setImageRegistrationMode(IMAGE_REGISTRATION_DEPTH_TO_COLOR);就是配准操作。