void D3D11RenderTarget::captureDepthBuffer(DepthImage32& result, const mat4f& perspectiveTransform)
{
captureDepthBuffer(result);
const mat4f inv = perspectiveTransform.getInverse();
//result.setInvalidValue(std::numeric_limits<float>::infinity()); //the default is -INF
for (unsigned int y = 0; y < result.getHeight(); y++) {
for (unsigned int x = 0; x < result.getWidth(); x++) {
float &v = result(x, y);
if (v >= 1.0f)
v = result.getInvalidValue();
else
{
//float dx = math::linearMap(0.0f, result.getWidth() - 1.0f, -1.0f, 1.0f, (float)x);
//float dy = math::linearMap(0.0f, result.getHeight() - 1.0f, -1.0f, 1.0f, (float)y);
//v = (inv * vec3f(dx, dy, v)).z;
vec2f p = D3D11GraphicsDevice::pixelToNDC(vec2i(x, y), result.getWidth(), result.getHeight());
v = (inv * vec3f(p.x, p.y, v)).z;
}
}
}
}
void VoxelGrid::integrate(const mat4f& intrinsic, const mat4f& cameraToWorld, const DepthImage32& depthImage, const BaseImage<unsigned short>& semantics)
{
const mat4f worldToCamera = cameraToWorld.getInverse();
BoundingBox3<int> voxelBounds = computeFrustumBounds(intrinsic, cameraToWorld, depthImage.getWidth(), depthImage.getHeight());
for (int k = voxelBounds.getMinZ(); k <= voxelBounds.getMaxZ(); k++) {
for (int j = voxelBounds.getMinY(); j <= voxelBounds.getMaxY(); j++) {
for (int i = voxelBounds.getMinX(); i <= voxelBounds.getMaxX(); i++) {
//transform to current frame
vec3f p = worldToCamera * voxelToWorld(vec3i(i, j, k));
//project into depth image
p = skeletonToDepth(intrinsic, p);
vec3i pi = math::round(p);
if (pi.x >= 0 && pi.y >= 0 && pi.x < (int)depthImage.getWidth() && pi.y < (int)depthImage.getHeight()) {
const float d = depthImage(pi.x, pi.y);
const unsigned short sem = semantics(pi.x, pi.y);
//check for a valid depth range
if (d != depthImage.getInvalidValue() && d >= m_depthMin && d <= m_depthMax) {
//update free space counter if voxel is in front of observation
if (p.z < d) {
(*this)(i, j, k).freeCtr++;
}
//compute signed distance; positive in front of the observation
float sdf = d - p.z;
float truncation = getTruncation(d);
if (sdf > -truncation) {
Voxel& v = (*this)(i, j, k);
if (std::abs(sdf) <= std::abs(v.sdf)) {
if (sdf >= 0.0f || v.sdf <= 0.0f) {
v.sdf = sdf;
v.color[0] = sem & 0xff; //ushort to vec2uc
v.color[1] = (sem >> 8) & 0xff;
v.weight = 1;
}
}
//std::cout << "v: " << v.sdf << " " << (int)v.weight << std::endl;
}
}
}
}
}
void D3D11RenderTarget::captureDepthBuffer(DepthImage32 &result)
{
auto &context = m_graphics->getContext();
context.CopyResource(m_captureDepth, m_depthStencil);
result.allocate(m_width, m_height);
D3D11_MAPPED_SUBRESOURCE resource;
UINT subresource = D3D11CalcSubresource(0, 0, 0);
HRESULT hr = context.Map(m_captureDepth, subresource, D3D11_MAP_READ, 0, &resource);
const BYTE *data = (BYTE *)resource.pData;
for (unsigned int y = 0; y < m_height; y++) {
memcpy(&result(0U, y), data + resource.RowPitch * y, m_width * sizeof(float));
}
context.Unmap(m_captureDepth, subresource);
}
void VoxelGrid::integrate(const mat4f& intrinsic, const mat4f& cameraToWorld, const DepthImage32& depthImage)
{
const mat4f worldToCamera = cameraToWorld.getInverse();
BoundingBox3<int> voxelBounds = computeFrustumBounds(intrinsic, cameraToWorld, depthImage.getWidth(), depthImage.getHeight());
//std::cout << "camera to world" << std::endl << cameraToWorld << std::endl;
//std::cout << "world to camera" << std::endl << worldToCamera << std::endl;
for (int k = voxelBounds.getMinZ(); k <= voxelBounds.getMaxZ(); k++) {
for (int j = voxelBounds.getMinY(); j <= voxelBounds.getMaxY(); j++) {
for (int i = voxelBounds.getMinX(); i <= voxelBounds.getMaxX(); i++) {
//transform to current frame
vec3f pf = worldToCamera * voxelToWorld(vec3i(i, j, k));
//vec3f p = worldToCamera * (m_gridToWorld * ((vec3f(i, j, k) + 0.5f)));
//project into depth image
vec3f p = skeletonToDepth(intrinsic, pf);
vec3i pi = math::round(p);
if (pi.x >= 0 && pi.y >= 0 && pi.x < (int)depthImage.getWidth() && pi.y < (int)depthImage.getHeight()) {
float d = depthImage(pi.x, pi.y);
//check for a valid depth range
if (d != depthImage.getInvalidValue() && d >= m_depthMin && d <= m_depthMax) {
//update free space counter if voxel is in front of observation
if (p.z < d) {
(*this)(i, j, k).freeCtr++;
}
//compute signed distance; positive in front of the observation
float sdf = d - p.z;
float truncation = getTruncation(d);
if (sdf > -truncation) {
if (sdf >= 0.0f) {
sdf = fminf(truncation, sdf);
}
else {
sdf = fmaxf(-truncation, sdf);
}
const float integrationWeightSample = 3.0f;
const float depthWorldMin = 0.4f;
const float depthWorldMax = 4.0f;
float depthZeroOne = (d - depthWorldMin) / (depthWorldMax - depthWorldMin);
float weightUpdate = std::max(integrationWeightSample * 1.5f * (1.0f - depthZeroOne), 1.0f);
Voxel& v = (*this)(i, j, k);
if (v.sdf == -std::numeric_limits<float>::infinity()) {
v.sdf = sdf;
}
else {
v.sdf = (v.sdf * (float)v.weight + sdf * weightUpdate) / (float)(v.weight + weightUpdate);
}
v.weight = (uchar)std::min((int)v.weight + (int)weightUpdate, (int)std::numeric_limits<unsigned char>::max());
}
}
}
}
}
}
}
