/// <summary>
/// Update the 3D Reconstruction with a new point cloud and pose.
///
/// It is expected this will get called in from the Tango binder thread.
/// </summary>
/// <param name="depth">Point cloud from Tango.</param>
/// <param name="depthPose">Pose matrix the point cloud corresponds too.</param>
private void _UpdateDepth(TangoXYZij depth, Matrix4x4 depthPose)
{
if (m_context == IntPtr.Zero)
{
Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace);
return;
}
APIPointCloud apiCloud;
apiCloud.numPoints = depth.xyz_count;
apiCloud.points = IntPtr.Zero;
apiCloud.timestamp = depth.timestamp;
// This copy is required until TangoXYZij stores its depth as XYZC.
long xyzPointerVal = depth.xyz.ToInt64();
for (int it = 0; it < depth.xyz_count; ++it)
{
int xyzIndex = it * 3;
int depthPointsIndex = it * 4;
Marshal.Copy(new IntPtr(xyzPointerVal + (xyzIndex * 4)), m_mostRecentDepthPoints, depthPointsIndex, 3);
m_mostRecentDepthPoints[depthPointsIndex + 3] = 1;
}
APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose);
if (!m_sendColorToUpdate)
{
GCHandle mostRecentDepthPointsHandle = GCHandle.Alloc(m_mostRecentDepthPoints, GCHandleType.Pinned);
apiCloud.points = Marshal.UnsafeAddrOfPinnedArrayElement(m_mostRecentDepthPoints, 0);
// No need to wait for a color image, update reconstruction immediately.
IntPtr rawUpdatedIndices;
Status result = (Status)API.Tango3DR_update(
m_context, ref apiCloud, ref apiDepthPose, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero,
out rawUpdatedIndices);
if (result != Status.SUCCESS)
{
Debug.Log("Tango3DR_update returned non-success." + Environment.StackTrace);
return;
}
_AddUpdatedIndices(rawUpdatedIndices);
API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices);
mostRecentDepthPointsHandle.Free();
}
else
{
lock (m_lockObject)
{
// We need both a color image and a depth cloud to update reconstruction. Cache the depth cloud
// because there are much less depth points than pixels.
m_mostRecentDepth = apiCloud;
m_mostRecentDepthPose = apiDepthPose;
m_mostRecentDepthIsValid = true;
}
}
}
/// <summary>
/// Update the 3D Reconstruction with a new image and pose.
///
/// It is expected this will get called in from the Tango binder thread.
/// </summary>
/// <param name="image">Color image from Tango.</param>
/// <param name="imagePose">Pose matrix the color image corresponds too.</param>
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);
}
}
/// <summary>
/// Update the 3D Reconstruction with a new point cloud and pose.
///
/// It is expected this will get called in from the Tango binder thread.
/// </summary>
/// <param name="depth">Point cloud from Tango.</param>
/// <param name="depthPose">Pose matrix the point cloud corresponds too.</param>
private void _UpdateDepth(TangoXYZij depth, Matrix4x4 depthPose)
{
if (m_context == IntPtr.Zero)
{
Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace);
return;
}
APIPointCloud apiCloud;
apiCloud.numPoints = depth.xyz_count;
apiCloud.points = depth.xyz;
apiCloud.timestamp = depth.timestamp;
APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose);
if (!m_sendColorToUpdate)
{
// No need to wait for a color image, update reconstruction immediately.
IntPtr rawUpdatedIndices;
Status result = (Status)API.Tango3DR_update(
m_context, ref apiCloud, ref apiDepthPose, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero,
out rawUpdatedIndices);
if (result != Status.SUCCESS)
{
Debug.Log("Tango3DR_update returned non-success." + Environment.StackTrace);
return;
}
_AddUpdatedIndices(rawUpdatedIndices);
API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices);
}
else
{
lock (m_lockObject)
{
// We need both a color image and a depth cloud to update reconstruction. Cache the depth cloud
// because there are much less depth points than pixels.
m_mostRecentDepth = apiCloud;
m_mostRecentDepth.points = IntPtr.Zero;
m_mostRecentDepthPose = apiDepthPose;
Marshal.Copy(apiCloud.points, m_mostRecentDepthPoints, 0, apiCloud.numPoints * 3);
m_mostRecentDepthIsValid = true;
}
}
}