Infodayne Infodayne - 4 months ago 33
Android Question

Generating depth map from point cloud

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.

I L I L
Answer

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_START_OF_SERVICE,
            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 accuracy of 0.4m - 5m in depth estimation. Mapping zero to 255 allows regions which were not estimated (value of zero) to be white.

Comments