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