public static unsafe void Update(this DepthFrame depth, PointCloudOfXYZ 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 <PointXYZ>() * 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; // just in case...
}
}
public void OrganizedExtractionTest()
{
using (var cloud = new PointCloudOfXYZ())
using (var clusterIndices = new VectorOfPointIndices())
{
using (var reader = new PCDReader())
reader.Read(DataPath("tutorials/table_scene_mug_stereo_textured.pcd"), cloud);
int organizedClusterCount;
using (var ec = new EuclideanClusterExtractionOfXYZ
{
ClusterTolerance = 0.01,
MinClusterSize = 100,
MaxClusterSize = 800000
})
{
using (var organized = new Search.OrganizedNeighborOfXYZ())
using (var tree = new Search.KdTreeOfXYZ())
{
organized.SetInputCloud(cloud);
ec.SetSearchMethod(organized);
ec.Extract(clusterIndices);
organizedClusterCount = clusterIndices.Count;
clusterIndices.Clear();
ec.SetSearchMethod(tree);
ec.Extract(clusterIndices);
Assert.AreEqual(clusterIndices.Count, organizedClusterCount, "organized neighbor cluster count did not match kdtree cluster count");
}
}
}
}
public unsafe void TestCtorWH()
{
using (var cloud = new PointCloudOfXYZ(10, 10))
{
ref PointXYZ foo = ref cloud.At(5, 5);
foo.X = 5;
var ptr = cloud.Data;
Assert.AreEqual(5, ptr[55].X);
}
public unsafe void PlanarSegmentationTutorialTest()
{
using (var cloud = new PointCloudOfXYZ())
{
cloud.Width = 15;
cloud.Height = 1;
cloud.Points.Resize(cloud.Width * cloud.Height);
for (var i = 0; i < cloud.Points.Count; i++)
{
(cloud.Data + i)->X = 1024 * rand();
(cloud.Data + i)->Y = 1024 * rand();
(cloud.Data + i)->Z = 1;
}
//set a few outliers
(cloud.Data + 0)->Z = 2f;
(cloud.Data + 3)->Z = -2f;
(cloud.Data + 6)->Z = 4f;
using (var seg = new SACSegmentationOfXYZ())
using (var inliers = new PointIndices())
using (var coefficients = new ModelCoefficients())
{
seg.OptimizeCoefficients = true;
seg.ModelType = SACModel.Plane;
seg.MethodType = SACMethod.RANSAC;
seg.DistanceThreshold = 0.01;
seg.SetInputCloud(cloud);
seg.Segment(inliers, coefficients);
if (inliers.Indices.Count == 0)
{
Assert.Fail("Could not estimate a planar model for the given dataset");
}
Assert.AreEqual(4, coefficients.Values.Count);
Assert.AreEqual(0, coefficients.Values[0]);
Assert.AreEqual(0, coefficients.Values[1]);
Assert.AreEqual(1, coefficients.Values[2]);
Assert.AreEqual(-1, coefficients.Values[3]);
Console.WriteLine($"Model inliers: {inliers.Indices.Count}");
}
}
}
public void StatisticalOutlierRemovalTutorialTest()
{
using (var cloud = new PointCloudOfXYZ())
using (var cloudFiltered = new PointCloudOfXYZ())
{
using (var reader = new PCDReader())
reader.Read(DataPath("tutorials/table_scene_lms400.pcd"), cloud);
using (var sor = new Filters.StatisticalOutlierRemovalOfXYZ())
{
sor.SetInputCloud(cloud);
sor.MeanK = 50;
sor.StdDevMulThresh = 1;
sor.filter(cloudFiltered);
Assert.IsTrue(cloudFiltered.Count > 0);
Assert.IsTrue(cloudFiltered.Count < cloud.Count);
}
}
}
public void TestIntegralFile()
{
//copying integral image tutorial from http://pointclouds.org/documentation/tutorials/normal_estimation_using_integral_images.php#normal-estimation-using-integral-images
using (var cloud = new PointCloudOfXYZ())
using (var normals = new PointCloudOfNormal())
using (var ne = new IntegralImageNormalEstimationPointXYZAndNormal())
{
using (var reader = new PCDReader())
if (reader.Read(DataPath("tutorials/table_scene_mug_stereo_textured.pcd"), cloud) < 0)
{
Assert.Fail("could not open pcd file");
}
ne.SetNormalEstimationMethod(IntegralImageNormalEstimation.NormalEstimationMethod.Average3DGradient);
ne.SetMaxDepthChangeFactor(0.02f);
ne.SetNormalSmoothingSize(10f);
ne.SetInputCloud(cloud);
ne.Compute(normals);
Assert.IsTrue(normals.Count == cloud.Count, "normals count did not match cloud count");
}
}
private void button3_Click(object sender, EventArgs e)
{
//Filter<PointXYZ> ds = new Filter <PointXYZ>;
PointCloudOfXYZ IN_cloud = new PointCloudOfXYZ();
PointCloudOfXYZ OUT_cloud = new PointCloudOfXYZ();
gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);
double tempx = 0;
double tempy = 0;
double tempz = 0;
//listBox1.Items.Clear();
Drawxs = false;
Zcdk = true; //再次读取
// CCValue.thStart = true;
this.openFileDialog1.ShowDialog();
string MyFileName = openFileDialog1.FileName;
if (MyFileName.Trim() == "")
{
return;
}
StreamReader MyReader = null;
try
{
MyReader = new StreamReader(MyFileName, System.Text.Encoding.Default);
string content = MyReader.ReadToEnd();
string[] str = content.Split(new string[] { "\r\n" }, StringSplitOptions.None);
char[] splitchar = new Char[] { ',' };//拆分数组
string[] data1;
for (int i = 0; i < str.Length - 1; i++)
{
data1 = str[i].Split(splitchar);
a.X = float.Parse(data1[0]);
a.Y = float.Parse(data1[1]);
a.Z = float.Parse(data1[2]);
IN_cloud.Add(a);
}
// ABC.SetInputCloud(IN_cloud);
// ABC.MeanK = 20;
// ABC.StdDevMulThresh = (1.0);
// ABC.filter(OUT_cloud);
maxx = new float[IN_cloud.Count];
maxy = new float[IN_cloud.Count];
maxz = new float[IN_cloud.Count];
for (int i = 0; i < IN_cloud.Count - 1; i++)
{
a = IN_cloud.Points[i];
if (a.Z > -1500)
{
continue;
}
else
{
maxx[i] = a.X;
maxy[i] = a.Y;
maxz[i] = a.Z;
tempx += maxx[i];
tempy += maxy[i];
tempz += maxz[i];
}
}
xds = IN_cloud.Count;
_ZBxT = tempx / xds;
_ZByT = tempy / xds;
_ZBzT = tempz / xds;
}
catch (Exception Err)
{
MessageBox.Show("读文本文件发生错误！请检查源文件是否是文本文件？" + Err.Message, "信息提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
return;
}
IN_cloud.Dispose();
OUT_cloud.Dispose();
GC.Collect();
//ABC.SetInputCloud(cloud);
}
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);
}
}
}
}
public void TestInitialize()
{
cloud = new PointCloudOfXYZ();
}
public void ClusterExtractionTutorialTest()
{
using (var cloud = new PointCloudOfXYZ())
using (var clusterIndices = new VectorOfPointIndices())
{
using (var reader = new PCDReader())
reader.Read(DataPath("tutorials/table_scene_lms400.pcd"), cloud);
using (var vg = new VoxelGridOfXYZ())
{
vg.SetInputCloud(cloud);
vg.LeafSize = new Vector3(0.01f);
var cloudFiltered = new PointCloudOfXYZ();
vg.filter(cloudFiltered);
using (var seg = new SACSegmentationOfXYZ()
{
OptimizeCoefficients = true,
ModelType = SACModel.Plane,
MethodType = SACMethod.RANSAC,
MaxIterations = 100,
DistanceThreshold = 0.02f
})
using (var cloudPlane = new PointCloudOfXYZ())
using (var coefficients = new Common.ModelCoefficients())
using (var inliers = new PointIndices())
{
int i = 0;
int nrPoints = cloudFiltered.Points.Count;
while (cloudFiltered.Points.Count > 0.3 * nrPoints)
{
seg.SetInputCloud(cloudFiltered);
seg.Segment(inliers, coefficients);
if (inliers.Indices.Count == 0)
{
Assert.Fail("could not estimate a planar model for the given dataset");
}
using (var extract = new ExtractIndicesOfXYZ()
{
Negative = false
})
{
extract.SetInputCloud(cloudFiltered);
extract.SetIndices(inliers.Indices);
extract.filter(cloudPlane);
extract.Negative = true;
var cloudF = new PointCloudOfXYZ();
extract.filter(cloudF);
cloudFiltered.Dispose();
cloudFiltered = cloudF;
}
i++;
}
Assert.IsTrue(i > 1, "Didn't find more than 1 plane");
var tree = new Search.KdTreeOfXYZ();
tree.SetInputCloud(cloudFiltered);
using (var ec = new EuclideanClusterExtractionOfXYZ
{
ClusterTolerance = 0.02,
MinClusterSize = 100,
MaxClusterSize = 25000
})
{
ec.SetSearchMethod(tree);
ec.SetInputCloud(cloudFiltered);
ec.Extract(clusterIndices);
}
foreach (var pis in clusterIndices)
{
using (var cloudCluster = new PointCloudOfXYZ())
{
foreach (var pit in pis.Indices)
{
cloudCluster.Add(cloudFiltered.Points[pit]);
}
cloudCluster.Width = cloudCluster.Points.Count;
cloudCluster.Height = 1;
}
}
}
}
}
}
