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