예제 #1
0
void display_points(){
	//画面のキャプチャ
	capture();

	//移動平均
	FIFOFilter();
	//除算->データ格納
	for(int i = 0; i < dPixels; i++)
	{
		aveDepthData[i] = (unsigned short)(Sum[i]*Kd); //移動加算値Sum[i]から移動平均値kを求める
	}

	//CameraSpacePointへ変換
	ERROR_CHECK2(coordinateMapper->MapDepthFrameToCameraSpace(
		width * height, (UINT16*)aveDepthData, 
	      width * height, cameraSpacePoints_ave),"MapDepthFrameToCameraSpace_ave");



	//**描画処理**//
	glEnable(GL_DEPTH_TEST);

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity(); //変換行列の初期化
	float cx = 0.0f;
	float cy = 0.0f;
	float cz = 1.0f;
	float x = r * cos(fai) * sin(theta) + cx;
	float y = r * sin(fai) + cy;
	float z = r * cos(fai) * cos(theta) + cz;
	gluLookAt(x, y, z, cx, cy, cz, 0.0f, 1.0f, 0.0f);

	glPointSize(1.0f);
	glBegin(GL_POINTS);
	for(int y = 0; y < height; y++){
		for(int x = 0; x < width; x++){
			int index = y * width + x;

			float px = cameraSpacePoints[index].X;
			float py = cameraSpacePoints[index].Y;
			float pz = cameraSpacePoints[index].Z;

			//float px = cameraSpacePoints_ave[index].X;
			//float py = cameraSpacePoints_ave[index].Y;
			//float pz = cameraSpacePoints_ave[index].Z;

			glColor3f(colorMap[index][0], colorMap[index][1], colorMap[index][2]);
			glVertex3f(px, py, pz);
		}
	}
	glEnd();

	glutSwapBuffers();
}
예제 #2
0
/*-------------------------------------------------------------------------
 * xmunmap - xmunmap
 *-------------------------------------------------------------------------
 */
SYSCALL xmunmap(int virtpage )
{
  STATWORD ps;
  int bsid, page;
  /* sanity check ! */
  if ( (virtpage < 4096) ){ 
    kprintf("xmummap call error: virtpage (%d) invalid! \n", virtpage);
    return SYSERR;
  }
  if(OK == bsm_lookup(currpid, VPN2VAD(virtpage), &bsid, &page)){
    write_back_frames(currpid, bsid);
  }
  ERROR_CHECK2( bsm_unmap(currpid, virtpage), ps);
  return OK;
}
예제 #3
0
/*-------------------------------------------------------------------------
 * xmmap - xmmap
 *-------------------------------------------------------------------------
 */
SYSCALL xmmap(int virtpage, bsd_t source, int npages)
{
  /* sanity check ! */
  STATWORD ps;
  if ( (virtpage < 4096) || INVALID_BSID(source) ||(npages < 1) || ( npages > 128)){
    kprintf("xmmap call error: parameter error! \n");
    return SYSERR;
  }
  if(bsm_tab[source].bs_status == BSM_MAPPED_PR){
    kprintf("Error: Trying to map private heap\n");
    return SYSERR;
  }
  disable(ps);
  ERROR_CHECK2( bsm_map(currpid, virtpage, source, npages), ps );
  
  restore(ps);
  return OK;
}
예제 #4
0
void GLWrapper::drawGeometry(GLuint vao, GLuint elementCount)
{
	GLLOG(glBindVertexArray(vao));ERROR_CHECK1(vao);
	GLLOG(glDrawElements(GL_TRIANGLES, elementCount, GL_UNSIGNED_INT, 0));ERROR_CHECK2(vao,elementCount);
}
예제 #5
0
void capture()
{
	IMultiSourceFrame *multiFrame = NULL;

	IColorFrame *colorFrame = NULL;
	IColorFrameReference *colorFrameReference = NULL;
	UINT colorBufferSize = 0;
	RGBQUAD *colorBuffer = NULL;

	IDepthFrame *depthFrame = NULL;
	IDepthFrameReference *depthFrameReference = NULL;
	UINT bufferSize = 0;
//	UINT16 *depthBuffer = NULL;

	IBodyIndexFrame *bodyIndexFrame = NULL;
	IBodyIndexFrameReference *bodyIndexFrameReference = NULL;
	UINT bodyIndexBufferSize = 0;
	static int lastTime = 0;
	static int currentTime = 0;
	HRESULT hr = -1;

	//フレームリーダーが読み込み可能になるのを待つループ(各Frameしか取らない)
  while(1) {
	  if((currentTime = GetTickCount()) > 33)
	  {
	    hr = multiFrameReader->AcquireLatestFrame(&multiFrame);
		lastTime = currentTime;
	  }else continue;

    if(FAILED(hr)) {
       //fprintf(stderr, "AcquireLatestFrame(&multiFrame)\n");
      Sleep(1);
      continue;
    }
    
    hr = multiFrame->get_ColorFrameReference(&colorFrameReference);
    if(FAILED(hr)) {
      Sleep(1);
      fprintf(stderr, "ColorFrameReference(&colorFrameReference)\n");
      SafeRelease(multiFrame);
      continue;
    }

    hr = colorFrameReference->AcquireFrame(&colorFrame);
    if(FAILED(hr)) {
      Sleep(1);
      fprintf(stderr, "AcquireFrame(&colorFrame)\n");		
      SafeRelease(colorFrameReference);
      SafeRelease(multiFrame);
      continue;
    }

	hr = multiFrame->get_DepthFrameReference(&depthFrameReference);
	if (FAILED(hr)) {
		Sleep(1);
		fprintf(stderr, "DepthFrameReference(&depthFrameReference)\n");
		SafeRelease(colorFrame);
		SafeRelease(colorFrameReference);
		SafeRelease(multiFrame);
		continue;
	}

	hr = depthFrameReference->AcquireFrame(&depthFrame);
	if (FAILED(hr)) {
		Sleep(1);
		fprintf(stderr, "AcquireFrame(&depthFrame)\n");
		SafeRelease(depthFrameReference);
		SafeRelease(colorFrame);
		SafeRelease(colorFrameReference);
		SafeRelease(multiFrame);
		continue;
	}

//	hr = depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer);
	hr = depthFrame->CopyFrameDataToArray( dPixels, &depthBuffer[0] );
	if (FAILED(hr)) {
		Sleep(1);
		fprintf(stderr, "AccessUnderlyingBuffer(&bufferSize, &depthBuffer\n");
		SafeRelease(depthFrame);
		SafeRelease(depthFrameReference);
		SafeRelease(colorFrame);
		SafeRelease(colorFrameReference);
		SafeRelease(multiFrame);
		continue;
	}


	hr = multiFrame->get_BodyIndexFrameReference(&bodyIndexFrameReference);
	if (FAILED(hr)) {
		Sleep(1);
		fprintf(stderr, "BodyIndexReference(&colorFrameReference)\n");
		free(depthBuffer);
		SafeRelease(depthFrame);
		SafeRelease(depthFrameReference);
		SafeRelease(colorFrame);
		SafeRelease(colorFrameReference);
		SafeRelease(multiFrame);
		continue;
	}

	hr = bodyIndexFrameReference->AcquireFrame(&bodyIndexFrame);
	if (FAILED(hr)) {
		Sleep(1);
		fprintf(stderr, "AcquireFrame(&bodyIndexFrame)\n");
		SafeRelease(bodyIndexFrameReference);
		free(depthBuffer);
		SafeRelease(depthFrame);
		SafeRelease(depthFrameReference);
		SafeRelease(colorFrame);
		SafeRelease(colorFrameReference);
		SafeRelease(multiFrame);
		continue;
	}

	hr = bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer);
	if(FAILED(hr))
	{
		Sleep(1);
		fprintf(stderr, "bodyIndexFrame->AccessUnderlyingBuffer(&bodyIndexBufferSize, &bodyIndexBuffer)");
		SafeRelease(bodyIndexFrame);
		SafeRelease(bodyIndexFrameReference);
		free(depthBuffer);
		SafeRelease(depthFrame);
		SafeRelease(depthFrameReference);
		SafeRelease(colorFrame);
		SafeRelease(colorFrameReference);
		SafeRelease(multiFrame);
		continue;
	}

    SafeRelease(colorFrameReference);
	SafeRelease(bodyIndexFrameReference);
	SafeRelease(depthFrameReference);
    break;
  }

  //深度値をBufferへ格納
 // ERROR_CHECK(depthFrame->AccessUnderlyingBuffer(&bufferSize, &depthBuffer));

	//カラーマップの設定データの読み込みと、colorBufferメモリの確保
	if(colorRGBX == NULL)
	{
		IFrameDescription *colorFrameDescription = NULL;

		ERROR_CHECK2(colorFrame->get_FrameDescription(&colorFrameDescription), "FrameDescription");
		ERROR_CHECK2(colorFrameDescription->get_Width(&colorWidth), "get_Width");
		ERROR_CHECK2(colorFrameDescription->get_Height(&colorHeight), "get_Height");
		colorRGBX = new RGBQUAD[colorWidth * colorHeight];

		glutReshapeWindow(width, height);

		ERROR_CHECK2(colorFrame->get_RawColorImageFormat(&imageFormat), "get_RawColorImageFormat");

		SafeRelease(colorFrameDescription);
	}

	//カラーイメージをcolorBufferへコピー
	if(imageFormat == ColorImageFormat_Bgra)
	{
		ERROR_CHECK2(colorFrame->AccessRawUnderlyingBuffer(&colorBufferSize,
			reinterpret_cast<BYTE**>(&colorBuffer)), "AccessRawUnderlyingBuffer");
	}else if(colorRGBX)
	{
		colorBuffer = colorRGBX;
		colorBufferSize = colorWidth * colorHeight * sizeof(RGBQUAD);
		ERROR_CHECK2(colorFrame->CopyConvertedFrameDataToArray(colorBufferSize,
			reinterpret_cast<BYTE*>(colorBuffer),
			ColorImageFormat_Bgra), "CopyConvertedFrameDataToArray");
	}else
	{
		//Error
	}

	//colorMapの初期化 一度だけ実行される
	if(colorMap == NULL)
	{
		colorMap = new float[colorWidth * colorHeight][3];
	}
	if (colorCoordinates == NULL)
	{
		colorCoordinates = new ColorSpacePoint[width * height];
	}
	if (cameraSpacePoints == NULL)
	{
		cameraSpacePoints = new CameraSpacePoint[width * height];
	}
	if (cameraSpacePoints_ave == NULL)
	{
		cameraSpacePoints_ave = new CameraSpacePoint[width * height];
	}

	ERROR_CHECK2(coordinateMapper->MapDepthFrameToColorSpace(
	      width * height, (UINT16*)depthBuffer, 
              width * height, colorCoordinates), "MapDepthFrameToColorSpace");

	ERROR_CHECK2(coordinateMapper->MapDepthFrameToCameraSpace(
	      width * height, (UINT16*)depthBuffer, 
	      width * height, cameraSpacePoints),"MapDepthFrameToCameraSpace");

	//colorBufferのデータを、colorMapにコピー
	for(int i = 0; i < height; i++){
		for(int j = 0; j < width; j++){
			int index = i * width + j;

			ColorSpacePoint colorPoint = colorCoordinates[index];
			int colorX = (int)(floor(colorPoint.X + 0.5));
			int colorY = (int)(floor(colorPoint.Y + 0.5));

			if(colorX >= 0 && colorX < colorWidth && colorY >= 0 && colorY < colorHeight)
			{
				int colorIndex = colorX + colorY * colorWidth;
				//格納先はミラーモードを解除
				float* colorp = colorMap[index];

				UCHAR* data = (UCHAR*)(colorBuffer + colorIndex);
				colorp[0] = (float)data[2] / 255.0f;
				colorp[1] = (float)data[1] / 255.0f;
				colorp[2] = (float)data[0] / 255.0f;
			}else
			{
				float* colorp = colorMap[index];
				colorp[0] = 0;
				colorp[1] = 0;
				colorp[2] = 0;
			}
		}
	}


	//フレームリソースを解放
  SafeRelease(colorFrame);
  SafeRelease(multiFrame);
  SafeRelease(bodyIndexFrame);
  SafeRelease(depthFrame);
}