private void _UpdateColor(TangoImageBuffer image, Matrix4x4 imagePose)
{
if (!m_sendColorToUpdate)
{
// There is no depth cloud to process.
return;
}
if (m_context == IntPtr.Zero)
{
Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace);
return;
}
lock (m_lockObject)
{
if (!m_mostRecentDepthIsValid)
{
return;
}
APIImageBuffer apiImage;
apiImage.width = image.width;
apiImage.height = image.height;
apiImage.stride = image.stride;
apiImage.timestamp = image.timestamp;
apiImage.format = (int)image.format;
apiImage.data = image.data;
APIPose apiImagePose = APIPose.FromMatrix4x4(ref imagePose);
// Update the depth points to have the right value
GCHandle mostRecentDepthPointsHandle = GCHandle.Alloc(m_mostRecentDepthPoints, GCHandleType.Pinned);
m_mostRecentDepth.points = Marshal.UnsafeAddrOfPinnedArrayElement(m_mostRecentDepthPoints, 0);
GCHandle thisHandle = GCHandle.Alloc(this, GCHandleType.Pinned);
IntPtr rawUpdatedIndices;
Status result = (Status)API.Tango3DR_update(
m_context, ref m_mostRecentDepth, ref m_mostRecentDepthPose,
ref apiImage, ref apiImagePose, ref m_colorCameraIntrinsics,
out rawUpdatedIndices);
m_mostRecentDepthIsValid = false;
thisHandle.Free();
mostRecentDepthPointsHandle.Free();
if (result != Status.SUCCESS)
{
Debug.Log("Tango3DR_update returned non-success." + Environment.StackTrace);
return;
}
_AddUpdatedIndices(rawUpdatedIndices);
API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices);
}
}