void Update()
{
if (_texData == null)
{
return;
}
// Ensure work resources.
if (_depthMat == null)
{
// RealSense image pixels are single channel unsigned 16-bit integers.
_depthRawMat = new Mat(_height, _width, CvType.CV_16UC1); // 16 bit unsigned one channel.
_depthMat = new Mat(_height, _width, CvType.CV_16SC1);
_depthPointsMat = new Mat(_height, _width, CvType.CV_16S); // Default is CV_32FC3, alernaitves are CV_16S, CV_32S or CV_32F.
_sensorMatrix = new Mat(4, 4, CvType.CV_64F);
//_outputMat = new Mat( _height, _width, CvType.CV_16UC1 );
_unityOutputTexture = new Texture2D(_width, _height, TextureFormat.R16, false, true);
}
// Read raw data into raw mat.
MatUtils.copyToMat(_texData, _depthRawMat);
// Convert to signed 16bit. Calib3d.reprojectImageTo3D only takes either CV_8UC1, CV_16SC1, CV_32S or CV_32F disparity image
_depthRawMat.convertTo(_depthMat, _depthMat.type());
// Reproject depth mat into into a 3-channel mat of points.
//Matrix4x4 m = _sensor.projectionMatrix.inverse * _sensor.worldToCameraMatrix;
Matrix4x4 m = new Matrix4x4(
new Vector4(1, 0, 0, 0),
new Vector4(0, 1, 0, 0),
new Vector4(0, 0, _test, 0),
new Vector4(0, 0, 0, 1)
);
m = m.inverse;
//Debug.Log( m );
_sensorMatrix.put(0, 0,
m.m00, m.m01, m.m02, m.m03,
m.m10, m.m11, m.m12, m.m13,
m.m20, m.m21, m.m22, m.m23,
m.m30, m.m31, m.m32, m.m33
//m.m00, m.m10, m.m20, m.m30,
//m.m01, m.m11, m.m21, m.m31,
//m.m02, m.m12, m.m22, m.m32,
//m.m03, m.m13, m.m23, m.m33
//1, 0, 0, 0,
//0, 1, 0, 0,
//0, 0, 1, 0,
//0, 0, 0, 1
);
bool handleMissingValues = false;
Calib3d.reprojectImageTo3D(_depthMat, _depthPointsMat, _sensorMatrix, handleMissingValues, _depthPointsMat.type());
// Copy z (blue) channel from _depthPointsMat to single channnel _depthRawMat.
MatOfInt fromTo = new MatOfInt(2, 0);
Core.mixChannels(new List <Mat>(new Mat[] { _depthPointsMat }), new List <Mat>(new Mat[] { _depthMat }), fromTo);
// Normalize to CvType.CV_8U.
//Core.normalize (imgDisparity16S, imgDisparity8U, 0, 255, Core.NORM_MINMAX, CvType.CV_8U);
// Read back to Unity.
Utils.fastMatToTexture2D(_depthMat, _unityOutputTexture);
// Output.
_debugTextureEvent.Invoke(_unityOutputTexture);
}
