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_;
}
