Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[Unity] Textured Point Cloud #1293

Merged
merged 3 commits into from
Mar 19, 2018
Merged

[Unity] Textured Point Cloud #1293

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5be0599
Additions to c# wrapper:
zivsha Mar 4, 2018
f52ced9
Updated PointCloudGenerator for unity to support colored texture
zivsha Mar 4, 2018
2cd2c43
Removing redundant allocations in pointcloud texture
zivsha Mar 15, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 34 additions & 2 deletions wrappers/csharp/Intel.RealSense/Frame.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,11 @@ public struct Vertex
public float y;
public float z;
}

public struct TextureCoordinate
{
public float u;
public float v;
}
public Points(IntPtr ptr) : base(ptr)
{
}
Expand All @@ -229,7 +233,6 @@ private IntPtr VertexData
}
}


/// <summary>
/// Copy frame data to Vertex array
/// </summary>
Expand All @@ -248,6 +251,35 @@ public void CopyTo(Vertex[] array)
handle.Free();
}
}

private IntPtr TextureData
{
get
{
object error;
return NativeMethods.rs2_get_frame_texture_coordinates(m_instance.Handle, out error);
}
}
/// <summary>
/// Copy frame data to TextureCoordinate array
/// </summary>
/// <param name="textureArray"></param>
public void CopyTo(TextureCoordinate[] textureArray)
{
if (textureArray == null)
throw new ArgumentNullException("textureArray");

var handle = GCHandle.Alloc(textureArray, GCHandleType.Pinned);
try
{
var size = Count * Marshal.SizeOf(typeof(TextureCoordinate));
NativeMethods.memcpy(handle.AddrOfPinnedObject(), TextureData, size);
}
finally
{
handle.Free();
}
}
}


Expand Down
13 changes: 12 additions & 1 deletion wrappers/csharp/Intel.RealSense/FrameQueue.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,12 @@ public FrameQueue(int capacity = 10)
public bool PollForFrame(out Frame frame)
{
object error;
return NativeMethods.rs2_poll_for_frame(m_instance.Handle, out frame, out error) > 0;
if(NativeMethods.rs2_poll_for_frame(m_instance.Handle, out frame, out error) > 0)
{
frame = FrameSet.CreateFrame(frame.m_instance.Handle);
return true;
}
return false;
}

public Frame WaitForFrame()
Expand All @@ -39,6 +44,12 @@ public FrameSet WaitForFrames()
return frame;
}

public void Enqueue(Frame f)
{
object error;
NativeMethods.rs2_frame_add_ref(f.m_instance.Handle, out error);
NativeMethods.rs2_enqueue_frame(f.m_instance.Handle, m_instance.Handle);
}
#region IDisposable Support
private bool disposedValue = false; // To detect redundant calls

Expand Down
7 changes: 7 additions & 0 deletions wrappers/csharp/Intel.RealSense/Processing.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,13 @@ public Points Calculate(Frame original)
NativeMethods.rs2_process_frame(m_instance.Handle, original.m_instance.Handle, out error);
return queue.WaitForFrame() as Points;
}
public void MapTexture(VideoFrame texture)
{
object error;
Options[Option.TextureSource].Value = Convert.ToSingle(texture.Profile.UniqueID);
NativeMethods.rs2_frame_add_ref(texture.m_instance.Handle, out error);
NativeMethods.rs2_process_frame(m_instance.Handle, texture.m_instance.Handle, out error);
}
}

}
15 changes: 15 additions & 0 deletions wrappers/unity/Assets/RealSenseSDK2.0/Scenes/RealSenseTextures.unity
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -285,6 +285,21 @@ Prefab:
propertyPath: textureBinding.m_PersistentCalls.m_Calls.Array.data[0].m_Target
value:
objectReference: {fileID: 312912468}
- target: {fileID: 198902074929829518, guid: c88c4d8330157094da830413654dd969,
type: 2}
propertyPath: randomSeed
value: -239482275
objectReference: {fileID: 0}
- target: {fileID: 198902074929829518, guid: c88c4d8330157094da830413654dd969,
type: 2}
propertyPath: EmissionModule.enabled
value: 0
objectReference: {fileID: 0}
- target: {fileID: 198902074929829518, guid: c88c4d8330157094da830413654dd969,
type: 2}
propertyPath: ShapeModule.enabled
value: 0
objectReference: {fileID: 0}
m_RemovedComponents: []
m_ParentPrefab: {fileID: 100100000, guid: c88c4d8330157094da830413654dd969, type: 2}
m_IsPrefabParent: 0
Expand Down
191 changes: 95 additions & 96 deletions wrappers/unity/Assets/RealSenseSDK2.0/Scripts/PointCloudGenerator.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,134 +10,133 @@

public class PointCloudGenerator : MonoBehaviour
{
private int streamWidth;
private int streamHeight;
private int totalImageSize;
private int particleSize;
private int particleCount;
private ParticleSystem.Particle[] particles;
private ParticleSystem.Particle[] particles = new ParticleSystem.Particle[0];
private PointCloud pc = new PointCloud();
public UnityEngine.Gradient gradient;
FrameQueue pointsQueue = new FrameQueue(1);
Points.Vertex[] vertices;
Points.TextureCoordinate[] textureCoordinate;
private byte[] lastColorImage;
Texture2D colorTexture;
private int colorFrameWidth;
private int colorFrameHeight;

public bool mirrored;
public float pointsSize = 0.01f;
public int skipParticles = 2;
public ParticleSystem pointCloudParticles;

// Use this for initialization
void Start()
{
Application.runInBackground = true;
particleCount = 0;
//RealSenseDevice.Instance.onNewSample += OnFrame;
RealSenseDevice.Instance.onNewSampleSet += OnFrames;

if (RealSenseDevice.Instance.ActiveProfile != null)
{
OnStartStreaming(RealSenseDevice.Instance.ActiveProfile);
}
else
{
RealSenseDevice.Instance.OnStart += OnStartStreaming;
}
particles = new ParticleSystem.Particle[particleSize];
}

private void OnStartStreaming(PipelineProfile activeProfile)
object l = new object();
private void OnFrames(FrameSet frames)
{
if(InitializeStream(activeProfile))
if(frames.DepthFrame == null)
{
RealSenseDevice.Instance.onNewSample += OnFrame;
Debug.Log("No depth frame in frameset, can't create point cloud");
return;
}
}

private void OnFrame(Frame frame)
{
if (frame.Profile.Stream == Intel.RealSense.Stream.Depth)
if (!UpdateParticleParams(frames.DepthFrame.Width, frames.DepthFrame.Height))
{
var depthFrame = frame as DepthFrame;
if (depthFrame == null)
{
Debug.Log("Frame is not a depth frame");
return;
}

UpdateParticleParams(depthFrame.Width, depthFrame.Height, depthFrame.Profile.Format);

var points = pc.Calculate(frame);
Debug.Log("Unable to craete point cloud");
return;
}

Points.Vertex[] vertices = new Points.Vertex[points.Count];
points.CopyTo(vertices);
for (int index = 0; index < vertices.Length; index += skipParticles)
using (var points = pc.Calculate(frames.DepthFrame))
{
if (frames.ColorFrame != null)
{
var v = vertices[index];
if (v.z > 0)
{
particles[index].position = new Vector3(v.x, v.y, v.z);
particles[index].startSize = pointsSize;
particles[index].startColor = gradient.Evaluate(v.z);
}
else
if (frames.ColorFrame.BitsPerPixel == 24)
{
particles[index].position = new Vector3(0, 0, 0);
particles[index].startSize = (float)0.0;
particles[index].startColor = new Color32(0, 0, 0, 0);
pc.MapTexture(frames.ColorFrame);
colorFrameWidth = frames.ColorFrame.Width;
colorFrameHeight = frames.ColorFrame.Height;
var newSize = frames.ColorFrame.Stride * colorFrameHeight;
lock (l)
{
if (lastColorImage == null || lastColorImage.Length != newSize)
lastColorImage = new byte[newSize];

frames.ColorFrame.CopyTo(lastColorImage);
}
}
}
}
else if(frame.Profile.Stream == Intel.RealSense.Stream.Color)
{
//pc.MapTexture(frame);
pointsQueue.Enqueue(points);
}
}

private void UpdateParticleParams(int width, int height, Format format)
private bool UpdateParticleParams(int width, int height)
{
streamWidth = width;
streamHeight = height;

if (format != Format.Z16)
var numParticles = (width * height);
if (particles.Length != numParticles)
{
Debug.Log("Unsupported format");
return;
particles = new ParticleSystem.Particle[numParticles];
}

const int bpp = 2;

if (totalImageSize != streamWidth * streamHeight * bpp)
{
totalImageSize = streamWidth * streamHeight * bpp;
particleSize = totalImageSize / skipParticles;
particles = new ParticleSystem.Particle[particleSize];
}

if (particleSize != totalImageSize / skipParticles)
{
particleSize = totalImageSize / skipParticles;
particles = new ParticleSystem.Particle[particleSize];
}
particleCount = particleSize;
return true;
}

void Update()
{
//TODO: Lock & copy particles?
pointCloudParticles.SetParticles(particles, particleCount);
}

private bool InitializeStream(PipelineProfile activeProfile)
{
var depthStream = activeProfile.Streams.FirstOrDefault(s => s.Stream == Intel.RealSense.Stream.Depth);
if (depthStream == null)
Frame frame;
if (pointsQueue.PollForFrame(out frame))
{
Debug.Log("No Depth stream available");
return false;
}
var depthProfile = depthStream as VideoStreamProfile;
//depthIntrinsic = depthProfile.GetIntrinsics();
streamWidth = depthProfile.Width;
streamHeight = depthProfile.Height;
return true;
}
using (Points points = frame as Points)
{
if (points == null)
throw new Exception("Frame in queue is not a points frame");

private float Remap(float value, float from1, float to1, float from2, float to2)
{
return (value - from1) / (to1 - from1) * (to2 - from2) + from2;
vertices = vertices ?? new Points.Vertex[points.Count];
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do not use this for Unity. Vertex matches Vector3, TextureCoordinate matches Vector2.
Use a Vector3[] instead of Vertex[] and manually marshal the array.

Then PointCloudGenerator.cs#L126 wouldn't allocate so much.
You're also allocating here: PointCloudGenerator.cs#L132, that's what Vector3.Zero is for...

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sounds right, how much do you think this affects performance though?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

One less copy per vertex should help.
Still, there's more to do, particles aren't really the most performant way of rendering point clouds... Meshes are better suited, since you can update all of Mesh.vertices at once with one write (assuming you already have the pointcloud as a Vector3[])

Copy link
Contributor Author

@zivsha zivsha Mar 15, 2018 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ev-mp , I've removed the redundant allocation PointCloudGenerator.cs#L132 in the next commit, but the vertex copy still requires change.

points.CopyTo(vertices);

lock (l)
{
if (textureCoordinate == null || textureCoordinate.Length != points.Count)
textureCoordinate = new Points.TextureCoordinate[points.Count];

points.CopyTo(textureCoordinate);

if (lastColorImage != null)
{
if (colorTexture == null || colorTexture.width != colorFrameWidth || colorTexture.height != colorFrameHeight)
{
colorTexture = new Texture2D(colorFrameWidth, colorFrameHeight, TextureFormat.RGB24, false, true)
{
wrapMode = TextureWrapMode.Clamp,
filterMode = FilterMode.Point
};
}

colorTexture.LoadRawTextureData(lastColorImage);
colorTexture.Apply();
}
}
Debug.Assert(vertices.Length == particles.Length);
int mirror = mirrored ? -1 : 1;
for (int index = 0; index < vertices.Length; index += skipParticles)
{
var v = vertices[index];
if (v.z > 0)
{
particles[index].position = new Vector3(v.x * mirror, v.y, v.z);
particles[index].startSize = pointsSize;
particles[index].startColor = colorTexture.GetPixelBilinear(textureCoordinate[index].u, textureCoordinate[index].v);
}
else //Required since we reuse the array
{
particles[index].position = Vector3.zero;
particles[index].startSize = 0;
particles[index].startColor = Color.black;
}
}
}
}
//Either way, update particles
pointCloudParticles.SetParticles(particles, particles.Length);
}
}