public void CombinedPositionAndOrientationInOutTest()
{
this.mlocmoq = new MarkerLocations();
this.sensor = new MarkerSensor(this.mlocmoq, 5, this.posDistType.Object, this.oriDistType.Object);
for (int i = 0; i < this.bulkData.Length; i++)
{
Pose marker = new Pose(
new Vector3(
this.bulkData[i][0],
this.bulkData[i][1],
this.bulkData[i][2]),
new Vector3(this.bulkData[i][3], this.bulkData[i][4], this.bulkData[i][5]));
Pose measurement = new Pose(
new Vector3(
this.bulkData[i][6],
this.bulkData[i][7],
this.bulkData[i][8]),
new Vector3(this.bulkData[i][9], this.bulkData[i][10], this.bulkData[i][11]));
Pose output = new Pose(
new Vector3(this.bulkData[i][12], this.bulkData[i][13], this.bulkData[i][14]),
new Vector3(this.bulkData[i][15], this.bulkData[i][16], this.bulkData[i][17]));
this.mlocmoq.AddMarker(i, marker);
this.TestComplexInOut(measurement, output, i);
}
}
public void Init()
{
this.mlocmoq = new MarkerLocations();
this.mlocmoq.AddMarker(
0,
new Pose(new Vector3(25, 13.52f, 5), new Vector3(0, 0, 0)));
this.mlocmoq.AddMarker(
1,
new Pose(new Vector3(12, 21.12f, 13), new Vector3(0, 0, 0)));
this.posDistType = new Mock <IDistribution>();
this.oriDistType = new Mock <IDistribution>();
this.sensor = new MarkerSensor(this.mlocmoq, this.posDistType.Object, this.oriDistType.Object);
}
public void SimpleUserPositionTest2()
{
Pose marker = new Pose(new Vector3(1, 0, 1), new Vector3(0, 0, 0));
Pose rel = new Pose(new Vector3(2, 0, 1), new Vector3(0, 180, 0));
MarkerLocations mloc = new MarkerLocations();
mloc.AddMarker(1, marker);
this.sensor = new MarkerSensor(mloc, 5, this.posDistType.Object, this.oriDistType.Object);
Dictionary <int, Pose> dic = new Dictionary <int, Pose>();
dic.Add(1, rel);
this.sensor.UpdateLocations(1, dic);
this.AssertVectorAreEqual(new Vector3(3, 0, 2), this.sensor.GetLastPosition().Data, this.vectorEpsilon);
}
public void GetMeasurementAtTimeStamp()
{
Pose marker = new Pose(new Vector3(2, 0, 1), new Vector3(0, 0, 0));
Pose rel = new Pose(new Vector3(1, 0, 1), new Vector3(0, 180, 0));
MarkerLocations mloc = new MarkerLocations();
mloc.AddMarker(1, marker);
this.sensor = new MarkerSensor(mloc, 5, this.posDistType.Object, this.oriDistType.Object);
Dictionary <int, Pose> dic = new Dictionary <int, Pose>();
dic.Add(1, rel);
this.sensor.UpdateLocations(1, dic);
this.AssertVectorAreEqual(new Vector3(3, 0, 2), this.sensor.GetPosition(1).Data, this.vectorEpsilon);
this.AssertRotationAreEqual(new Vector3(0, 180, 0), this.sensor.GetOrientation(1).Data, this.vectorEpsilon);
}
public void TestOverflow()
{
this.sensor = new MarkerSensor(this.mlocmoq, 6, this.posDistType.Object, this.oriDistType.Object);
Dictionary <int, Pose> dic = new Dictionary <int, Pose>();
dic.Add(1, new Pose(new Vector3(1, 2, 3), new Vector3(90, 180, 270)));
Pose pose2 = new Pose(new Vector3(4, 5, 6), new Vector3(90, 180, 270));
dic.Add(0, pose2);
this.sensor.UpdateLocations(this.defaultTimeStamp, dic);
this.sensor.UpdateLocations(this.defaultTimeStamp, dic);
this.sensor.UpdateLocations(this.defaultTimeStamp, dic);
this.sensor.UpdateLocations(this.defaultTimeStamp, dic);
this.sensor.UpdateLocations(this.defaultTimeStamp, dic);
Assert.AreEqual(6, this.sensor.GetAllOrientations().Count);
}
public Pose RunOnRawMarkerData()
{
const Int32 BufferSize = 128;
using (var fileStream = File.OpenRead(TestContext.CurrentContext.TestDirectory + "\\P1OR180.txt"))
using (var streamReader = new StreamReader(fileStream, Encoding.UTF8, true, BufferSize))
{
ParticleFilter filter = new ParticleFilter(new double[] { 5, 2, 5, 360, 360, 360 }, 30, 0.005);
MarkerSensor msens = new MarkerSensor(1, TestContext.CurrentContext.TestDirectory + "\\MarkerMapRealistic.xml");
filter.AddOrientationSource(msens);
filter.AddPositionSource(msens);
Dictionary <int, Pose> dic = new Dictionary <int, Pose>();
Pose pose = null;
string line;
for (int i = 0; i < 10; i++)
{
line = streamReader.ReadLine();
string[] strdata = line.Split(' ');
float[] posxyz = new float[]
{ Convert.ToSingle(strdata[3]), Convert.ToSingle(strdata[4]), Convert.ToSingle(strdata[5]) };
float[] orixyz = new float[]
{ Convert.ToSingle(strdata[0]), Convert.ToSingle(strdata[1]), Convert.ToSingle(strdata[2]) };
dic.Add(0,
new Pose(new Vector3(posxyz),
new Vector3(orixyz)));
msens.UpdateLocations(dic);
pose = filter.CalculatePose(i);
dic.Clear();
Assert.AreEqual(1, msens.GetLastPosition().Data.X, 0.5);
Assert.AreEqual(1, msens.GetLastPosition().Data.Y, 0.5);
Assert.AreEqual(1, msens.GetLastPosition().Data.Z, 0.5);
//Assert.AreEqual(0, msens.GetLastOrientation().Data.X, 8);
//Assert.AreEqual(90, msens.GetLastOrientation().Data.Y, 8);
//Assert.AreEqual(0, msens.GetLastOrientation().Data.Z, 8);
}
return(pose);
}
}
/// <summary>
/// Method called when creating a UserController.
/// </summary>
public override void Init()
{
this.markerSensor = new MarkerSensor(new MarkerLocations(Path), new Normal(this.orientationStd), new Normal(this.positionStd));
this.markerDetector = MarkerDetector.Instance;
this.markerTransform = new GameObject("UsedToCreateTransform").transform;
}
