HRESULT KinectSensor::processColor()
{
	if (! (WAIT_OBJECT_0 == WaitForSingleObject(m_hNextColorFrameEvent, 0)) )	return S_FALSE;

	NUI_IMAGE_FRAME imageFrame;

	HRESULT hr = S_OK;
	hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pColorStreamHandle, 0, &imageFrame);
	if ( FAILED(hr) ) { return hr; }

	NUI_LOCKED_RECT LockedRect;
	hr = imageFrame.pFrameTexture->LockRect(0, &LockedRect, NULL, 0);
	if ( FAILED(hr) ) { return hr; }

	// loop over each row and column of the color
#pragma omp parallel for
	for (int yi = 0; yi < (int)getColorHeight(); ++yi) {
		LONG y = yi;

		LONG* pDest = ((LONG*)m_colorRGBX) + (int)getColorWidth() * y;
		for (LONG x = 0; x < (int)getColorWidth(); ++x) {
			// calculate index into depth array
			//int depthIndex = x/m_colorToDepthDivisor + y/m_colorToDepthDivisor * getDepthWidth();	//TODO x flip
			int depthIndex = (getDepthWidth() - 1 - x/m_colorToDepthDivisor) + y/m_colorToDepthDivisor * getDepthWidth();

			// retrieve the depth to color mapping for the current depth pixel
			LONG colorInDepthX = m_colorCoordinates[depthIndex * 2];
			LONG colorInDepthY = m_colorCoordinates[depthIndex * 2 + 1];

			// make sure the depth pixel maps to a valid point in color space
			if ( colorInDepthX >= 0 && colorInDepthX < (int)getColorWidth() && colorInDepthY >= 0 && colorInDepthY < (int)getColorHeight() ) {
				// calculate index into color array
				LONG colorIndex = colorInDepthY * (int)getColorWidth() + colorInDepthX;	//TODO x flip
				//LONG colorIndex = colorInDepthY * (int)getColorWidth() + (getColorWidth() - 1 - colorInDepthX);

				// set source for copy to the color pixel
				LONG* pSrc = ((LONG *)LockedRect.pBits) + colorIndex;					
				LONG tmp = *pSrc;

				tmp|=0xFF000000; // Flag for is valid

				*pDest = tmp;
			} else {
				*pDest = 0x00000000;
			}
			pDest++;
		}
	}


	hr = imageFrame.pFrameTexture->UnlockRect(0);
	if ( FAILED(hr) ) { return hr; };

	hr = m_pNuiSensor->NuiImageStreamReleaseFrame(m_pColorStreamHandle, &imageFrame);


	return hr;
}
HRESULT BinaryDumpReader::createFirstConnected()
{
	std::string filename = GlobalAppState::getInstance().s_BinaryDumpReaderSourceFile;
	std::cout << "Start loading binary dump" << std::endl;
	//BinaryDataStreamZLibFile inputStream(filename, false);
	BinaryDataStreamFile inputStream(filename, false);
	CalibratedSensorData sensorData;
	inputStream >> sensorData;
	std::cout << "Loading finished" << std::endl;
	std::cout << sensorData << std::endl;

	DepthSensor::init(sensorData.m_DepthImageWidth, sensorData.m_DepthImageHeight, std::max(sensorData.m_ColorImageWidth,1u), std::max(sensorData.m_ColorImageHeight,1u));
	mat4f intrinsics(sensorData.m_CalibrationDepth.m_Intrinsic);
	initializeIntrinsics(sensorData.m_CalibrationDepth.m_Intrinsic(0,0), sensorData.m_CalibrationDepth.m_Intrinsic(1,1), sensorData.m_CalibrationDepth.m_Intrinsic(0,2), sensorData.m_CalibrationDepth.m_Intrinsic(1,2));

	m_NumFrames = sensorData.m_DepthNumFrames;
	assert(sensorData.m_ColorNumFrames == sensorData.m_DepthNumFrames || sensorData.m_ColorNumFrames == 0);		
	releaseData();
	m_DepthD16Array = new USHORT*[m_NumFrames];
	for (unsigned int i = 0; i < m_NumFrames; i++) {
		m_DepthD16Array[i] = new USHORT[getDepthWidth()*getDepthHeight()];
		for (unsigned int k = 0; k < getDepthWidth()*getDepthHeight(); k++) {
			m_DepthD16Array[i][k] = (USHORT)(sensorData.m_DepthImages[i][k]*1000.0f + 0.5f);
		}
	}

	std::cout << "loading depth done" << std::endl;
	if (sensorData.m_ColorImages.size() > 0) {
		m_bHasColorData = true;
		m_ColorRGBXArray = new BYTE*[m_NumFrames];
		for (unsigned int i = 0; i < m_NumFrames; i++) {
			m_ColorRGBXArray[i] = new BYTE[getColorWidth()*getColorHeight()*getColorBytesPerPixel()];
			for (unsigned int k = 0; k < getColorWidth()*getColorHeight(); k++) {
				const BYTE* c = (BYTE*)&(sensorData.m_ColorImages[i][k]); 
				m_ColorRGBXArray[i][k*getColorBytesPerPixel()+0] = c[0];
				m_ColorRGBXArray[i][k*getColorBytesPerPixel()+1] = c[1];
				m_ColorRGBXArray[i][k*getColorBytesPerPixel()+2] = c[2];
				m_ColorRGBXArray[i][k*getColorBytesPerPixel()+3] = 255;
				//I don't know really why this has to be swapped...
			}
			//std::string outFile = "colorout//color" + std::to_string(i) + ".png";
			//ColorImageR8G8B8A8 image(getColorHeight(), getColorWidth(), (vec4uc*)m_ColorRGBXArray[i]);
			//FreeImageWrapper::saveImage(outFile, image);
		}
	} else {
		m_bHasColorData = false;
	}
	sensorData.deleteData();

	std::cout << "loading color done" << std::endl;


	return S_OK;
}
HRESULT BinaryDumpReader::processDepth()
{
	if(m_CurrFrame >= m_NumFrames)
	{
		GlobalAppState::get().s_playData = false;
		std::cout << "binary dump sequence complete - press space to run again" << std::endl;
		m_CurrFrame = 0;
	}

	if(GlobalAppState::get().s_playData) {

		float* depth = getDepthFloat();
		memcpy(depth, m_data.m_DepthImages[m_CurrFrame], sizeof(float)*getDepthWidth()*getDepthHeight());

		incrementRingbufIdx();

		if (m_bHasColorData) {
			memcpy(m_colorRGBX, m_data.m_ColorImages[m_CurrFrame], sizeof(vec4uc)*getColorWidth()*getColorHeight());
		}

		m_CurrFrame++;
		return S_OK;
	} else {
		return S_FALSE;
	}
}
Exemple #4
0
void RGBDSensor::computePointCurrentPointCloud(PointCloudf& pc, const mat4f& transform /*= mat4f::identity()*/) const
{
	if (!(getColorWidth() == getDepthWidth() && getColorHeight() == getDepthHeight()))	throw MLIB_EXCEPTION("invalid dimensions");

	for (unsigned int i = 0; i < getDepthWidth()*getDepthHeight(); i++) {
		unsigned int x = i % getDepthWidth();
		unsigned int y = i / getDepthWidth();
		vec3f p = depthToSkeleton(x,y);
		if (p.x != -std::numeric_limits<float>::infinity() && p.x != 0.0f)	{

			vec3f n = getNormal(x,y);
			if (n.x != -FLT_MAX) {
				pc.m_points.push_back(p);
				pc.m_normals.push_back(n);
				vec4uc c = m_colorRGBX[i];
				pc.m_colors.push_back(vec4f(c.z/255.0f, c.y/255.0f, c.x/255.0f, 1.0f));	//there's a swap... dunno why really
			}
		}
	}
	for (auto& p : pc.m_points) {
		p = transform * p;
	}
	mat4f invTranspose = transform.getInverse().getTranspose();
	for (auto& n : pc.m_normals) {
		n = invTranspose * n;
		n.normalize();
	}
}
Exemple #5
0
void RGBDSensor::recordFrame()
{
	m_recordedDepthData.push_back(m_depthFloat[m_currentRingBufIdx]);
	m_recordedColorData.push_back(m_colorRGBX);

	m_depthFloat[m_currentRingBufIdx] = new float[getDepthWidth()*getDepthHeight()];
	m_colorRGBX = new vec4uc[getColorWidth()*getColorHeight()];
}
HRESULT BinaryDumpReader::processDepth()
{
	if (m_CurrFrame < m_NumFrames) {
		std::cout << "curr Frame " << m_CurrFrame << std::endl;
		memcpy(m_depthD16, m_DepthD16Array[m_CurrFrame], sizeof(USHORT)*getDepthWidth()*getDepthHeight());
		
		if (m_bHasColorData) {
			memcpy(m_colorRGBX, m_ColorRGBXArray[m_CurrFrame], getColorBytesPerPixel()*getColorWidth()*getColorHeight());
		}

		m_CurrFrame++;
		return S_OK;
	} else {
		return S_FALSE;
	}
}
bool StructureSensor::processDepth()
{
	std::pair<float*,UCHAR*> frames = m_server.process(m_oldDepth, m_oldColor);
	if (frames.first == NULL || frames.second == NULL) return false;

	// depth
	memcpy(m_depthFloat[m_currentRingBufIdx], frames.first, sizeof(float)*getDepthWidth()*getDepthHeight());

	// color
	memcpy(m_colorRGBX, (vec4uc*)frames.second, sizeof(vec4uc)*getColorWidth()*getColorHeight());

	m_oldDepth = frames.first;
	m_oldColor = frames.second;

	return true;
}
bool SensorDataReader::processDepth()
{
	if (m_currFrame >= m_numFrames)
	{
		GlobalAppState::get().s_playData = false;
		//std::cout << "binary dump sequence complete - press space to run again" << std::endl;
		stopReceivingFrames();
		std::cout << "binary dump sequence complete - stopped receiving frames" << std::endl;
		m_currFrame = 0;
	}

	if (GlobalAppState::get().s_playData) {

		float* depth = getDepthFloat();

		//TODO check why the frame cache is not used?
		ml::SensorData::RGBDFrameCacheRead::FrameState frameState = m_sensorDataCache->getNext();
		//ml::SensorData::RGBDFrameCacheRead::FrameState frameState;
		//frameState.m_colorFrame = m_sensorData->decompressColorAlloc(m_currFrame);
		//frameState.m_depthFrame = m_sensorData->decompressDepthAlloc(m_currFrame);


		for (unsigned int i = 0; i < getDepthWidth()*getDepthHeight(); i++) {
			if (frameState.m_depthFrame[i] == 0) depth[i] = -std::numeric_limits<float>::infinity();
			else depth[i] = (float)frameState.m_depthFrame[i] / m_sensorData->m_depthShift;
		}

		incrementRingbufIdx();

		if (m_bHasColorData) {
			for (unsigned int i = 0; i < getColorWidth()*getColorHeight(); i++) {
				m_colorRGBX[i] = vec4uc(frameState.m_colorFrame[i]);
			}
		}
		frameState.free();

		m_currFrame++;
		return true;
	}
	else {
		return false;
	}
}
Exemple #9
0
void RGBDSensor::saveRecordedFramesToFile( const std::string& filename )
{
	if (m_recordedDepthData.size() == 0 || m_recordedColorData.size() == 0) return;

	CalibratedSensorData cs;
	cs.m_DepthImageWidth = getDepthWidth();
	cs.m_DepthImageHeight = getDepthHeight();
	cs.m_ColorImageWidth = getColorWidth();
	cs.m_ColorImageHeight = getColorHeight();
	cs.m_DepthNumFrames = (unsigned int)m_recordedDepthData.size();
	cs.m_ColorNumFrames = (unsigned int)m_recordedColorData.size();

	cs.m_CalibrationDepth.m_Intrinsic = getDepthIntrinsics();
	cs.m_CalibrationDepth.m_Extrinsic = getDepthExtrinsics();
	cs.m_CalibrationDepth.m_IntrinsicInverse = cs.m_CalibrationDepth.m_Intrinsic.getInverse();
	cs.m_CalibrationDepth.m_ExtrinsicInverse = cs.m_CalibrationDepth.m_Extrinsic.getInverse();

	cs.m_CalibrationColor.m_Intrinsic = getColorIntrinsics();
	cs.m_CalibrationColor.m_Extrinsic = getColorExtrinsics();
	cs.m_CalibrationColor.m_IntrinsicInverse = cs.m_CalibrationColor.m_Intrinsic.getInverse();
	cs.m_CalibrationColor.m_ExtrinsicInverse = cs.m_CalibrationColor.m_Extrinsic.getInverse();

	cs.m_DepthImages.resize(cs.m_DepthNumFrames);
	cs.m_ColorImages.resize(cs.m_ColorNumFrames);
	unsigned int dFrame = 0;
	for (auto& a : m_recordedDepthData) {
		cs.m_DepthImages[dFrame] = a;
		dFrame++;
	}
	unsigned int cFrame = 0;
	for (auto& a : m_recordedColorData) {
		cs.m_ColorImages[cFrame] = a;
		cFrame++;
	}

	cs.m_trajectory = m_recordedTrajectory;

	std::cout << cs << std::endl;
	std::cout << "dumping recorded frames... ";

	std::string actualFilename = filename;
	while (util::fileExists(actualFilename)) {
		std::string path = util::directoryFromPath(actualFilename);
		std::string curr = util::fileNameFromPath(actualFilename);
		std::string ext = util::getFileExtension(curr);
		curr = util::removeExtensions(curr);
		std::string base = util::getBaseBeforeNumericSuffix(curr);
		unsigned int num = util::getNumericSuffix(curr);
		if (num == (unsigned int)-1) {
			num = 0;
		}
		actualFilename = path + base + std::to_string(num+1) + "." + ext;
	}
	BinaryDataStreamFile outStream(actualFilename, true);
	//BinaryDataStreamZLibFile outStream(filename, true);
	outStream << cs;
	std::cout << "done" << std::endl;

	m_recordedDepthData.clear();
	m_recordedColorData.clear();	//destructor of cs frees all allocated data
	//m_recordedTrajectory.clear();
}