Generating depth map from point cloud

Question

I am trying to generate a depth map from the point cloud. I know that I can project the point cloud to the image plane, however there is already a function (ScreenCoordinateToWorldNearestNeighbor) in the TangoSupport script that finds the XYZ point given a screen coordinate.

I am unable to get this support function to work, and it seems that one or more of my inputs are invalid. I am updating my depthmap texture in the OnTangoDepthAvailable event.

public void OnTangoDepthAvailable(TangoUnityDepth tangoDepth)
{
    _depthAvailable = true;
    Matrix4x4 ccWorld = _Camera.transform.localToWorldMatrix;
    bool isValid = false;
    Vector3 colorCameraPoint = new Vector3();
    for (int i = 0; i < _depthMapSize; i++)
    {
        for (int j = 0; j < _depthMapSize; j++)
        {
            if (TangoSupport.ScreenCoordinateToWorldNearestNeighbor(
                _PointCloud.m_points, _PointCloud.m_pointsCount,
                tangoDepth.m_timestamp, 
                _ccIntrinsics,
                ref ccWorld, 
                new Vector2(i / (float)_depthMapSize, j / (float)_depthMapSize),
                out colorCameraPoint, out isValid) == Common.ErrorType.TANGO_INVALID)
            {
                _depthTexture.SetPixel(i, j, Color.red);
                continue;
            }

            if (isValid)
            {
                //_depthTexture.SetPixel(i, j, new Color(colorCameraPoint.z, colorCameraPoint.z, colorCameraPoint.z));
                _depthTexture.SetPixel(i, j,
                    new Color(0,UnityEngine.Random.value,0));
            }
            else
            {
                _depthTexture.SetPixel(i, j, Color.white);
            }
        }
    }
    _depthTexture.Apply();
    _DepthMapQuad.material.mainTexture = _depthTexture;
}

If I had to guess, I would say that I am passing in the wrong matrix (ccWorld). Here is what it says in the documents for the matrix param:

Transformation matrix of the color camera with respect to the Unity world frame.

The result is a white depth map, which means that the function is returning successfully, however the isValid is false meaning that it couldn't find any nearby point cloud point after projection.

Any ideas? Also I noticed that the performance is pretty bad, even when my depth map is 8x8. Should I not be updating the depthmap when ever new depth data is available (inside OnTangoDepthAvailable)?

Edit: I was able to make the function return successfully, however now it doesn't find a point cloud point nearby after projection. The resulting depth map is always white. I am printing out all the arguments, and it all looks correct, so I think I am passing in the wrong matrix.

Answer 1

You should update your SDK and Project Tango Dev Kit. Here is an example of getting depth map on Android, perhaps you get a hint for unity:

public class MainActivity extends AppCompatActivity {

    private Tango mTango;
    private TangoConfig mTangoConfig;
    private TangoPointCloudManager mPointCloudManager;
    private AtomicBoolean tConnected = new AtomicBoolean(false);
    Random rand = new Random();
    private ImageView imageDepthMap;


    private static final ArrayList<TangoCoordinateFramePair> framePairs = new ArrayList<TangoCoordinateFramePair>();

    {
        framePairs.add(new TangoCoordinateFramePair(
                TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH,
                TangoPoseData.COORDINATE_FRAME_DEVICE));
    }
        @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);


           //initialize the imageView
            imageDepthMap = (ImageView)findViewById(R.id.imageView);

        //initialize pointCloudManager

        mPointCloudManager = new TangoPointCloudManager();


    }

    @Override
    protected void onResume(){

        super.onResume();
        //obtain the tango configuration

        if(tConnected.compareAndSet(false, true)) {


            try {

                setTango();

            } catch (TangoOutOfDateException tE) {

                tE.printStackTrace();
            }

        }
    }

    @Override
    protected void onPause(){

        super.onPause();

        if(tConnected.compareAndSet(true, false)) {
            try {
                //disconnect Tango service so other applications can use it
                mTango.disconnect();
            } catch (TangoException e) {
                e.printStackTrace();
            }
        }
    }


    private void setTango(){

        mTango = new Tango(MainActivity.this, new Runnable() {
            @Override
            public void run() {

                TangoSupport.initialize();
                mTangoConfig = new TangoConfig();
                mTangoConfig = mTango.getConfig(TangoConfig.CONFIG_TYPE_CURRENT);
                mTangoConfig.putBoolean(TangoConfig.KEY_BOOLEAN_DEPTH, true); //activate depth sensing

                mTango.connect(mTangoConfig);

                mTango.connectListener(framePairs, new Tango.OnTangoUpdateListener() {
                @Override
                public void onPoseAvailable(TangoPoseData tangoPoseData) {

                }

                @Override
                public void onXyzIjAvailable(TangoXyzIjData pointCloud) {

                    // Log.d("gDebug", "xyZAvailable");
                    //TangoXyzIjData pointCloud = mPointCloudManager.getLatestXyzIj();
                    // Update current camera pose

                    if (pointCloud.ijRows * pointCloud.ijCols > 0){
                        try {
                            // Calculate the last camera color pose.
                            TangoPoseData lastFramePose = TangoSupport.getPoseAtTime(0,
                                    TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE,
                                    TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR,
                                    TangoSupport.TANGO_SUPPORT_ENGINE_OPENGL, 0);


                            if (pointCloud != null) {

                                //obtain depth info per pixel
                                TangoSupport.DepthBuffer depthBuf = TangoSupport.upsampleImageNearestNeighbor(pointCloud, mTango.getCameraIntrinsics(TangoCameraIntrinsics.TANGO_CAMERA_COLOR), lastFramePose);

                                //create Depth map
                                int[] intBuff = convertToInt(depthBuf.depths, depthBuf.width, depthBuf.height);                              

                                final Bitmap Image = Bitmap.createBitmap(intBuff, depthBuf.width, depthBuf.height, Bitmap.Config.ARGB_8888);

                                runOnUiThread(new Runnable() {
                                    @Override
                                    public void run() {
                                        imageDepthMap.setImageBitmap(Image);
                                    }
                                });

                            }
                        } catch (TangoErrorException e) {
                            Log.e("gDebug", "Could not get valid transform");
                        }
                }
            }

            @Override
            public void onFrameAvailable(int i) {

                //Log.d("gDebug", "Frame Available from " + i);
            }

            @Override
            public void onTangoEvent(TangoEvent tangoEvent) {

            }
        });
            }
        });


    }

    private int[] convertToInt(FloatBuffer pointCloudData, int width, int height){
        double mulFact = 255.0/5.0;
        int byteArrayCapacity = width * height;
        int[] depthMap = new int[byteArrayCapacity];
        int grayPixVal = 0;

        pointCloudData.rewind();
        for(int i =0; i < byteArrayCapacity; i++){

            //obtain grayscale representation
            grayPixVal = (int)(mulFact * (5.0- pointCloudData.get(i)));
            depthMap[i] = Color.rgb(grayPixVal, grayPixVal, grayPixVal);

        }



        return depthMap;
    }

}

I extracted this code from my already working version. Try to fix any config related errors. The code assumes depth sensing range of 0.4m - 5m in depth estimation. Mapping zero to 255 allows regions which were not estimated (value of zero) to be white.