public void UpdateSLAMPointCloud()
{
int Error = 0;
var pose = new List <HelixToolkit.Wpf.SharpDX.Geometry3D.Line>();
var line = new LineBuilder();
Vector3 previousPose = new Vector3(Convert.ToSingle(poseX), Convert.ToSingle(poseY), Convert.ToSingle(poseZ));
while (IsSLAMOn)
{
var vc = new Vector3Collection();
var id = new IntCollection();
var cc = new Color4Collection();
var poseVect = new List <double[]>();
var colorVect = new List <double[]>();
Vector3 currentPose = new Vector3(Convert.ToSingle(poseX), Convert.ToSingle(poseY), Convert.ToSingle(poseZ));
line.AddLine(previousPose, currentPose);
SLAMPoseInfo = line.ToLineGeometry3D();
previousPose = currentPose;
try
{
RealsenseControl.GetSLAMPointCloud(ref poseVect, ref colorVect, T265ToLACCOffset.X, T265ToLACCOffset.Y, T265ToLACCOffset.Z);
for (int i = 0; i < poseVect.Count; i++)
{
vc.Add(new Vector3(Convert.ToSingle(poseVect[i][0]), Convert.ToSingle(poseVect[i][1]), Convert.ToSingle(poseVect[i][2])));
cc.Add(new Color4(0.1f, 0.1f, 0.1f, 0.5f));
//cc.Add(new Color4(Convert.ToSingle(colorVect[i][0]), Convert.ToSingle(colorVect[i][1]), Convert.ToSingle(colorVect[i][2]), 0.5f));
id.Add(i);
}
SLAMPointCloud = new PointGeometry3D()
{
Positions = vc, Indices = id, Colors = cc
};
}
catch
{
Error++;
Trace.WriteLine("Error Count is " + Error);
}
Thread.Sleep(50);
}
}
public void UpdateSLAMPointCloud()
{
var line = new LineBuilder();
Vector3 previousPose = new Vector3(Convert.ToSingle(systemPoseX), Convert.ToSingle(systemPoseY), Convert.ToSingle(systemPoseZ));
var poseVect = new List <double[]>();
var colorVect = new List <double[]>();
Point3D lineVect = new Point3D(0, 0, 0.3);
while (IsSLAMOn)
{
Thread.Sleep(500);
var vc = new Vector3Collection();
var cc = new Color4Collection();
poseVect = new List <double[]>();
colorVect = new List <double[]>();
Vector3 currentPose = new Vector3(Convert.ToSingle(systemPoseX), Convert.ToSingle(systemPoseY), Convert.ToSingle(systemPoseZ));
line.AddLine(previousPose, currentPose);
SLAMPoseInfo = line.ToLineGeometry3D();
previousPose = currentPose;
RealsenseControl.GetSLAMPointCloud(ref poseVect, ref colorVect);
for (int i = 0; i < poseVect.Count; i++)
{
vc.Add(new Vector3(Convert.ToSingle(poseVect[i][0]), Convert.ToSingle(poseVect[i][1]), Convert.ToSingle(poseVect[i][2])));
//cc.Add(new Color4(0.1f, 0.1f, 0.1f, 0.5f));
cc.Add(new Color4(Convert.ToSingle(colorVect[i][0]), Convert.ToSingle(colorVect[i][1]), Convert.ToSingle(colorVect[i][2]), 0.5f));
//id.Add(i);
}
for (int i = 0; i < 10; ++i)
{
var pointx = new Point3D(0.1 * i, 0, 0);
var pointy = new Point3D(0, 0.1 * i, 0);
var pointz = new Point3D(0, 0, 0.1 * i);
vc.Add(new Vector3(Convert.ToSingle(pointx.X), Convert.ToSingle(pointx.Y), Convert.ToSingle(pointx.Z)));
cc.Add(new Color4(1f, 0f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointy.X), Convert.ToSingle(pointy.Y), Convert.ToSingle(pointy.Z)));
cc.Add(new Color4(0f, 1f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointz.X), Convert.ToSingle(pointz.Y), Convert.ToSingle(pointz.Z)));
cc.Add(new Color4(0f, 0f, 1f, 1f));
}
for (int i = 0; i < 10; ++i)
{
var pointx = currentSystemTranformation.Transform(new Point3D(0.1 * i, 0, 0));
var pointy = currentSystemTranformation.Transform(new Point3D(0, 0.1 * i, 0));
var pointz = currentSystemTranformation.Transform(new Point3D(0, 0, 0.1 * i));
vc.Add(new Vector3(Convert.ToSingle(pointx.X), Convert.ToSingle(pointx.Y), Convert.ToSingle(pointx.Z)));
cc.Add(new Color4(1f, 0f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointy.X), Convert.ToSingle(pointy.Y), Convert.ToSingle(pointy.Z)));
cc.Add(new Color4(0f, 1f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointz.X), Convert.ToSingle(pointz.Y), Convert.ToSingle(pointz.Z)));
cc.Add(new Color4(0f, 0f, 1f, 1f));
}
SLAMPointCloud = new PointGeometry3D()
{
Positions = vc, Colors = cc
};
SLAMVector3s = vc;
}
Debug.WriteLine("SLAM Point Cloud Count is " + poseVect.Count);
}