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 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));
}