public static unsafe void Update(this DepthFrame depth, uint *color, PointCloudOfXYZRGBA cloud)
{
var fd = depth.FrameDescription;
var sensor = depth.DepthFrameSource.KinectSensor;
var pixels = fd.LengthInPixels;
MatchSize(depth, cloud);
var pPtr = cloud.Data;
var pSize = (uint)Marshal.SizeOf <PointXYZRGBA>() * pixels;
using (var dBuffer = depth.LockImageBuffer())
sensor.CoordinateMapper.MapDepthFrameToCameraSpaceUsingIntPtr(
dBuffer.UnderlyingBuffer, dBuffer.Size,
(IntPtr)pPtr, pSize);
//we have the data copied raw, but it's misaligned, as kinect is 12 bytes/pixel. we need to 'expand' the data.
//copying from the back will prevent data loss.
var vptr = (Vector3 *)pPtr;
for (var i = pixels - 1; i >= 0; i--)
{
pPtr[i].V = vptr[i];
pPtr[i].data[3] = 1;
//also copy the color in the process.
pPtr[i].RGBA = color[i];
}
}
static void Main(string[] args)
{
using (var cloud = new PointCloudOfXYZRGBA())
{
using (var reader = new PCDReader())
reader.Read(DataPath("tutorials/table_scene_mug_stereo_textured.pcd"), cloud);
using (var visualizer = new Visualizer("a window"))
{
visualizer.AddPointCloud(cloud);
visualizer.SetPointCloudRenderingProperties(RenderingProperties.PointSize, 2);
visualizer.SetPointCloudRenderingProperties(RenderingProperties.Opacity, 0.95);
while (!visualizer.WasStopped)
{
visualizer.SpinOnce(100);
}
}
}
}
public unsafe void SupervoxelClusteringTutorialTest()
{
var voxelResolution = 0.008f;
var seedResolution = 0.1f;
var colorImportance = 0.2f;
var spatialImportance = 0.4f;
var normalImportance = 1f;
using (var cloud = new PointCloudOfXYZRGBA())
using (var adjacentSupervoxelCenters = new PointCloudOfXYZRGBA())
{
using (var reader = new PCDReader())
//Assert.AreEqual(0, reader.Read(DataPath("tutorials/correspondence_grouping/milk_cartoon_all_small_clorox.pcd"), cloud));
Assert.AreEqual(0, reader.Read(DataPath("tutorials/table_scene_mug_stereo_textured.pcd"), cloud));
var min = new Vector3(float.MaxValue);
var max = new Vector3(float.MinValue);
foreach (var p in cloud.Points)
{
min = Vector3.Min(min, p.V);
max = Vector3.Max(max, p.V);
}
using (var normals = new PointCloudOfNormal(cloud.Width, cloud.Height))
using (var super = new Segmentation.SupervoxelClusteringOfXYZRGBA(voxelResolution, seedResolution))
using (var clusters = new Segmentation.SupervoxelClustersOfXYZRGBA())
{
using (var ne = new IntegralImageNormalEstimationPointXYZAndNormal())
using (var noColor = new PointCloudOfXYZ(cloud.Width, cloud.Height))
{
var count = cloud.Count;
var cptr = cloud.Data;
var nptr = noColor.Data;
for (var i = 0; i < count; i++)
{
(nptr + i)->V = (cptr + i)->V;
}
ne.SetNormalEstimationMethod(IntegralImageNormalEstimation.NormalEstimationMethod.Average3DGradient);
ne.SetMaxDepthChangeFactor(0.02f);
ne.SetNormalSmoothingSize(10f);
ne.SetInputCloud(noColor);
ne.Compute(normals);
}
super.SetInputCloud(cloud);
super.SetNormalCloud(normals);
super.SetColorImportance(colorImportance);
super.SetSpatialImportance(spatialImportance);
super.SetNormalImportance(normalImportance);
super.Extract(clusters);
Assert.IsTrue(clusters.Count > 0);
using (var adjacency = new MultiMapOfuintAnduint())
{
super.GetSupervoxelAdjacency(adjacency);
Assert.AreEqual(350, adjacency.Count);
var i = 0;
foreach (var kvp in adjacency)
{
i++;
}
Assert.AreEqual(350, i);
using (var labelItr = adjacency.Begin())
using (var end = adjacency.End())
{
for (; !labelItr.Equals(end);)
{
var supervoxelLabel = labelItr.Key;
var supervoxel = clusters.At(supervoxelLabel);
foreach (var kvp in adjacency.EqualRange(supervoxelLabel))
{
var neighbor = clusters.At(kvp.Value);
adjacentSupervoxelCenters.Add(neighbor.Centroid);
}
adjacency.UpperBound(supervoxelLabel, labelItr);
}
}
Assert.AreEqual(350, adjacentSupervoxelCenters.Count);
}
}
}
}
