优化传感器对准压头

apps/autocal/tests/test_sensor_bundle.py

@@ -17,6 +17,7 @@ import numpy as np
 import pytest
 from orisys_autocal.sensor.export_bundle import (
     BundleError,
+    housing_tail_origin_mm,
     load_bundle,
     mm_per_unit_from_pose,
 )
@@ -116,6 +117,29 @@ def test_the_surface_area_is_physically_plausible(bundle):
     assert bundle.surface_area_mm2 == pytest.approx(355.77, abs=1.0)
 
 
+def test_the_origin_is_the_unsensed_housing_tail():
+    """The tail is the housing end the sensing patch does not cover."""
+    housing = np.array(
+        [
+            [-2.0, 0.0, 0.0],
+            [2.0, 0.0, 0.0],
+            [0.0, 4.0, 0.2],
+            [-1.0, 0.1, 20.0],
+            [1.0, 0.1, 20.2],
+        ]
+    )
+    sensing = np.array([[0.0, 2.0, 5.0], [0.0, 2.0, 19.8]])
+    origin = housing_tail_origin_mm(housing, sensing)
+    assert origin[0] == pytest.approx(0.0)
+    assert origin[1] == pytest.approx(0.0)
+    assert origin[2] == pytest.approx(0.0)
+
+
+def test_a_loaded_bundle_puts_the_tail_on_the_origin(bundle):
+    origin = housing_tail_origin_mm(bundle.housing_vertices_mm, bundle.vertices_mm)
+    assert origin == pytest.approx(np.zeros(3), abs=1e-6)
+
+
 def test_the_stored_normals_are_coherent(bundle):
     """Mixed winding would make 'the normal' meaningless."""
     normals = bundle.face_normals