c# - Generating point cloud from a mask and depth map - Stack Overflow

admin管理员组

文章数量:1310228

I have a mask texture and depth map, the mask texture has resoultion of 316* 360 and the depth map has resoultion of 1280 * 720, I'm trying to check for each pixel in the mask if it's in the depth map, but I'm getting a lot of points, and they are not related to the mask, like sampling lots of points from depth map and not correlated to the mask that's the mask

private List<Vector3> ExtractPointsFromMaskWithZEDMat(Vector2 maskSize, Texture2D maskTexture, ZEDMat depthMat, CalibrationParameters paras, Pose pose)
    {
        List<Vector3> points = new List<Vector3>();
        Pose p = new Pose();
        zedManager.zedCamera.GetPosition(ref p, REFERENCE_FRAME.WORLD);
       
        float fx = paras.leftCam.fx;
        float fy = paras.leftCam.fy;
        float cx = paras.leftCam.cx;
        float cy = paras.leftCam.cy;

        int depthWidth = depthMat.GetWidth();   // 1280
        int depthHeight = depthMat.GetHeight(); // 720

        float scaleX = maskSize.x / depthWidth;
        float scaleY = maskSize.y / depthHeight;
        int stride = 2; 

        for (int maskY = 0; maskY < maskSize.y; maskY += stride)
        {
            for (int maskX = 0; maskX < maskSize.x; maskX += stride)
            {

                Color maskColor = maskTexture.GetPixel(maskX, (int)maskSize.y - 1 - maskY);
                if (maskColor.a < 0.5f) continue;

                int depthX = Mathf.FloorToInt(maskX / scaleX);
                int depthY = Mathf.FloorToInt(maskY / scaleY);
                if (depthX >= depthWidth || depthY >= depthHeight) continue;
                float depth;
                depthMat.GetValue(depthX, depthY, out depth, ZEDMat.MEM.MEM_CPU);
                if (depth <= 0.01f || float.IsNaN(depth)) continue;

                float xCam = (depthX - cx) * depth / fx;
                float yCam = (depthY - cy) * depth / fy;
                float zCam = depth;
                Vector3 cameraPoint = new Vector3(xCam, yCam, zCam);
                Vector3 worldPoint = pose.rotation * cameraPoint + pose.translation;
                worldPoint.y = -worldPoint.y;
                if (worldPoint.magnitude <= 5.0f)
                {
                    points.Add(worldPoint);
                }
            }
        }
        return points;
    }

本文标签： cGenerating point cloud from a mask and depth mapStack Overflow