public void GetAnimatedPoseTest()
{
IpObject target = CreateIpObject(); // TODO: Initialize to an appropriate value
target.SetKeyframe(0);
target.Position = new Point3D(1, 1, 1);
target.Orientation = new AxisAngle3D()
{
X = 1,
Y = 1,
Z = 1
};
target.SetKeyframe(10);
double time = 0F; // TODO: Initialize to an appropriate value
Pose3D expected = new Pose3D()
{
Orientation = target.Orientation,
Position = target.Position,
Time = 10
}; // TODO: Initialize to an appropriate value
Pose3D actual;
var intermediates = new List <Pose3D>();
for (int i = 0; i < 10; i++)
{
intermediates.Add(target.GetAnimatedPose(i));
}
actual = target.GetAnimatedPose(10);
Assert.AreEqual(expected, actual);
Assert.Inconclusive("Verify the correctness of this test method.");
}
public void Jacobian()
{
const double ds = 1e-5;
Pose3D f = a.AddOdometry(odometry2);
double[][] jacobian = a.AddOdometryJacobian(odometry2);
double[][] numerical = new double[6][];
for (int i = 0; i < 6; i++)
{
double[] da = new double[6];
da[i] = ds;
Pose3D a2 = a.AddGlobal(da);
//Pose3D f2 = a.AddOdometry(Pose3D.Identity.FromLinear(odometry2).AddGlobal(da).ToLinear());
Pose3D f2 = a2.AddOdometry(odometry2);
double[] df = f2.SubtractGlobal(f);
numerical[i] = df.Divide(ds);
}
numerical = numerical.Transpose();
Console.WriteLine(jacobian.ToString("f6"));
Console.WriteLine("---");
Console.WriteLine(numerical.ToString("f6"));
Assert.IsTrue(numerical.IsEqual(jacobian, 1e-4));
}
// Converts given |position| and |rotation| from Unity space to audio space.
private static void ConvertAudioTransformFromUnity(ref Vector3 position,
ref Quaternion rotation)
{
transformMatrix = Pose3D.FlipHandedness(Matrix4x4.TRS(position, rotation, Vector3.one));
position = transformMatrix.GetColumn(3);
rotation = Quaternion.LookRotation(transformMatrix.GetColumn(2), transformMatrix.GetColumn(1));
}
public Matrix4x4 ToMatrix4x4()
{
Matrix4x4 mat4 = (Matrix4x4)Marshal.PtrToStructure(valuePtr, typeof(Matrix4x4));
// Transpose the matrix from row-major (GVR) to column-major (Unity),
// and change from LHS to RHS coordinates.
return(Pose3D.FlipHandedness(mat4.transpose));
}
public void SendInfo(int personID, Pose3D headPose)
{
InfoMessage msg = new InfoMessage();
msg.id = personID;
msg.pose = headPose;
NetworkServer.SendToAll(MyMsgType.Info, msg);
}
public void FitGaussianMultipleLandmarks()
{
Pose3D linearpoint = new Pose3D();
double[] estimate = new double[6] {
-0.002, 0.003, 0.001, 0, 0, 0
};
Map map = new Map(3);
var measurements = new List <PixelRangeMeasurement>();
measurements.Add(new PixelRangeMeasurement(115.16312, 0, 1.019803903));
measurements.Add(new PixelRangeMeasurement(0, 57.58156, 1.004987562));
measurements.Add(new PixelRangeMeasurement(28.79078, 0, 2.002498439));
map.Add(new Gaussian(new double[3] {
0.2, 0, 1
}, identitycov, 1.0));
map.Add(new Gaussian(new double[3] {
0, 0.1, 1
}, identitycov, 1.0));
map.Add(new Gaussian(new double[3] {
0.1, 0, 2
}, identitycov, 1.0));
Gaussian expected = new Gaussian(new double[7] {
0, 0, 0, 1, 0, 0, 0
},
new double[7][] { new double[7] {
1, 0, 0, 0, 0, 0, 0
},
new double[7] {
0, 1, 0, 0, 0, 0, 0
},
new double[7] {
0, 0, 1, 0, 0, 0, 0
},
new double[7] {
0, 0, 0, 1, 0, 0, 0
},
new double[7] {
0, 0, 0, 0, 1, 0, 0
},
new double[7] {
0, 0, 0, 0, 0, 1, 0
},
new double[7] {
0, 0, 0, 0, 0, 0, 1
} },
1.0);
Gaussian result = LoopyPHDNavigator.FitGaussian(estimate, measurements, map, linearpoint);
Console.WriteLine(result);
Assert.AreEqual(expected, result);
}
private void doEventDataCallback(Pose3D pose3D)
{
for (int i = 0; i < Callbacks.Count; i++)
{
pose3d_listener gameObjectCallback = Callbacks [i];
if (gameObjectCallback != null)
{
gameObjectCallback(pose3D);
}
}
}
public void QueueReader()
{
while (!StopQueue)
{
// If the queue is empty, run the delay for checking queue
if (PoseQueue.Count <= 0)
{
Thread.Sleep(QueueCheckDelay);
try
{
ProcessFiles();
}
catch (IOException)
{
Console.WriteLine("IOException. File is probably in-use by another program.");
}
catch (UnauthorizedAccessException)
{
Console.WriteLine("UnauthorizedAccessException. File is probably in-use by another program.");
}
continue;
}
else if (PoseQueue.Count > 20)
{
PoseQueue.RemoveRange(0, PoseQueue.Count - 1);
}
// Parse saved text
JObject parsedPose = JObject.Parse(PoseQueue[0]);
PoseQueue.RemoveAt(0);
// Do JSON stuff to grab keypoints
//Console.WriteLine(parsedPose.ToString());
if (parsedPose["people"].HasValues && parsedPose["people"][0].HasValues)
{
//Console.WriteLine(parsedPose["people"].ToString());
//Console.WriteLine(parsedPose["people"][0].Value<JArray>("pose_keypoints_2d"));
//foreach (double f in parsedPose["people"][0].Value<JArray>("pose_keypoints_2d").Values<double>())
//{
// Console.WriteLine(f);
//}
double[] keypoints = new List <double>(parsedPose["people"][0].Value <JArray>("pose_keypoints_3d").Values <double>()).ToArray();
poseEventHandler.ExecuteHandlers(Pose3D.ParseDoubleArray(keypoints));
}
}
}
public void SubtractAdd()
{
Pose3D recovered = b.AddOdometry(a.DiffOdometry(b));
Assert.AreEqual(a.State[0], recovered.State[0], 1e-3);
Assert.AreEqual(a.State[1], recovered.State[1], 1e-3);
Assert.AreEqual(a.State[2], recovered.State[2], 1e-3);
Assert.AreEqual(a.State[3], recovered.State[3], 1e-3);
Assert.AreEqual(a.State[4], recovered.State[4], 1e-3);
Assert.AreEqual(a.State[5], recovered.State[5], 1e-3);
Assert.AreEqual(a.State[6], recovered.State[6], 1e-3);
}
public void FitGaussianWrongLocation()
{
Pose3D linearpoint = new Pose3D();
double[] estimate = new double[6];
Map map = new Map(3);
var measurements = new List <PixelRangeMeasurement>();
estimate[2] = -0.05;
measurements.Add(new PixelRangeMeasurement(0, 0, 1));
map.Add(new Gaussian(new double[3] {
0, 0, 1.05
}, identitycov, 1.0));
// this expectation assumes a diagonal measurement error and the identity pose
SimulatedVehicle dummy = new SimulatedVehicle();
double a = 1.0 / dummy.MeasurementCovariance[0][0];
double b = 1.0 / dummy.MeasurementCovariance[1][1];
double c = 1.0 / dummy.MeasurementCovariance[2][2];
double f2 = dummy.Measurer.VisionFocal * dummy.Measurer.VisionFocal;
double[] expstate = new double[7] {
0, 0, 0.05, 1, 0, 0, 0
};
double[][] expinfo = new double[6][] { new double[6] {
a *f2, 0, 0, 0, a *f2, 0
},
new double[6] {
0, b *f2, 0, -b * f2, 0, 0
},
new double[6] {
0, 0, c, 0, 0, 0
},
new double[6] {
0, -b * f2, 0, b *f2, 0, 0
},
new double[6] {
a *f2, 0, 0, 0, a *f2, 0
},
new double[6] {
0, 0, 0, 0, 0, 0
} };
double[][] expcov = expinfo.SingularInverse();
Gaussian result = LoopyPHDNavigator.FitGaussian(estimate, measurements, map, linearpoint);
Console.WriteLine(result.Covariance.ToString("e5"));
Console.WriteLine(result.Mean.ToString("e5"));
Assert.IsTrue(expstate.IsEqual(result.Mean, 1e-4));
Assert.IsTrue(expcov.IsEqualRelative(result.Covariance, 0.001));
}
public List <List <Vector3> > Process(
ref Area?xAll,
ref Area?yAll,
ref Area?xFrame,
ref Area?yFrame,
bool frameMode,
bool background,
Pose3D pose
) => ProcessInternal(
ref xAll,
ref yAll,
ref xFrame,
ref yFrame,
frameMode,
background,
p => (pose * p).Let(it => new Vector3(it.X, it.Y, float.NaN)));
public void FitGaussianNoMap()
{
Pose3D linearpoint = new Pose3D();
double[] estimate = new double[6];
Map map = new Map(3);
var measurements = new List <PixelRangeMeasurement>();
double[][] covariance = MatrixExtensions.Zero(6).SingularInverse();
Gaussian expected = new Gaussian(new double[6] {
0, 0, 0, 0, 0, 0
}, covariance, 1.0);
Gaussian result = LoopyPHDNavigator.FitGaussian(estimate, measurements, map, linearpoint);
Assert.IsTrue(expected.Equals(result, 1e-5));
}
// Use this for initialization
void Start()
{
netManager = GameObject.FindObjectOfType <NetworkManager> ();
netID = GetComponent <NetworkIdentity> ();
// We need this, so a player knows which camera to turn on (their own).
// Without it, all players get 1 shared camera, which is, in fact, awful for gameplay.
if (netID.isLocalPlayer)
{
mainCamera.gameObject.SetActive(true);
}
nView = GetComponent <NetworkView> ();
gameOver = false;
devicePose = Cardboard.SDK.HeadPose;
startingDir = 0;
inTurn = false;
}
public void FitMeasurementOnlyTranslation()
{
PRM3DMeasurer measurer = new PRM3DMeasurer();
Pose3D pose0 = Pose3D.Identity;
var measurement = new PixelRangeMeasurement(0, 0, 1);
double[] landmark = new double[3] {
0, 0, 2.5
};
Pose3D fitted = measurer.FitToMeasurement(pose0, measurement, landmark);
Pose3D expected = new Pose3D(new double[3] {
0, 0, 1.5
}, Quaternion.Identity);
Assert.IsTrue(expected.Equals(fitted, 1e-5));
}
public void setup()
{
Quaternion arot = Quaternion.CreateFromYawPitchRoll(0.4, 1.6, 0.1).Normalize();
Quaternion brot = Quaternion.CreateFromYawPitchRoll(0.4, 0.6, 0.5).Normalize();
a = new Pose3D(new double[3] {
0.1, 0.3, 0.2
}, arot);
b = new Pose3D(new double[3] {
0.5, -0.4, 0.7
}, brot);
odometry = new double[6] {
0.12, 2.17, 1.03, 0.21, 0.05, 1.05
};
odometry2 = new double[6] {
0.13, 0.09, 0.05, 0.02, 1.20, 0.20
};
}
public void FitMeasurementGeneral()
{
PRM3DMeasurer measurer = new PRM3DMeasurer();
Pose3D pose0 = new Pose3D(new double[3] {
2.0, 0.2, 0.1
},
new Quaternion(1, 2, 3, 4).Normalize());
var measurement = new PixelRangeMeasurement(-120, 50, 1.3);
double[] landmark = new double[3] {
0.1, -1.0, 1.2
};
Pose3D fpose = measurer.FitToMeasurement(pose0, measurement, landmark);
var measured = measurer.MeasurePerfect(fpose, landmark);
Assert.IsTrue(measurement.ToLinear().IsEqual(measured.ToLinear(), 1e-5));
}
public void Show(Matrix4x4 userMatrix, bool useRecommended, float distance, Matrix4x4 model)
{
#if UNITY_ANDROID && !UNITY_EDITOR
currentDistance = distance;
#endif // UNITY_ANDROID && !UNITY_EDITOR
if (useRecommended)
{
worldMatrix = getRecommendedMatrix(distance);
}
else
{
// Convert to GVR coordinates.
worldMatrix = Pose3D.FlipHandedness(userMatrix).transpose;
}
Matrix4x4 matToSet = worldMatrix * model.transpose;
IntPtr mat_ptr = Marshal.AllocHGlobal(Marshal.SizeOf(matToSet));
Marshal.StructureToPtr(matToSet, mat_ptr, true);
gvr_keyboard_set_world_from_keyboard_matrix(keyboard_context, mat_ptr);
gvr_keyboard_show(keyboard_context);
}
public void FitMeasurementUnmeasurableLandmark()
{
PRM3DMeasurer measurer = new PRM3DMeasurer();
Pose3D pose0 = Pose3D.Identity;
var measurement = new PixelRangeMeasurement(0, 0, 1);
double[] landmark = new double[3] {
0, 1, 0
};
Pose3D fitted = measurer.FitToMeasurement(pose0, measurement, landmark);
Quaternion rotation = new Quaternion(Math.Cos(Math.PI / 2 / 2),
(-Math.Sin(Math.PI / 2 / 2)).Multiply(new double[3] {
1, 0, 0
}));
Pose3D expected = new Pose3D(new double[3] {
0, 0, 0
}, rotation);
Assert.IsTrue(expected.Equals(fitted, 1e-5));
}
// Update is called once per frame
void Update()
{
headview = Cardboard.SDK.HeadPose;
GetComponent <Text>().text = "Heading: " + headview.Orientation.eulerAngles;
}
public void PoseInterpolate()
{
//Console.WriteLine("Test");
if (eventListener.newPose == null || eventListener.TimeBetweenPoses == 0)
{
//Console.WriteLine("Error: Can't run interpolator...");
return;
}
else if (eventListener.curPose == null || eventListener.lastPose == null)
{
eventListener.curPose = eventListener.newPose;
}
else // Interpolate
{
//Console.WriteLine("Interpolating pose...");
if (lastPose != eventListener.lastPose)
{
curPoseTimePassed = 0;
lastPose = eventListener.lastPose;
}
InterpolatorDelay = (int)Math.Floor(eventListener.TimeBetweenPoses / InterpolatorMultiplier);
double interpolateDivider = (InterpolatorDelay > 0 ? ((eventListener.TimeBetweenPoses - curPoseTimePassed) / InterpolatorDelay) : 0);
interpolateDivider = (interpolateDivider < 1 ? 1 : interpolateDivider);
if (interpolateDivider >= 1 && InterpolatorDelay > 0)
{
//Console.WriteLine("Interpolate Divider: " + interpolateDivider);
Pose newPose = eventListener.newPose;
if (typeof(Pose2D) == newPose.GetType())
{
List <KeyPoint2D> newCurPose = new List <KeyPoint2D>();
for (int i = 0; i < newPose.KeyPoints.Count; i++)
{
KeyPoint2D keyPoint = ((Pose2D)newPose).GetKeyPoint2D(i);
KeyPoint2D newKeyPoint = InterpolateKeyPoint2Ds(((Pose2D)lastPose).GetKeyPoint2D(keyPoint.BodyPoint), keyPoint, interpolateDivider);
if (newKeyPoint != null && newKeyPoint.IsValid)
{
newCurPose.Add(newKeyPoint);
}
else
{
newCurPose.Add(keyPoint);
}
}
Pose2D interPose = new Pose2D(newCurPose.ToArray());
foreach (KeyPoint2D keyPoint in newPose.KeyPoints)
{
if (interPose.GetKeyPoint2D(keyPoint.BodyPoint) == null)
{
interPose.KeyPoints.Add(keyPoint);
}
}
eventListener.curPose = interPose;
}
else if (typeof(Pose3D) == newPose.GetType())
{
List <KeyPoint3D> newCurPose = new List <KeyPoint3D>();
for (int i = 0; i < newPose.KeyPoints.Count; i++)
{
KeyPoint3D keyPoint = ((Pose3D)newPose).GetKeyPoint3D(i);
KeyPoint3D newKeyPoint = InterpolateKeyPoint3Ds(((Pose3D)lastPose).GetKeyPoint3D(keyPoint.BodyPoint), keyPoint, interpolateDivider);
if (newKeyPoint != null && newKeyPoint.IsValid)
{
newCurPose.Add(newKeyPoint);
}
else
{
newCurPose.Add(keyPoint);
}
}
Pose3D interPose = new Pose3D(newCurPose.ToArray());
foreach (KeyPoint3D keyPoint in newPose.KeyPoints)
{
if (interPose.GetKeyPoint3D(keyPoint.BodyPoint) == null)
{
interPose.KeyPoints.Add(keyPoint);
}
}
eventListener.curPose = interPose;
}
}
}
//Console.WriteLine("Sending pose...");
SendPose(eventListener.curPose);
}
public void SendPose(Pose pose)
{
try
{
//foreach (KeyPoint point in pose.KeyPoints)
//{
// Console.WriteLine(point.ToString());
//}
if (pose != null && typeof(Pose2D) == pose.GetType())
{
Pose2D pose2D = (Pose2D)pose;
KeyPoint2D nose = pose2D.GetKeyPoint2D(BodyPoint.Nose_or_Top_Head);
//KeyPoint2D leftEye = pose2D.GetKeyPoint2D(BodyPoint.Left_Eye);
//KeyPoint2D rightEye = pose2D.GetKeyPoint2D(BodyPoint.Right_Eye);
if (nose != null && nose.IsValid) //&& leftEye != null && rightEye != null && nose.IsValid && leftEye.IsValid && rightEye.IsValid)
{
//if (!HeadsetStatus)
//{
// program.headTrackingService.ChangeStatus(true);
// HeadsetStatus = true;
//}
//double height = (leftEye.Y + rightEye.Y) / 2;
program.headTrackingService.SendPositionOnly(nose.X, nose.Y, 0);
}
//else
//{
// if (HeadsetStatus)
// {
// program.headTrackingService.ChangeStatus(false);
// HeadsetStatus = false;
// }
//}
KeyPoint2D rightHand = pose2D.GetKeyPoint2D(BodyPoint.Right_Wrist);
if (rightHand != null && rightHand.IsValid)
{
program.controllerService2.SetControllerState(0, rightHand.X, rightHand.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
}
KeyPoint2D leftHand = pose2D.GetKeyPoint2D(BodyPoint.Left_Wrist);
if (leftHand != null && leftHand.IsValid)
{
program.controllerService.SetControllerState(1, leftHand.X, leftHand.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
}
}
else if (pose != null && typeof(Pose3D) == pose.GetType())
{
Pose3D pose3D = (Pose3D)pose;
KeyPoint3D nose = pose3D.GetKeyPoint3D(BodyPoint.Nose_or_Top_Head);
if (nose != null && nose.IsValid)
{
//program.headTrackingService.SendPositionOnly(nose.X, nose.Y, 0);
program.headTrackingService.SendRotationAndPosition(nose.Yaw, nose.Pitch, nose.Roll, nose.X, nose.Y, nose.Z);
//program.headTrackingService.SendAsyncOffset(0, 0, 0);
}
KeyPoint3D rightHand = pose3D.GetKeyPoint3D(BodyPoint.Right_Wrist);
if (rightHand != null && rightHand.IsValid)
{
program.controllerService2.SetControllerState(0, rightHand.X, rightHand.Y, rightHand.Z, rightHand.Yaw, rightHand.Pitch, rightHand.Roll, 0, 0, 0, false, false, false);
}
KeyPoint3D leftHand = pose3D.GetKeyPoint3D(BodyPoint.Left_Wrist);
if (leftHand != null && leftHand.IsValid)
{
program.controllerService.SetControllerState(1, leftHand.X, leftHand.Y, leftHand.Z, leftHand.Yaw, leftHand.Pitch, leftHand.Roll, 0, 0, 0, false, false, false);
}
KeyPoint3D rightFoot = pose3D.GetKeyPoint3D(BodyPoint.Right_Heel);
if (rightFoot != null && rightFoot.IsValid)
{
//program.controllerService3.SetControllerState(2, rightFoot.X, rightFoot.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
}
KeyPoint3D leftFoot = pose3D.GetKeyPoint3D(BodyPoint.Left_Heel);
if (leftFoot != null && leftFoot.IsValid)
{
//program.controllerService4.SetControllerState(3, leftFoot.X, leftFoot.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
}
}
}
catch (Exception)
{
program.ConnectOrReconnect();
}
}
///<exclude/>
public bool Equals(Pose3D other)
{
if (ReferenceEquals(null, other)) return false;
if (ReferenceEquals(this, other)) return true;
return other._Position.Equals(_Position) && other._Orientation.Equals(_Orientation);
}
private void updateSensorState(Pose3D pose3d)
{
instance.head_orientation = pose3d.Orientation;
}
