奥比中光(Astra S)开发(两个相机分别获得点云和RGB图像)

对Astra S深度相机的操作又进行封装一次,工程文件和第三方库文件在我的资源中有下载

工程包含目录为:

//按照自己的路径进行更改
D:\Program Files\ThirdParty\OpenNI2\Include
D:\Program Files\ThirdParty\OpenCV241\Include
D:\Program Files\ThirdParty\Eigen3.2.5

库目录为:

//按照自己的路径进行更改
D:\Program Files\ThirdParty\OpenNI2\Lib
D:\Program Files\ThirdParty\OpenCV241\Lib

附加依赖项

glut32.lib
OpenNI2.lib
opencv_calib3d2410.lib
opencv_core2410.lib
opencv_highgui2410.lib

.h文件

#include 
#include 
#include 
#include "opencv/highgui.h"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include 
#include 

#define SAMPLE_READ_WAIT_TIMEOUT 2000 //2000ms

using namespace std;
using namespace cv;
using namespace openni;


typedef struct
{
	/* Red value of this pixel. */
	uint8_t b;
	/* Green value of this pixel. */
	uint8_t g;
	/* Blue value of this pixel. */
	uint8_t r;
} OniBGR888Pixel;

class CameraOperate
{

private:
	openni::Device device1;
	openni::Device device2;
	VideoStream depth1, color1;
	openni::VideoFrameRef m_depth_frame1;
	openni::VideoFrameRef m_RGB_frame1;

	openni::VideoFrameRef m_depth_frame2;
	openni::VideoFrameRef m_RGB_frame2;

	openni::VideoFrameRef frame1;
	
	VideoStream depth2, color2;
	openni::VideoFrameRef frame2;

	int readyStream ;
	float vertex[1024][1280][3];
	int mHeight ;
	int mWidth ;

public:
	CameraOperate();
	~CameraOperate();

	int InitializeDevice();                                              //传感器的初始化函数
	int OpenDevice(const char* uri,int Num);                                     //打开传感器函数
	int CloseDevice(int Num);                                                   //关闭传感器函数
	int CreateRGBStream(int Num);                                               //创建RGB_IR流
	int CreateDepthStream(int Num);                                             //创建Depth流
	int StartRGBStream(int Num);                                                //开始RGB_IR流
	int SatrtDepthStream(int Num);                                              //开始Depth流
	int StopRGBStream(int Num);                                                 //暂停RGB_IR流
	int StopDepthStream(int Num);                                               //暂停Depth流
	int DestoryRGBStream();                                              //暂停和销毁RGB_IR流
	int DestoryDepthStream(int Num);                                            //暂停和销毁Depth流
	int ReadRGBDepth_Frame(Mat& color, Mat& depth, int Num);                     //读取RGB、IR和Depth


	void ConvertProjectiveToWorld(int u, int v, int z, float& px, float& py, float& pz);
	void TransformPointToPoint(float dst[3], const float src[3]);
	void ConvertWorldToProjective(float x, float y, float z, float& u, float& v);
	int FrameToDepth(const VideoFrameRef& frame, Mat& depth);
	void MappingDepth2Color(cv::Mat &src, cv::Mat &dst);
	int MatToPointCloud(Mat depthMap, Mat colorMap);
	int SavePointCloud(const char* fileName);
	int FrameToRGB(const VideoFrameRef &frame, Mat &color);
	
};

.cpp文件

#include "CCameraOperate.h"

//构造函数
 CameraOperate::CameraOperate()
{
	 readyStream = -1;
	 mHeight = 0;
	 mWidth = 0;
}

 //析构函数
 CameraOperate::~CameraOperate()
 {
 
 }

 //初始化函数
 int CameraOperate::InitializeDevice()
 {
	 openni::Status rc = openni::STATUS_OK;
	 rc = openni::OpenNI::initialize();
	 if (rc != openni::STATUS_OK)
	 {
		 printf("Initialize failed\n%s\n", openni::OpenNI::getExtendedError());
		 return 1;
	 }
	 return 0;
 
 }

 //开启设备
 int CameraOperate::OpenDevice(const char* uri,int Num)
 {
	 if (Num == 1)
	 {
		 openni::Status rc = openni::STATUS_OK;
		 rc = device1.open(uri);
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't open device1\n%s\n", openni::OpenNI::getExtendedError());
			 return 2;
		 }
	 }
	 else
	 {
		 openni::Status rc = openni::STATUS_OK;
		 rc = device2.open(uri);
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't open device2\n%s\n", openni::OpenNI::getExtendedError());
			 return 2;
		 }
	 }
	 return 0;
 }

 //关闭设备
 int  CameraOperate::CloseDevice(int Num)
 {
	 if (Num == 1)
	 {
		 device1.close();
	 }
	 else
	 {
		 device2.close();
	 }
		 return 0;
 }

//创建RGB_IR流
 int CameraOperate::CreateRGBStream(int Num)
 {
	 if (Num == 1)
	 {
		 openni::Status rc = openni::STATUS_OK;
		 rc = color1.create(device1, openni::SENSOR_COLOR);
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't Create RGB Stream\n%s\n", openni::OpenNI::getExtendedError());
			 return 3;
		 }
	 }
	 else
	 {
		 openni::Status rc = openni::STATUS_OK;
		 rc = color2.create(device2, openni::SENSOR_COLOR);
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't Create RGB Stream\n%s\n", openni::OpenNI::getExtendedError());
			 return 3;
		 }
	 }
	 return 0;
 }

 int CameraOperate::CreateDepthStream(int Num)
 {
	 if (Num == 1)
	 {

		 openni::Status rc = openni::STATUS_OK;
		 rc = depth1.create(device1, openni::SENSOR_DEPTH);
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't Create Depth Stream\n%s\n", openni::OpenNI::getExtendedError());
			 return 4;
		 }
	 }
	 else
	 {
		 openni::Status rc = openni::STATUS_OK;
		 rc = depth2.create(device2, openni::SENSOR_DEPTH);
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't Create Depth Stream\n%s\n", openni::OpenNI::getExtendedError());
			 return 4;
		 }
	 }
	 return 0;
 }
	
int CameraOperate::StartRGBStream(int Num)
	 {
		 if (Num==1)
		 {
			 openni::Status rc = color1.start();
			 if (rc != openni::STATUS_OK)
			 {
				 printf("Couldn't start the rgb stream\n%s\n", openni::OpenNI::getExtendedError());
				 return 5;
			 }
		 }
		 else
		 {

			 openni::Status rc = color2.start();
			 if (rc != openni::STATUS_OK)
			 {
				 printf("Couldn't start the rgb stream\n%s\n", openni::OpenNI::getExtendedError());
				 return 5;
			 }
		 }
		 return 0;
 }

 int CameraOperate::SatrtDepthStream(int Num)
 {
	 if (Num == 1)
	 {
		 openni::Status rc = openni::STATUS_OK;
		 if (!depth1.isValid())
		 {
			 int ba = 0;
			 return -1;
		 }
		 rc = depth1.start();
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't start the depth stream\n%s\n", openni::OpenNI::getExtendedError());
			 return 6;
		 }
	 }
	 else
	 {
		 openni::Status rc = openni::STATUS_OK;
		 if (!depth2.isValid())
		 {
			 int ba = 0;
			 return -1;
		 }
		 rc = depth2.start();
		 if (rc != openni::STATUS_OK)
		 {
			 printf("Couldn't start the depth stream\n%s\n", openni::OpenNI::getExtendedError());
			 return 6;
		 }
	 }
	 return 0;
 }

//停止RGBStream
 int CameraOperate::StopRGBStream(int Num)
 {
	 if (Num == 1)
	 {
		 color1.stop();
	 }
	 else
	 {
		 color2.stop();
	 }
		 return 0;
	 
 }

 //停止DepthStream
 int CameraOperate::StopDepthStream(int Num)
 {
	 if (Num == 1)
	 {
		 if (!depth1.isValid())
		 {
			 int ba = 0;
			 return -1;
		 }
		 depth1.stop();
	 }
	 else
	 {
		 if (!depth2.isValid())
		 {
			 int ba = 0;
			 return -1;
		 }
		 depth2.stop();
	 }

	 return 0;

 }

 //销毁RGBStream
 int CameraOperate::DestoryRGBStream()
 {
	 color1.destroy();
	 return 0;
 }
 
 //销毁DepthStream
 int CameraOperate::DestoryDepthStream(int Num)
 {
	 if (Num == 1)
	 {
		 depth1.destroy();
	 }
	 else
	 {
		 depth2.destroy();
	 }
	 return 0;
 }

//读取RGB、Depth一帧的数据并转换成为Mat形式的Color和Depth矩阵
 int CameraOperate::ReadRGBDepth_Frame(Mat& BGRcolor1, Mat& depth3,int Num)
 {
	
	 if (Num==1)
	 {
		 openni::Status rc = openni::STATUS_OK;
		 Mat  cvRGBImg1(480, 640, CV_8UC3);
		 VideoStream* streams_Depth1 =&depth1 ;
		 int readyStream_Depth1 = -1;
		 rc = OpenNI::waitForAnyStream(&streams_Depth1, 1, &readyStream_Depth1, SAMPLE_READ_WAIT_TIMEOUT);
		 if (rc == STATUS_OK)
		 {
			 depth1.readFrame(&m_depth_frame1);
			 //color1.readFrame(&frame1);

			 if (m_depth_frame1.isValid())
			 {
				 FrameToDepth(m_depth_frame1, depth3);
				
			 }

		 }


		 VideoStream* streams_RGB1 = &color1;
		 int readyStream_RGB1 = -1;
		 rc = OpenNI::waitForAnyStream(&streams_RGB1, 1, &readyStream_RGB1, SAMPLE_READ_WAIT_TIMEOUT);
		 if (rc == STATUS_OK)
		 {
			 color1.readFrame(&m_RGB_frame1);
			 //color1.readFrame(&frame1);

			 if (m_RGB_frame1.isValid())
			 {
				 
				 FrameToRGB(m_RGB_frame1, BGRcolor1);
			 }

		 }



	 }
	 else
	 {
		 openni::Status rc = openni::STATUS_OK;
		 Mat  cvRGBImg2(480, 640, CV_8UC3);
		 VideoStream* streams_Depth2 = &depth2;
		 int readyStream_Depth2 = -1;
		 rc = OpenNI::waitForAnyStream(&streams_Depth2, 1, &readyStream_Depth2, SAMPLE_READ_WAIT_TIMEOUT);
		 if (rc == STATUS_OK)
		 {
			 depth2.readFrame(&m_depth_frame2);
			 //color1.readFrame(&frame1);

			 if (m_depth_frame2.isValid())
			 {
				 FrameToDepth(m_depth_frame2, depth3);

			 }

		 }

		 VideoStream* streams_RGB2 = &color2;
		 int readyStream_RGB2 = -1;
		 rc = OpenNI::waitForAnyStream(&streams_RGB2, 1, &readyStream_RGB2, SAMPLE_READ_WAIT_TIMEOUT);
		 if (rc == STATUS_OK)
		 {
			 color2.readFrame(&m_RGB_frame2);
			
			 if (m_RGB_frame2.isValid())
			 {

				 FrameToRGB(m_RGB_frame2, BGRcolor1);
			 }

		 }
	 }
	 return 0;
 }

 void CameraOperate::ConvertProjectiveToWorld(int u, int v, int z, float& px, float& py, float& pz)
 {

	 float ifx, ify;
	 ifx = 1. / 550.31324;
	 ify = 1. / 530.95258;

	 float tx = (u - 308.68595) * ifx;
	 float ty = (v - 239.44117) * ify;
	 float x0 = tx;
	 float y0 = ty;

	for (int j = 0; j < 5; j++)
	{
		double r2 = tx * tx + ty * ty;
		double icdist = (1) / (1 + ((0.00000000 * r2 + 0.31352)*r2 - 0.07364)*r2);
		double deltaX = 2 * (-0.01153) * tx*ty + (-0.00525) * (r2 + 2 * tx*tx);
		double deltaY = (-0.01153) * (r2 + 2 * ty*ty) + 2 * (-0.00525) * tx*ty;
		tx = (x0 - deltaX)*icdist;
		ty = (y0 - deltaY)*icdist;
	}
	 
	 px = z * tx;
	 py = z * ty;
	 pz = z;
 }

 void CameraOperate::TransformPointToPoint(float dst[3], const float src[3])
 {
	 dst[0] = 0.99990161 * src[0] + -0.00319713 * src[1] + -0.01365809 * src[2] + 0.02504946;
	 dst[1] = 0.00327611 * src[0] + 0.99997802 * src[1] + 0.00576428 * src[2] + 0.00006017;
	 dst[2] = 0.01363936 * src[0] + -0.00580846 * src[1] + 0.99989011 * src[2] + -0.00260149;
 }

 void CameraOperate::ConvertWorldToProjective(float x, float y, float z, float& u, float& v)
 {
	
	 float tx = x / z;
	float ty = y / z;
	float r2 = tx*tx + ty*ty;
	float f = 1 + -0.01595141 * r2 + 0.02629677 * r2*r2 + 0.00000000 * r2*r2*r2;
	tx *= f;
	ty *= f;
	float dx = tx + 2 * 0.00000000 * tx*ty + 0.00000000 * (r2 + 2 * tx*tx);
	float dy = ty + 2 * 0.00000000 * tx*ty + 0.00000000 * (r2 + 2 * ty*ty);
	tx = dx;
	ty = dy;
	u = tx * 524.18651391 + 320.79175117;
	v = ty * 486.50458757 + 240.99268524;
 }

 int CameraOperate::FrameToDepth(const VideoFrameRef& frame, Mat& depth)
 {

	 uint16_t *pPixel;
	 for (int y = 0; y < 480; y++)
	 {
		 pPixel = ((uint16_t*)((char*)frame.getData() + ((int)(y)* frame.getStrideInBytes())));
		 uint16_t* data = (uint16_t*)depth.ptr(y);
		 for (int x = 0; x < 640; x++)
		 {
			 *data++ = (*pPixel);
			 pPixel++;
		 }
	 }
	 return 0;
 }

 void CameraOperate::MappingDepth2Color(cv::Mat &src, cv::Mat &dst)
 {
	 double  z;
	 float pixel[2];
	 float point[3], to_point[3];
	 uint16_t u, v, d;
	 uint16_t u_rgb = 0, v_rgb = 0;
	 cv::Mat newdepth(dst.rows, dst.cols, CV_16UC1, cv::Scalar(0));
	 for (v = 0; v < src.rows; v++)
	 {
		 for (u = 0; u < src.cols; u++)
		 {
			 d = src.at(v, u);
			 z = (double)d;

			 ConvertProjectiveToWorld(u, v, z, point[0], point[1], point[2]);
			 TransformPointToPoint(to_point, point);
			 ConvertWorldToProjective(to_point[0], to_point[1], to_point[2], pixel[0], pixel[1]);

			 u_rgb = (uint16_t)(pixel[0]);
			 v_rgb = (uint16_t)(pixel[1]);

			 if (u_rgb < 0 || u_rgb >= newdepth.cols || v_rgb < 0 || v_rgb >= newdepth.rows) continue;
			 uint16_t *val = (uint16_t *)newdepth.ptr(v_rgb)+u_rgb;
			 *val = d;
		 }
	 }

	 dst = newdepth;
 }

 int CameraOperate::MatToPointCloud(Mat depthMap, Mat colorMap)
 {
	 int depthW = depthMap.cols;
	 int depthH = depthMap.rows;
	 mWidth = depthW;
	 mHeight = depthH;

	 // update vertex
#pragma omp parallel for
	 for (int v = 0; v < depthH; v++)
	 {
		 unsigned short* pDepth = (unsigned short*)depthMap.ptr(v);

		 for (int u = 0; u < depthW; u++)
		 {
			 int z = *pDepth++;
			 float px = 0;
			 float py = 0;
			 float pz = 0;
			 ConvertProjectiveToWorld(u, v, z, px, py, pz);            //调用ConvertProjectiveToWorld函数把深度转换成世界坐标系
			 vertex[v][u][0] = px;
			 vertex[v][u][1] = py;
			 vertex[v][u][2] = (float)z;
		 }
	 }

	 return 0;

 }

 int CameraOperate::SavePointCloud(const char* fileName)
 {

	 ofstream ouF;
	 ouF.open(fileName, ofstream::out);
	 if (!ouF)
	 {
		 cerr << "failed to open the file : " << fileName << endl;
		 return -1;
	 }
	 /*
	 ouF << "ply" << std::endl;
	 ouF << "format ascii 1.0" << std::endl;
	 ouF << "comment made by Orbbec " << std::endl;
	 ouF << "comment Orbbec Co.,Ltd." << std::endl;
	 ouF << "element vertex " << mWidth * mHeight << std::endl;
	 ouF << "property float32 x" << std::endl;
	 ouF << "property float32 y" << std::endl;
	 ouF << "property float32 z" << std::endl;
	 ouF << "property uint8 red" << std::endl;
	 ouF << "property uint8 green" << std::endl;
	 ouF << "property uint8 blue" << std::endl;
	 ouF << "element face 0" << std::endl;
	 ouF << "property list uint8 int32 vertex_index" << std::endl;
	 ouF << "end_header" << std::endl;
	 */

	 for (int v = 0; v < mHeight; v++)
	 {
		 for (int u = 0; u < mWidth; u++)
		 {
			 float x = vertex[v][u][0];
			 float y = vertex[v][u][1];
			 float z = vertex[v][u][2];

			 ouF << x << " " << y << " " << z << std::endl;
			 //ouF << x << " " << y << " " << z << " " << r << " " << g << " " << b << std::endl;
		 }
	 }

	 ouF.close();

	 return 0;
 }

 int CameraOperate::FrameToRGB(const VideoFrameRef &frame, Mat &color)
 {

	 double resizeFactor = std::min((640 / (double)frame.getWidth()), (480 / (double)frame.getHeight()));
	 unsigned int texture_x = (unsigned int)(640 - (resizeFactor * frame.getWidth())) / 2;
	 unsigned int texture_y = (unsigned int)(480 - (resizeFactor * frame.getHeight())) / 2;
	 OniBGR888Pixel ss; 
	 for (unsigned int y = 0; y < (480 - 2 * texture_y); ++y)
	 {
		 uint8_t* data = (uint8_t*)color.ptr(y);
		
		 for (unsigned int x = 0; x < (640 - 2 * texture_x); ++x)
		 {
			 OniRGB888Pixel* streamPixel = (OniRGB888Pixel*)((char*)frame.getData() + ((int)(y / resizeFactor) * frame.getStrideInBytes())) + (int)(x / resizeFactor);
			
			 ss.b = streamPixel->b;
			 ss.g = streamPixel->g;
			 ss.r = streamPixel->r;
			 memcpy(data, &ss, 3);
			 data = data + 3;
		 }
	 }
	
	 return 0;
 }

主函数

/*****************************************************************************
*                                                                            *
*  OpenNI 2.x Alpha                                                          *
*  Copyright (C) 2012 PrimeSense Ltd.                                        *
*                                                                            *
*  This file is part of OpenNI.                                              *
*                                                                            *
*  Licensed under the Apache License, Version 2.0 (the "License");           *
*  you may not use this file except in compliance with the License.          *
*  You may obtain a copy of the License at                                   *
*                                                                            *
*      http://www.apache.org/licenses/LICENSE-2.0                            *
*                                                                            *
*  Unless required by applicable law or agreed to in writing, software       *
*  distributed under the License is distributed on an "AS IS" BASIS,         *
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  *
*  See the License for the specific language governing permissions and       *
*  limitations under the License.                                            *
*                                                                            *
*****************************************************************************/
#include 
#include 
#include 
#include "opencv/highgui.h"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include 
#include 
#include "CCameraOperate.h"

#define SAMPLE_READ_WAIT_TIMEOUT 2000 //2000ms
#define REC_WIDTH   640;
#define REC_HEIGHT  480;
#define CONFIG_INI          "./config.ini"
#define CAMERA_PARAMS_INI   "./camera_params.ini"

using namespace openni;
using namespace std;
using namespace cv;


void WriteLog(double RunTime, double NotStartStream1, double WaitFailed1, double NotStartStream2, double WaitFailed2, double RunNum)
{
	SYSTEMTIME st;
	GetLocalTime(&st);
	FILE *fp;
	fp = fopen("log.txt", "at");
	fprintf(fp, "MyLogInfo: %d:%d:%d:%d\n ", st.wHour, st.wMinute, st.wSecond, st.wMilliseconds);
	fprintf(fp, "运行时间:%f\n", RunTime);
	fprintf(fp, "运行次数:%f \n", RunNum);
	fprintf(fp, "设备1流开始失败次数:%f  设备1等待失败次数:%f \n", NotStartStream1, WaitFailed1);
	fprintf(fp, "设备2流开始失败次数:%f  设备2等待失败次数:%f \n", NotStartStream2, WaitFailed2);
	fclose(fp);
}

int main(int argc, char** argv)
{
	openni::Status rc = openni::STATUS_OK;
	openni::Status rc1 = openni::STATUS_OK;
	openni::Status rc2 = openni::STATUS_OK;
	//传感器1的矩阵
	Mat     cvRGBImg1(480, 640, CV_8UC3);
	Mat     cvBGRImg1(480, 640, CV_8UC3);
	Mat     cvIRImg1(480, 640, CV_16UC1);
	Mat     cvDepthImg1(480, 640, CV_16UC1);
	Mat     CaliDepth1(480, 640, CV_16UC1);
	//传感器2的矩阵
	Mat     cvRGBImg2(480, 640, CV_8UC3);
	Mat     cvIRImg2(480, 640, CV_16UC1);
	Mat     cvDepthImg2(480, 640, CV_16UC1);
	Mat     CaliDepth2(480, 640, CV_16UC1);
	Mat     cvBGRImg2(480, 640, CV_8UC3);

	CameraOperate camera;
//	CameraOperate camera2;

	rc = openni::OpenNI::initialize();//初始化OpenNI环境
	if (rc != openni::STATUS_OK)
	{
		printf("%s: Initialize failed\n%s\n", argv[0], openni::OpenNI::getExtendedError());
		return 1;
	}

	openni::Array deviceList;
	openni::OpenNI::enumerateDevices(&deviceList);//获取到设备列表

	const char* device1Uri;//设备标识
	const char* device2Uri;

	switch (argc)
	{
	case 1:
		if (deviceList.getSize() < 2)
		{
			printf("Missing devices\n");
			openni::OpenNI::shutdown();
			return 1;
		}
		device1Uri = deviceList[1].getUri();
		device2Uri = deviceList[0].getUri();
		break;
	case 2:
		if (deviceList.getSize() < 1)
		{
			printf("Missing devices\n");
			openni::OpenNI::shutdown();
			return 1;
		}
		device1Uri = argv[1];
		if (strcmp(deviceList[0].getUri(), device1Uri) != 0)
			device2Uri = deviceList[0].getUri();
		else
			device2Uri = deviceList[1].getUri();
		break;
	default:
		device1Uri = argv[1];
		device2Uri = argv[2];
	}


	SYSTEMTIME sys;
	double Time0 = 0;
	double Time1 = 0;
	double Time11 = 0;
	double Time2 = 0;
	double Time22 = 0;
	double Time33 = 0;
	double Sum = 0;
	GetLocalTime(&sys);
	time_t t1;
	time_t t2;
	time(&t1);


	double NotStartStream1 = 0;                       //设备1不能打开流的次数
	double NotStartStream2 = 0;                       //设备2不能打开流的次数
	double WaitFailed1 = 0;                            //设备1等待超时的次数
	double WaitFailed2 = 0;                            //设备1等待超时的次数

	int OpenStatue1=camera.OpenDevice(device1Uri,1);
	int CreateDepthStatue1 = camera.CreateRGBStream(1);
	int CreateColorStatue1 = camera.CreateDepthStream(1);
	int StartRGBStatue1 = camera.StartRGBStream(1);

	if ((OpenStatue1 || CreateDepthStatue1 || CreateColorStatue1 || StartRGBStatue1)!=0)
	{
		printf("设备1初始化有问题!");
	}


	int OpenStatue2 = camera.OpenDevice(device2Uri,2);
	int CreateDepthStatue2 = camera.CreateRGBStream(2);
	int CreateColorStatue2 = camera.CreateDepthStream(2);
	int StartRGBStatue2 = camera.StartRGBStream(2);
	if ((OpenStatue2 || CreateDepthStatue2 || CreateColorStatue2 || StartRGBStatue2) != 0)
	{
		printf("设备2初始化有问题!");
	}
  
	while (1)
	{
		if (rc1 != openni::STATUS_OK)
		{
			int OpenStatue1 = camera.OpenDevice(device1Uri, 1);
			int CreateColorStatue1 = camera.CreateDepthStream(1);
		}

		if (rc2 != openni::STATUS_OK)
		{
			int OpenStatue2 = camera.OpenDevice(device1Uri, 2);
			int CreateColorStatue2 = camera.CreateDepthStream(2);
		}
	
	   while (1)
		{

			GetLocalTime(&sys);                                                                          //得到系统时间
			Time0 = sys.wMinute * 60 * 1000 + sys.wSecond * 1000 + sys.wMilliseconds;

			/*******************设备1的启读取********************************************/

			int OpenDeviceStatue1 = camera.SatrtDepthStream(1);
			if (OpenDeviceStatue1 != 0)
			{
				printf("%s: Couldn't start depth1 stream %d on device %s\n%s\n", argv[0], openni::SENSOR_DEPTH, device1Uri, openni::OpenNI::getExtendedError());
				NotStartStream1++;
				rc1 = rc;
				camera.StopDepthStream(1);
				camera.DestoryDepthStream(1);
				camera.CloseDevice(1);
				break;                                                                                   //调到主循环外面重新启动设备1

			}

			int ReadFrameStatue1 = camera.ReadRGBDepth_Frame(cvBGRImg1, cvDepthImg1, 1);

			if (ReadFrameStatue1 == 0)
			{

				
				IplImage Image1 = IplImage(cvBGRImg1);
				cvSaveImage("Image1.jpg", &Image1);
				camera.MappingDepth2Color(cvDepthImg1, CaliDepth1);
				camera.MatToPointCloud(CaliDepth1, cvRGBImg1);
				camera.SavePointCloud("PointCloud1.txt");
				rc1 = openni::STATUS_OK;
			
			}

			int StopStatue1=camera.StopDepthStream(1);
			if (StopStatue1 != 0)
			{
				printf("Stop failed! (timeout is %d ms)\n%s\n", SAMPLE_READ_WAIT_TIMEOUT, OpenNI::getExtendedError());
			
			}
			//camera1.DestoryDepthStream(1);
			GetLocalTime(&sys);
			Time1 = sys.wMinute * 60 * 1000 + sys.wSecond * 1000 + sys.wMilliseconds;
			Time11 = Time1 - Time0;
			cout << "设备1花费时间为:" << Time11 << endl;

			/*******************设备2的启读取********************************************/
 

			int OpenDeviceStatue2 = camera.SatrtDepthStream(2);
			if (OpenDeviceStatue2 != 0)
			{
				printf("%s: Couldn't start depth2 stream %d on device %s\n%s\n", argv[0], openni::SENSOR_DEPTH, device1Uri, openni::OpenNI::getExtendedError());
				NotStartStream2++;
				DWORD dw = GetLastError();
				rc2 = rc;
				camera.StopDepthStream(2);
				camera.DestoryDepthStream(2);
				camera.CloseDevice(2);
				break;                                                                                   //调到主循环外面重新启动设备1

			}
		
			int ReadFrameStatue2 = camera.ReadRGBDepth_Frame(cvBGRImg2, cvDepthImg2, 2);

			if (ReadFrameStatue2 == 0)
			{
				IplImage Image2 = IplImage(cvBGRImg2);
				cvSaveImage("Image2.jpg", &Image2);
				camera.MappingDepth2Color(cvDepthImg2, CaliDepth2);
				camera.MatToPointCloud(CaliDepth2, cvRGBImg2);
				camera.SavePointCloud("PointCloud2.txt");
				rc2 = openni::STATUS_OK;

			}
			int StopStatue2 = camera.StopDepthStream(2);
			if (StopStatue2 != 0)
			{
				printf("Stop failed! (timeout is %d ms)\n%s\n", SAMPLE_READ_WAIT_TIMEOUT, OpenNI::getExtendedError());

			}

			GetLocalTime(&sys);
			Time2 = sys.wMinute * 60 * 1000 + sys.wSecond * 1000 + sys.wMilliseconds;
			Time22 = Time2 - Time1;
			cout << "设备2花费时间为:" << Time22 << endl;
			Time33 = Time2 - Time0;
			cout << "设备1,2花费时间为:" << Time33 << endl;
			cout << "运行次数" << Sum << endl;
			Sum++;
			time(&t2);
			cout << "运行时间为:" << t2 - t1 << endl;
			cout << endl;
			
			if ((int)Sum % 100 == 0)
			{
				WriteLog(t2 - t1, NotStartStream1, WaitFailed1, NotStartStream2, WaitFailed2, Sum);

		    }

	 }
  }

}

你可能感兴趣的:(C++,编程语言,Astra,S,奥比中光,深度相机,点云,RGB图像)