public CameraSpacePoint[] GetPoints()
{
var vals = new Point3f[KinectSettings.COLOR_PIXEL_COUNT];
GetArray(0, 0, vals);
return(vals.Select(v => new CameraSpacePoint()
{
X = v.X, Y = v.Y, Z = v.Z
}).ToArray());
}
public static CameraSpacePoint[] ToCamSpacePoints(this Mat mat)
{
if (mat.Channels() == 4)
{
var vals = new Vec4f[mat.Total()];
mat.GetArray(0, 0, vals);
return(vals.Select(v => new CameraSpacePoint()
{
X = v.Item0, Y = v.Item1, Z = v.Item2
}).ToArray());
}
else
{
var vals = new Point3f[mat.Total()];
mat.GetArray(0, 0, vals);
return(vals.Select(v => new CameraSpacePoint()
{
X = v.X, Y = v.Y, Z = v.Z
}).ToArray());
}
}
private void RunThread(CancellationToken token)
{
Intel.RealSense.PointCloud pc = new Intel.RealSense.PointCloud();
DecimationFilter dec_filter = new DecimationFilter();
SpatialFilter spat_filter = new SpatialFilter();
TemporalFilter temp_filter = new TemporalFilter();
dec_filter.Options[Option.FilterMagnitude].Value = DecimationMagnitude;
spat_filter.Options[Option.FilterMagnitude].Value = SpatialMagnitude;
spat_filter.Options[Option.FilterSmoothAlpha].Value = Math.Min(1.0f, (float)SpatialSmoothAlpha);
spat_filter.Options[Option.FilterSmoothDelta].Value = (float)SpatialSmoothDelta;
temp_filter.Options[Option.FilterSmoothAlpha].Value = Math.Min(1.0f, (float)TemporalSmoothAlpha);
temp_filter.Options[Option.FilterSmoothDelta].Value = (float)TemporalSmoothDelta;
List <ProcessingBlock> filters = new List <ProcessingBlock> {
dec_filter, spat_filter, temp_filter
};
Align align_to_depth = new Align(Stream.Depth);
var cfg = new Config();
cfg.EnableStream(Stream.Depth, 640, 480);
cfg.EnableStream(Stream.Color, 1280, 720, Format.Rgb8);
var pipeline = new Pipeline();
PipelineProfile pp = null;
try
{
pp = pipeline.Start(cfg);
}
catch (Exception e)
{
RhinoApp.WriteLine("RsToolkit: " + e.Message);
return;
}
while (!token.IsCancellationRequested)
{
try
{
using (var frames = pipeline.WaitForFrames())
{
var aligned = align_to_depth.Process <FrameSet>(frames).DisposeWith(frames);
var color = aligned.ColorFrame.DisposeWith(frames);
pc.MapTexture(color);
var filtered = aligned[Stream.Depth].DisposeWith(frames);
foreach (var filter in filters)
{
filtered = filter.Process(filtered).DisposeWith(frames);
}
Points points = pc.Process <Points>(filtered);
var vertices = new Point3f[points.Count];
var tex_coords = new Point2f[points.Count];
points.CopyVertices <Point3f>(vertices);
points.CopyTextureCoords <Point2f>(tex_coords);
Debug.Assert(vertices.Length == tex_coords.Length);
// ======== CULL INVALID POINTS ========
if (true)
{
var flags = new bool[vertices.Length];
int new_size = 0;
for (int i = 0; i < vertices.Length; ++i)
{
if (vertices[i].Z > 0.1)
{
flags[i] = true;
new_size++;
}
}
var new_vertices = new Point3f[new_size];
var new_tex_coords = new Point2f[new_size];
for (int i = 0, j = 0; i < vertices.Length; ++i)
{
if (flags[i])
{
new_vertices[j] = vertices[i];
new_tex_coords[j] = tex_coords[i];
++j;
}
}
vertices = new_vertices;
tex_coords = new_tex_coords;
}
// ======== TRANSFORM ========
if (m_xform.IsValid)
{
Parallel.For(0, vertices.Length - 1, (i) =>
{
vertices[i].Transform(m_xform);
});
}
// ======== CLIP TO BOX ========
if (m_clipping_box.IsValid &&
m_clipping_box.X.Length > 0 &&
m_clipping_box.Y.Length > 0 &&
m_clipping_box.Z.Length > 0)
{
Point3d box_centre = m_clipping_box.Plane.Origin;
double minx = m_clipping_box.X.Min + box_centre.X, maxx = m_clipping_box.X.Max + box_centre.X;
double miny = m_clipping_box.Y.Min + box_centre.Y, maxy = m_clipping_box.Y.Max + box_centre.Y;
double minz = m_clipping_box.Z.Min + box_centre.Z, maxz = m_clipping_box.Z.Max + box_centre.Z;
var flags = new bool[vertices.Length];
int new_size = 0;
for (int i = 0; i < vertices.Length; ++i)
{
if (
vertices[i].X <maxx && vertices[i].X> minx &&
vertices[i].Y <maxy && vertices[i].Y> miny &&
vertices[i].Z <maxz && vertices[i].Z> minz
)
{
flags[i] = true;
new_size++;
}
}
var new_vertices = new Point3f[new_size];
var new_tex_coords = new Point2f[new_size];
for (int i = 0, j = 0; i < vertices.Length; ++i)
{
if (flags[i])
{
new_vertices[j] = vertices[i];
new_tex_coords[j] = tex_coords[i];
++j;
}
}
vertices = new_vertices;
tex_coords = new_tex_coords;
}
Debug.Assert(vertices.Length == tex_coords.Length);
var point_colors = GetPointColors(color, tex_coords);
RPointCloud new_pointcloud = new RPointCloud();
new_pointcloud.AddRange(vertices.Select(x => new Point3d(x)), point_colors);
lock (m_pointcloud)
m_pointcloud = new_pointcloud;
}
}
catch (Exception e)
{
RhinoApp.WriteLine("RsToolkit: " + e.Message);
m_is_on = false;
break;
}
}
RhinoApp.WriteLine("RsToolkit: Task cancelled.");
if (pipeline != null)
{
pipeline.Stop();
}
}