Last active
May 16, 2019 09:17
-
-
Save Rosthouse/674da3f25370e0c36468f83184d1e305 to your computer and use it in GitHub Desktop.
ZED Progressial Mapper
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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