Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 9 additions & 5 deletions src/machinevisiontoolbox/BundleAdjust.py
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down Expand Up @@ -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:
Expand All @@ -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__()

Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down
40 changes: 40 additions & 0 deletions tests/test_bundleadjust.py
Original file line number Diff line number Diff line change
Expand Up @@ -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__":
Expand Down
Loading