diff --git a/src/machinevisiontoolbox/BundleAdjust.py b/src/machinevisiontoolbox/BundleAdjust.py index d3ca98dd..56dda726 100644 --- a/src/machinevisiontoolbox/BundleAdjust.py +++ b/src/machinevisiontoolbox/BundleAdjust.py @@ -11,6 +11,10 @@ try: import pgraph + # pgraph 1.0 has a single Vertex class and deprecates UVertex; 0.6.x has only + # UVertex. A vertex has no direction of its own in 1.0, so they are equivalent. + _Vertex = getattr(pgraph, "Vertex", None) or pgraph.UVertex + pgraph_installed = True except ImportError: print("pgraph not installed") @@ -62,7 +66,7 @@ def isfixed(self) -> bool: """ return self._fixed - class ViewPoint(pgraph.UVertex, _Common): + class ViewPoint(_Vertex, _Common): def __init__( self, x: np.ndarray, fixed: bool = False, color: str | None = None ) -> None: @@ -80,7 +84,7 @@ def __init__( the camera is not ``fixed`` it will be adjusted during the optimization. - :seealso: :class:`PGraph.UVertex` + :seealso: :class:`pgraph.Vertex` """ super().__init__() @@ -101,7 +105,7 @@ def pose(self) -> SE3: return SE3(t) * UnitQuaternion.Vec3(qv).SE3() - class Landmark(pgraph.UVertex, _Common): + class Landmark(_Vertex, _Common): def __init__(self, P: np.ndarray, fixed: bool = False) -> None: """ Create new landmark point @@ -114,7 +118,7 @@ def __init__(self, P: np.ndarray, fixed: bool = False) -> None: Represent a world point in the bundle adjustment problem. If the point is not ``fixed`` it will be adjusted during the optimization. - :seealso: :meth:`pgraph.UVertex` + :seealso: :class:`pgraph.Vertex` """ super().__init__() self._P = P @@ -379,7 +383,7 @@ def add_landmark(self, P: np.ndarray, fixed: bool = False) -> Landmark: self.g.add_vertex(l) self.landmarks.append(l) if fixed: - self.fixedlandmarks.append(c) + self.fixedlandmarks.append(l) l.ba = self # back reference to the BA problem self.index_valid = False return l diff --git a/tests/test_bundleadjust.py b/tests/test_bundleadjust.py index 6506ae39..75c16718 100644 --- a/tests/test_bundleadjust.py +++ b/tests/test_bundleadjust.py @@ -46,6 +46,46 @@ def test_optimize_empty_graph_raises(self): with self.assertRaises(ValueError): ba.optimize(iterations=5) + def test_fixed_landmark(self): + """A fixed landmark is recorded as fixed, excluded from the variable + states, and not moved by the optimization (previously add_landmark + raised NameError for fixed=True).""" + cam = CentralCamera.Default() + P = mkcube(0.2, pose=SE3.Tz(1.0)) + p0 = cam.project_point(P, pose=SE3()) + p1 = cam.project_point(P, pose=SE3.Tx(0.1)) + + ba = BundleAdjust(cam) + v0 = ba.add_view(SE3(), fixed=True) + v1 = ba.add_view(SE3()) + landmarks = [] + for k in range(P.shape[1]): + landmark = ba.add_landmark(P[:, k], fixed=(k == 0)) + landmarks.append(landmark) + ba.add_projection(v0, landmark, p0[:, k]) + ba.add_projection(v1, landmark, p1[:, k]) + + self.assertTrue(landmarks[0].isfixed) + self.assertFalse(landmarks[1].isfixed) + self.assertEqual(ba.fixedlandmarks, [landmarks[0]]) + self.assertEqual(ba.nvarstates, 6 + 3 * (P.shape[1] - 1)) + + x_new, err = ba.optimize(iterations=20) + self.assertLess(err, 1.0) + np.testing.assert_array_equal(landmarks[0].P, P[:, 0]) + + def test_vertex_base_class(self): + """ViewPoint and Landmark subclass pgraph.Vertex where it exists + (pgraph 1.0 deprecates UVertex, which warns on access).""" + import pgraph + + mod = __import__(BundleAdjust.__module__, fromlist=["ViewPoint"]) + if not hasattr(pgraph, "Vertex"): + self.skipTest("pgraph < 1.0 has only UVertex") + self.assertIs(mod._Vertex, pgraph.Vertex) + self.assertTrue(issubclass(mod.ViewPoint, pgraph.Vertex)) + self.assertTrue(issubclass(mod.Landmark, pgraph.Vertex)) + # ----------------------------------------------------------------------- # if __name__ == "__main__":