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;
}
}
}
}
double InteractionRays::score(bool visibilityOptional, const ml::TriMeshRayAcceleratorf &occluder, const mat4f &modelToWorld) const
{
double score = 0.0;
double maxScore = (double)rays.size();
for (const auto &r : rays)
{
if (visibilityOptional)
{
score += r.score;
}
else if (r.visible)
{
//
// check to see if the ray is occluded by occluder
//
bool visible = true;
Rayf modelRay = modelToWorld.getInverse() * r.ray;
auto intersection = occluder.intersect(modelRay);
if (intersection.valid())
{
double centroidDist = ml::dist(r.ray.origin(), r.centroidPos);
double intersectionDist = ml::dist(r.ray.origin(), modelToWorld.transformAffine(intersection.getSurfacePosition()));
if (intersectionDist < centroidDist)
visible = false;
}
if (visible)
score += r.score;
}
}
if (maxScore == 0.0)
return 0.0;
return score / maxScore;
}
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();
}
}
void Synthesizer::computeScanPlacement(const DisembodiedObject &object, const mat4f &modelToWorld, ObjectScoreEntry &result)
{
const Category &category = database->categories.getCategory(object.model->categoryName);
const ModelAssetData &asset = assets->loadModel(*graphics, object.modelId);
memcpy(&columnsTemp.columns(0, 0), &baseColumns.columns(0, 0), sizeof(SceneColumn)* baseColumns.columns.getNumElements());
//columnsTemp.includeObjectRayTracing(accelerator, bbox3f(worldBBox), modelToWorld, modelToWorld.getInverse(), category.supportCategory);
columnsTemp.includeObjectMaxZMap(asset.getMaxZMap(), modelToWorld, modelToWorld.getInverse(), asset.getModelToVoxel(), category.isObjectSupport);
//columnsTemp.includeObjectBBoxSimplification(worldBBox, category.supportCategory);
result.scanPlacementScore = ColumnRepresentation::score(sceneTemplate->geometry.columns, columnsTemp);
}
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;
}
}
}
}
}
double Synthesizer::collisionScore(const DisembodiedObject &baseObject, const mat4f &modelToWorld) const
{
const auto *modelAccelerator = &assets->loadModel(*graphics, baseObject.modelId).getAcceleratorSimplified();
const Category &category = database->categories.getCategory(baseObject.model->categoryName);
if (category.isBBoxCollision)
modelAccelerator = &assets->loadModel(*graphics, baseObject.modelId).getAcceleratorBBoxOnly();
const mat4f worldToModel = modelToWorld.getInverse();
//
// some categories should be checked for collision with the architecture; generally, this list should be kept as small as possible because this
// is *very* expensive
//
UINT objectStartIndex = 1;
if (category.name == "FloorLamp" || category.name == "MagazineBin" || category.name == "ChestOfDrawers" || category.name == "Bed" || category.name == "SideTable")
objectStartIndex = 0;
//
// check collisions against objects in scene
//
for (UINT objectIndex = objectStartIndex; objectIndex < scene.objects.size(); objectIndex++)
{
auto &sceneObject = scene.objects[objectIndex];
const auto *sceneObjectAccelerator = &assets->loadModel(*graphics, sceneObject.object.modelId).getAcceleratorSimplified();
if (database->categories.getCategory(sceneObject.object.model->categoryName).isBBoxCollision)
sceneObjectAccelerator = &assets->loadModel(*graphics, sceneObject.object.modelId).getAcceleratorBBoxOnly();
const mat4f sceneObjectToModel = worldToModel * sceneObject.modelToWorld;
if (modelAccelerator->collision(*sceneObjectAccelerator, sceneObjectToModel)) {
return 0.0;
}
}
//
// check collision against activity blockers
//
if (!category.isAgentSupport)
{
for (const ActivityBlocker &blocker : activityBlockers)
{
if (modelAccelerator->collision(*blocker.accel, worldToModel)) {
return 0.0;
}
}
}
return 1.0;
}
void CUDARayCastSDF::rayIntervalSplatting(const HashData& hashData, const HashParams& hashParams, const DepthCameraData& cameraData, const mat4f& lastRigidTransform)
{
if (hashParams.m_numOccupiedBlocks == 0) return;
if (m_params.m_maxNumVertices <= 6*hashParams.m_numOccupiedBlocks) { // 6 verts (2 triangles) per block
MLIB_EXCEPTION("not enough space for vertex buffer for ray interval splatting");
}
m_params.m_numOccupiedSDFBlocks = hashParams.m_numOccupiedBlocks;
m_params.m_viewMatrix = MatrixConversion::toCUDA(lastRigidTransform.getInverse());
m_params.m_viewMatrixInverse = MatrixConversion::toCUDA(lastRigidTransform);
//m_data.updateParams(m_params); // !!! debugging
m_rayIntervalSplatting.rayIntervalSplatting(DXUTGetD3D11DeviceContext(), hashData, cameraData, m_data, m_params, m_params.m_numOccupiedSDFBlocks*6);
}
double Synthesizer::collisionBBoxScore(const DisembodiedObject &object, const mat4f &modelToWorld) const
{
const auto &modelAccelerator = assets->loadModel(*graphics, object.modelId).getAcceleratorSimplified();
for (UINT objectIndex = 1; objectIndex < scene.objects.size(); objectIndex++)
{
auto &object = scene.objects[objectIndex];
const auto &sceneObjectAccelerator = assets->loadModel(*graphics, object.object.modelId).getAcceleratorSimplified();
mat4f sceneObjectToModel = modelToWorld.getInverse() * object.modelToWorld;
if (modelAccelerator.collisionBBoxOnly(sceneObjectAccelerator, sceneObjectToModel)) {
return 0.0;
}
}
return 1.0;
}
void D3D11RenderTarget::captureDepthBuffer(ColorImageR32 &result, const mat4f &perspectiveTransform)
{
captureDepthBuffer(result);
auto inv = perspectiveTransform.getInverse();
result.setInvalidValue(std::numeric_limits<float>::infinity());
for (UINT y = 0; y < result.getHeight(); y++)
{
for (UINT 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)).length();
}
}
}
}
void RGBDSensor::initializeColorExtrinsics(const mat4f& m) {
m_colorExtrinsics = m;
m_colorExtrinsicsInv = m.getInverse();
}
void RGBDSensor::initializeDepthExtrinsics(const mat4f& m) {
m_depthExtrinsics = m;
m_depthExtrinsicsInv = m.getInverse();
}
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());
}
}
}
}
}
}
}
void Synthesizer::makeInteractionMapRays(const DisembodiedObject &object, const mat4f &modelToWorld, const Agent &agent, const string &interactionMapName, InteractionRays &result) const
{
const string &categoryName = object.model->categoryName;
result.rays.clear();
if (!database->interactionMaps.hasInteractionMapEntry(object.modelId, interactionMapName))
{
return;
}
const InteractionMapEntry &entry = database->interactionMaps.getInteractionMapEntry(object.modelId, interactionMapName);
const auto ¢roids = entry.centroids;
if (centroids.size() == 0 || (centroids.size() > 10 && synthParams().scanName == "dining-large-nowalld"))
{
return;
}
vector < pair<const ml::TriMeshAcceleratorBVHf*, mat4f> > allAccelerators;
for (UINT objectIndex = 0; objectIndex < scene.objects.size(); objectIndex++)
{
//
// skip the base architecture, since collisions with that are very unlikely to occlude things
//
if (object.contactType == ContactWall || objectIndex != 0)
{
const auto &object = scene.objects[objectIndex];
const auto &accelerator = assets->loadModel(*graphics, object.object.modelId).getAcceleratorOriginal();
allAccelerators.push_back(std::make_pair(&accelerator, object.modelToWorld.getInverse()));
}
}
const auto &selfAccelerator = assets->loadModel(*graphics, object.modelId).getAcceleratorOriginal();
allAccelerators.push_back(std::make_pair(&selfAccelerator, modelToWorld.getInverse()));
result.rays.resize(centroids.size());
bool visibilityOptional = (interactionMapName == "backSupport" || interactionMapName == "hips");
vec3f rayOrigin = agent.headPos;
if (interactionMapName == "fingertip")
{
rayOrigin = agent.handPos();
}
if (interactionMapName == "backSupport")
{
rayOrigin = agent.spinePos();
}
UINT centroidIndex = 0;
for (const auto ¢roid : centroids)
{
InteractionRayEntry &rayEntry = result.rays[centroidIndex];
rayEntry.centroidPos = modelToWorld * centroid.pos;
rayEntry.ray = ml::Rayf(rayOrigin, rayEntry.centroidPos - rayOrigin);
rayEntry.score = 1.0;
rayEntry.visible = false;
ml::TriMeshRayAcceleratorf::Intersection intersection;
UINT intersectObjectIndex;
if (ml::TriMeshRayAcceleratorf::getFirstIntersectionTransform(rayEntry.ray, allAccelerators, intersection, intersectObjectIndex))
{
//
// NOTE: intersection is in model space.
//
double centroidDist = ml::dist(rayOrigin, rayEntry.centroidPos);
double intersectDist = ml::dist(rayOrigin, modelToWorld.transformAffine(intersection.getSurfacePosition()));
rayEntry.visible = (fabs(centroidDist - intersectDist) < 0.01);
if (visibilityOptional || rayEntry.visible)
{
vec3f surfaceNormal = modelToWorld.transformNormalAffine(intersection.getSurfaceNormal()).getNormalized();
double minNormalAngle = std::min(vec3f::angleBetween( surfaceNormal, rayEntry.ray.direction()),
vec3f::angleBetween(-surfaceNormal, rayEntry.ray.direction()));
double cosineVisiblityScore = std::max(0.0, cos(ml::math::degreesToRadians(minNormalAngle)));
double score = 1.0;
if (interactionMapName == "gaze")
{
// for monitors, gaze is better when directly viewing the object head-on
//if (categoryName == "Monitor" || categoryName == "Laptop")
score = cosineVisiblityScore;
}
if (interactionMapName == "fingertip")
{
score -= 0.01f * centroidDist;
}
if (interactionMapName == "backSupport")
{
// back support only counts as visible when it is behind the agent
if ((rayEntry.ray.direction() | agent.gazeDir) > 0.0f)
{
score = 0.0;
}
else
{
// back support is best when directly behind the agent
score = -(rayEntry.ray.direction() | agent.gazeDir);
}
}
rayEntry.score = score;
}
}
centroidIndex++;
}
}
