PXCImage* DrawWorld::DepthToWorldByQueryVertices(PXCImage *depth, vector<PXCPoint3DF32>& vertices) { if (!drawVertices_) return 0; PXCImage::ImageInfo drawVerticesInfo = drawVertices_->QueryInfo(); PXCImage::ImageData drawVerticesDat; if (PXC_STATUS_NO_ERROR > drawVertices_->AcquireAccess(PXCImage::ACCESS_WRITE, drawVerticesInfo.format, &drawVerticesDat)) return 0; /* Retrieve vertices */ float brightness = 200.f; PXCImage::ImageInfo dinfo = depth->QueryInfo(); pxcBYTE* pdrawVerticesDat = drawVerticesDat.planes[0]; for (pxcI32 y = 1; y < dinfo.height - 1; y++) { for (pxcI32 x = 1; x < dinfo.width - 1; x++) { pdrawVerticesDat[4 * x] = pdrawVerticesDat[4 * x + 1] = pdrawVerticesDat[4 * x + 2] = 0; float fLight = 0; PXCPoint3DF32 v0 = vertices[y * dinfo.width + x]; if (v0.z != 0) { //取v0四角点 PXCPoint3DF32 v1 = vertices[(y - 1) * dinfo.width + (x - 1)]; PXCPoint3DF32 v2 = vertices[(y - 1) * dinfo.width + (x + 1)]; PXCPoint3DF32 v3 = vertices[(y + 1) * dinfo.width + (x + 1)]; PXCPoint3DF32 v4 = vertices[(y + 1) * dinfo.width + (x - 1)]; //转成向量 v1.x = v1.x - v0.x; v1.y = v1.y - v0.y; v1.z = v1.z - v0.z; v2.x = v2.x - v0.x; v2.y = v2.y - v0.y; v2.z = v2.z - v0.z; v3.x = v3.x - v0.x; v3.y = v3.y - v0.y; v3.z = v3.z - v0.z; v4.x = v4.x - v0.x; v4.y = v4.y - v0.y; v4.z = v4.z - v0.z; //求出四个法向量 PXCPoint3DF32 vn1 = cross(v1, v2); norm(vn1); PXCPoint3DF32 vn2 = cross(v2, v3); norm(vn2); PXCPoint3DF32 vn3 = cross(v3, v4); norm(vn3); PXCPoint3DF32 vn4 = cross(v4, v1); norm(vn4); // vn1.x += vn2.x + vn3.x + vn4.x; vn1.y += vn2.y + vn3.y + vn4.y; vn1.z += vn2.z + vn3.z + vn4.z; norm(vn1); // norm(light_); fLight = dot(vn1, light_); } pdrawVerticesDat[4 * x] = pdrawVerticesDat[4 * x + 1] = pdrawVerticesDat[4 * x + 2] = pxcBYTE(abs(fLight) * 255); pdrawVerticesDat[4 * x + 3] = MAXBYTE; } //换到下一个行 pdrawVerticesDat += drawVerticesDat.pitches[0]; } drawVertices_->ReleaseAccess(&drawVerticesDat); return drawVertices_; }
void Graphics::DrawBitmap(PXCCapture::Sample* sample) { PXCImage *imageDepth = sample->depth; assert(imageDepth); PXCImage::ImageInfo imageDepthInfo = imageDepth->QueryInfo(); m_outputImageInfo.width = 1024; m_outputImageInfo.height = 1024; m_outputImageInfo.format = PXCImage::PIXEL_FORMAT_RGB32; m_outputImageInfo.reserved = 0; m_outputImage = m_session->CreateImage(&m_outputImageInfo); assert(m_outputImage); PXCImage::ImageData imageDepthData; if(imageDepth->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_RGB32, &imageDepthData) >= PXC_STATUS_NO_ERROR) { memset(&m_outputImageData, 0, sizeof(m_outputImageData)); pxcStatus status = m_outputImage->AcquireAccess(PXCImage::ACCESS_WRITE, PXCImage::PIXEL_FORMAT_RGB32, &m_outputImageData); if(status < PXC_STATUS_NO_ERROR) return; int stridePixels = m_outputImageData.pitches[0]; pxcBYTE *pixels = reinterpret_cast<pxcBYTE*> (m_outputImageData.planes[0]); memset(pixels, 0, stridePixels * m_outputImageInfo.height); // get access to depth data PXCPoint3DF32* vertices = new PXCPoint3DF32[imageDepthInfo.width * imageDepthInfo.height]; PXCProjection* projection(m_senseManager->QueryCaptureManager()->QueryDevice()->CreateProjection()); if (!projection) { if (vertices) delete[] vertices; return; } projection->QueryVertices(imageDepth, vertices); projection->Release(); int strideVertices = imageDepthInfo.width; // render vertices int numVertices = 0; for (int y = 0; y < imageDepthInfo.height; y++) { const PXCPoint3DF32 *verticesRow = vertices + y * strideVertices; for (int x = 0; x < imageDepthInfo.width; x++) { const PXCPoint3DF32 &v = verticesRow[x]; if (v.z <= 0.0f) { continue; } int ix = 0, iy = 0; if(ProjectVertex(v, ix, iy)) { pxcBYTE *ptr = m_outputImageData.planes[0]; ptr += iy * m_outputImageData.pitches[0]; ptr += ix * 4; ptr[0] = pxcBYTE(255.0f * 0.5f); ptr[1] = pxcBYTE(255.0f * 0.5f); ptr[2] = pxcBYTE(255.0f * 0.5f); ptr[3] = pxcBYTE(255.0f); } numVertices++; } } if (vertices) delete[] vertices; if (m_bitmap) { DeleteObject(m_bitmap); m_bitmap = 0; } HWND hwndPanel = GetDlgItem(m_window, IDC_PANEL); HDC dc = GetDC(hwndPanel); BITMAPINFO binfo; memset(&binfo, 0, sizeof(binfo)); binfo.bmiHeader.biWidth = m_outputImageData.pitches[0] / 4; binfo.bmiHeader.biHeight = -(int)m_outputImageInfo.height; binfo.bmiHeader.biBitCount = 32; binfo.bmiHeader.biPlanes = 1; binfo.bmiHeader.biSize = sizeof(BITMAPINFOHEADER); binfo.bmiHeader.biCompression = BI_RGB; Sleep(1); m_bitmap = CreateDIBitmap(dc, &binfo.bmiHeader, CBM_INIT, m_outputImageData.planes[0], &binfo, DIB_RGB_COLORS); ReleaseDC(hwndPanel, dc); m_outputImage->ReleaseAccess(&m_outputImageData); imageDepth->ReleaseAccess(&imageDepthData); m_outputImage->Release(); } }
PXCImage* DrawWorld::SegmentationWorld(PXCImage *depth, vector<PXCPoint3DF32> &vertices, PointSet &seg) { if (!drawVertices_) return 0; PXCImage::ImageInfo drawVerticesInfo = drawVertices_->QueryInfo(); PXCImage::ImageData drawVerticesDat; if (PXC_STATUS_NO_ERROR > drawVertices_->AcquireAccess(PXCImage::ACCESS_WRITE, drawVerticesInfo.format, &drawVerticesDat)) return 0; /* Retrieve vertices */ float brightness = 200.f; PXCImage::ImageInfo dinfo = depth->QueryInfo(); pxcBYTE* pdrawVerticesDat = drawVerticesDat.planes[0]; memset(pdrawVerticesDat, -1, 4 * dinfo.width*dinfo.height); for (auto p : seg) { float fLight = 0; pxcI32 x = p.x * 2, y = p.y * 2; PXCPoint3DF32 v0 = vertices[y * dinfo.width + x]; if (v0.z != 0) { //取v0四角点 PXCPoint3DF32 v1 = vertices[(y - 1) * dinfo.width + (x - 1)]; PXCPoint3DF32 v2 = vertices[(y - 1) * dinfo.width + (x + 1)]; PXCPoint3DF32 v3 = vertices[(y + 1) * dinfo.width + (x + 1)]; PXCPoint3DF32 v4 = vertices[(y + 1) * dinfo.width + (x - 1)]; //转成向量 v1.x = v1.x - v0.x; v1.y = v1.y - v0.y; v1.z = v1.z - v0.z; v2.x = v2.x - v0.x; v2.y = v2.y - v0.y; v2.z = v2.z - v0.z; v3.x = v3.x - v0.x; v3.y = v3.y - v0.y; v3.z = v3.z - v0.z; v4.x = v4.x - v0.x; v4.y = v4.y - v0.y; v4.z = v4.z - v0.z; //求出四个法向量 PXCPoint3DF32 vn1 = cross(v1, v2); norm(vn1); PXCPoint3DF32 vn2 = cross(v2, v3); norm(vn2); PXCPoint3DF32 vn3 = cross(v3, v4); norm(vn3); PXCPoint3DF32 vn4 = cross(v4, v1); norm(vn4); // vn1.x += vn2.x + vn3.x + vn4.x; vn1.y += vn2.y + vn3.y + vn4.y; vn1.z += vn2.z + vn3.z + vn4.z; norm(vn1); // norm(light_); fLight = dot(vn1, light_); } pdrawVerticesDat[4 * x] = pdrawVerticesDat[4 * x + 1] = pdrawVerticesDat[4 * x + 2] = pxcBYTE(abs(fLight) * 255); } drawVertices_->ReleaseAccess(&drawVerticesDat); return drawVertices_; }