Skip to content

Instantly share code, notes, and snippets.

@Rosthouse
Last active May 16, 2019 09:17
Show Gist options
  • Save Rosthouse/674da3f25370e0c36468f83184d1e305 to your computer and use it in GitHub Desktop.
Save Rosthouse/674da3f25370e0c36468f83184d1e305 to your computer and use it in GitHub Desktop.
ZED Progressial Mapper
using System;
using System.Collections;
using System.Threading.Tasks;
using sl;
using UnityEngine;
public class ProgressiveMapper
{
private ZEDCamera camera;
private float resolution;
private float range;
private bool texturing;
private int iterationCounter;
private ProgressiveMesh progMesh;
public event MeshUpdated MeshUpdated;
public bool IsMapping
{
get; private set;
}
public ProgressiveMapper(ZEDCamera wrapper) : this(wrapper, 0.5f, 5f, 1000)
{
}
public ProgressiveMapper(ZEDCamera camera, float resolution, float range, int MAX_SUBMESH = 1000)
{
this.resolution = resolution;
this.range = range;
this.camera = camera;
MeshUpdated += delegate
{
}; // MeshUpdated can never be empty. Should no listener be added, updates will be sent into the void.
camera.CreateCamera(0, true); // Enable verbose logging
}
/// <summary>
/// Enables tracking & spatial mapping on the camera.
/// </summary>
/// <param name="texturing">If true, the texture will also be applied to the mapped mesh.</param>
/// <returns>True, if the mapping could be started, false otherwise.</returns>
public bool StartMapping(bool texturing)
{
this.IsMapping = true;
this.texturing = texturing;
var qIdent = Quaternion.identity;
var start = Vector3.zero;
this.iterationCounter = 0;
this.progMesh = new ProgressiveMesh(1000);
if (camera.EnableSpatialMapping(resolution, range, this.texturing) != ERROR_CODE.SUCCESS)
{
Debug.Log("Spatial Mapping not enabled");
return false;
}
camera.RequestMesh();
return true;
}
public bool StopMapping(bool saveTexture)
{
this.IsMapping = false;
this.camera.DisableSpatialMapping();
if (saveTexture)
{
//TODO: Implement saving texture
}
return true;
}
/// <summary>
/// Retrieves the mesh & Texture from the camera.
/// </summary>
/// <param name="tick">Configures how much time should be spent between each iteration</param>
/// <returns>An IEnumerator. This method is thought to be used as a coroutine.</returns>
public IEnumerator UpdateMesh(float tick)
{
while (IsMapping)
{
if (camera.GetMeshRequestStatus() != sl.ERROR_CODE.SUCCESS)
{
Debug.Log("Camera not yet ready");
yield return null;
}
else
{
Debug.Log($"Iteration {this.iterationCounter}");
this.progMesh = UpdateMesh(this.progMesh);
if (texturing)
{
this.progMesh = this.ResizeMesh(this.progMesh);
this.progMesh = this.RetrieveMesh(this.progMesh, false);
this.progMesh = this.FilterMesh(progMesh);
this.progMesh = this.ResizeMesh(this.progMesh);
this.progMesh = this.RetrieveMesh(this.progMesh, false);
Task<ProgressiveMesh> applyTextureTask = this.ApplyTexture(this.progMesh);
applyTextureTask.Start();
yield return new WaitForTaskCompletion(applyTextureTask);
this.progMesh = applyTextureTask.Result;
this.progMesh = this.ResizeMesh(this.progMesh);
this.progMesh = this.RetrieveMesh(this.progMesh, true);
}
else
{
this.progMesh = this.ResizeMesh(this.progMesh);
this.progMesh = this.RetrieveMesh(this.progMesh);
}
this.MeshUpdated(this.progMesh);
this.camera.RequestMesh();
this.iterationCounter++;
yield return new WaitForSeconds(tick);
}
}
}
public ProgressiveMesh ResizeMesh(ProgressiveMesh progressiveMesh)
{
progressiveMesh.vertices = new Vector3[progressiveMesh.numVertices];
progressiveMesh.triangles = new int[3 * progressiveMesh.numTriangles];
if (texturing)
{
progressiveMesh.uvs = new Vector2[progressiveMesh.numVertices];
progressiveMesh.texture = new Texture2D(progressiveMesh.textureSize[0], progressiveMesh.textureSize[1], TextureFormat.RGB24, false);
}
return progressiveMesh;
}
private ProgressiveMesh RetrieveMesh(ProgressiveMesh progressiveMesh, bool updateTexture = false)
{
var errorCode = camera.RetrieveMesh(
progressiveMesh.vertices,
progressiveMesh.triangles,
progressiveMesh.MAX_SUBMESH,
updateTexture ? progressiveMesh.uvs : null,
updateTexture ? progressiveMesh.texture.GetNativeTexturePtr() : IntPtr.Zero
);
if (errorCode != ERROR_CODE.SUCCESS)
{
var error = camera.UpdateMesh(
progressiveMesh.numVerticesInSubmesh,
progressiveMesh.numTrianglesInSubmesh,
ref progressiveMesh.numUpdatedMeshes,
progressiveMesh.updatedIndices,
ref progressiveMesh.numVertices,
ref progressiveMesh.numTriangles,
progressiveMesh.MAX_SUBMESH
);
if (error != ERROR_CODE.SUCCESS)
{
throw new Exception($"Error while updating mesh; {error}");
}
}
return progressiveMesh;
}
private ProgressiveMesh FilterMesh(ProgressiveMesh progressiveMesh)
{
this.camera.FilterMesh(
FILTER.MEDIUM,
progressiveMesh.numVerticesInSubmesh,
progressiveMesh.numTrianglesInSubmesh,
ref progressiveMesh.numUpdatedMeshes,
progressiveMesh.updatedIndices,
ref progressiveMesh.numVertices,
ref progressiveMesh.numTriangles,
progressiveMesh.MAX_SUBMESH
);
return progressiveMesh;
}
private Task<ProgressiveMesh> ApplyTexture(ProgressiveMesh progressiveMesh)
{
Task<ProgressiveMesh> t = new Task<ProgressiveMesh>(() =>
{
try
{
this.camera.ApplyTexture(
progressiveMesh.numVerticesInSubmesh,
progressiveMesh.numTrianglesInSubmesh,
ref progressiveMesh.numUpdatedMeshes,
progressiveMesh.updatedIndices,
ref progressiveMesh.numVertices,
ref progressiveMesh.numTriangles,
progressiveMesh.textureSize,
progressiveMesh.MAX_SUBMESH
);
}
catch (Exception e)
{
Debug.LogError($"Error while applying texture: {e.Message}");
}
return progressiveMesh;
});
return t;
}
private ProgressiveMesh UpdateMesh(ProgressiveMesh progressiveMesh)
{
var error = camera.UpdateMesh(
progressiveMesh.numVerticesInSubmesh,
progressiveMesh.numTrianglesInSubmesh,
ref progressiveMesh.numUpdatedMeshes,
progressiveMesh.updatedIndices,
ref progressiveMesh.numVertices,
ref progressiveMesh.numTriangles,
progressiveMesh.MAX_SUBMESH
);
if (error != ERROR_CODE.SUCCESS)
{
throw new Exception($"Error while updating mesh; {error}");
}
return progressiveMesh;
}
/// <summary>
/// Notifies listener when the mesh is ready and can be processed.
/// </summary>
/// <param name="progMesh">The next iteration of the mesh to send.</param>
private void NotifyMeshUpdated(ref ProgressiveMesh progMesh)
{
this.MeshUpdated(progMesh);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment