Add basic tests for g2opy

Author: RainerKuemmerle

pytest.ini

@@ -0,0 +1,6 @@
+[pytest]
+testpaths = python/tests
+python_files = test_*.py
+python_classes = Test*
+python_functions = test_*
+addopts = -v --tb=short

python/tests/__init__.py

@@ -0,0 +1 @@
+"""Pytest test suite for g2o Python bindings (g2opy)."""

python/tests/conftest.py

@@ -0,0 +1,142 @@
+"""Pytest fixtures for g2o Python bindings tests."""
+
+import sys
+import tempfile
+from pathlib import Path
+
+# Ensure build lib is in path BEFORE any imports - this is critical
+build_lib_path = Path(__file__).resolve().parent.parent.parent / "build" / "lib"
+build_lib_str = str(build_lib_path)
+if build_lib_str not in sys.path:
+    sys.path.insert(0, build_lib_str)
+
+# This must come after path setup
+import g2opy as g2o
+import numpy as np
+import pytest
+
+
+def pytest_runtest_setup(item):
+    """Inject g2o into test module globals before each test runs."""
+    if item.module:
+        item.module.g2o = g2o
+
+
+@pytest.fixture
+def g2o_module():
+    """Provide access to g2o module."""
+    return g2o
+
+
+@pytest.fixture
+def temp_g2o_file():
+    """Create a temporary file for saving .g2o graphs."""
+    with tempfile.NamedTemporaryFile(suffix=".g2o", delete=False) as f:
+        yield f.name
+    Path(f.name).unlink(missing_ok=True)
+
+
+@pytest.fixture
+def basic_se2_optimizer():
+    """Create a basic SE2 optimizer ready for optimization."""
+    optimizer = g2o.SparseOptimizer()
+    solver = g2o.BlockSolverSE2(g2o.LinearSolverEigenSE2())
+    optimizer.set_algorithm(g2o.OptimizationAlgorithmLevenberg(solver))
+    return optimizer
+
+
+@pytest.fixture
+def basic_se3_optimizer():
+    """Create a basic SE3 optimizer ready for optimization."""
+    optimizer = g2o.SparseOptimizer()
+    solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
+    optimizer.set_algorithm(g2o.OptimizationAlgorithmLevenberg(solver))
+    return optimizer
+
+
+@pytest.fixture
+def simple_se2_graph(basic_se2_optimizer):
+    """Create a simple 2D SLAM graph with 3 poses and 2 edges."""
+    optimizer = basic_se2_optimizer
+
+    # Add 3 vertices
+    poses = [
+        g2o.SE2(0, 0, 0),
+        g2o.SE2(1, 0, 0),
+        g2o.SE2(1, 1, np.pi / 2),
+    ]
+
+    for i, pose in enumerate(poses):
+        v = g2o.VertexSE2()
+        v.set_id(i)
+        v.set_estimate(pose)
+        if i == 0:
+            v.set_fixed(True)  # Fix first vertex
+        optimizer.add_vertex(v)
+
+    # Add 2 edges connecting consecutive poses
+    measurements = [
+        g2o.SE2(1, 0, 0),
+        g2o.SE2(0, 1, np.pi / 2),
+    ]
+
+    for i, measurement in enumerate(measurements):
+        edge = g2o.EdgeSE2()
+        edge.set_vertex(0, optimizer.vertex(i))
+        edge.set_vertex(1, optimizer.vertex(i + 1))
+        edge.set_measurement(measurement)
+        edge.set_information(np.eye(3))
+        optimizer.add_edge(edge)
+
+    return optimizer
+
+
+@pytest.fixture
+def simple_se3_graph(basic_se3_optimizer):
+    """Create a simple 3D SLAM graph with 2 poses and 1 edge."""
+    optimizer = basic_se3_optimizer
+
+    # Add 2 SE3 vertices
+    poses = [
+        g2o.Isometry3d(np.eye(3), [0, 0, 0]),
+        g2o.Isometry3d(np.eye(3), [1, 0, 0]),
+    ]
+
+    for i, pose in enumerate(poses):
+        v = g2o.VertexSE3()
+        v.set_id(i)
+        v.set_estimate(pose)
+        if i == 0:
+            v.set_fixed(True)
+        optimizer.add_vertex(v)
+
+    # Add edge
+    edge = g2o.EdgeSE3()
+    edge.set_vertex(0, optimizer.vertex(0))
+    edge.set_vertex(1, optimizer.vertex(1))
+    edge.set_measurement(g2o.Isometry3d(np.eye(3), [1, 0, 0]))
+    edge.set_information(np.eye(6))
+    optimizer.add_edge(edge)
+
+    return optimizer
+
+
+@pytest.fixture
+def vector_vertex():
+    """Create a VectorXVertex for testing."""
+    vertex = g2o.VectorXVertex()
+    vertex.set_dimension(3)
+    vertex.set_estimate(np.array([1.0, 2.0, 3.0]))
+    return vertex
+
+
+@pytest.fixture
+def vector_edge(vector_vertex):
+    """Create a VariableVectorXEdge for testing."""
+    edge = g2o.VariableVectorXEdge()
+    edge.set_dimension(3)
+    edge.resize(1)
+    edge.set_vertex(0, vector_vertex)
+    edge.set_measurement(np.array([1.5, 2.5, 3.5]))
+    edge.set_information(np.eye(3))
+    return edge

python/tests/test_core.py

@@ -0,0 +1,135 @@
+"""Tests for g2o core functionality (optimizer, solvers, algorithms)."""
+
+# g2o will be available after conftest.py sets sys.path
+import g2opy as g2o
+import numpy as np
+import pytest
+
+
+class TestSparseOptimizer:
+    """Test SparseOptimizer basic functionality."""
+
+    def test_create_optimizer(self):
+        """Test creating a SparseOptimizer."""
+        optimizer = g2o.SparseOptimizer()
+        assert optimizer is not None
+        assert len(optimizer.vertices()) == 0
+        assert len(optimizer.edges()) == 0
+
+    def test_add_vertex(self, basic_se2_optimizer):
+        """Test adding vertices to optimizer."""
+        v = g2o.VertexSE2()
+        v.set_id(0)
+        v.set_estimate(g2o.SE2(0, 0, 0))
+
+        basic_se2_optimizer.add_vertex(v)
+        assert len(basic_se2_optimizer.vertices()) == 1
+        assert basic_se2_optimizer.vertex(0) is not None
+
+    def test_add_edge(self, simple_se2_graph):
+        """Test adding edges to optimizer."""
+        assert len(simple_se2_graph.edges()) == 2
+
+    def test_vertex_retrieval(self, simple_se2_graph):
+        """Test retrieving vertices."""
+        for i in range(3):
+            v = simple_se2_graph.vertex(i)
+            assert v is not None
+            assert v.id() == i
+
+    def test_chi2_compute(self, simple_se2_graph):
+        """Test computing chi2 error."""
+        initial_chi2 = simple_se2_graph.chi2()
+        assert initial_chi2 >= 0
+
+    def test_initialize_optimization(self, simple_se2_graph):
+        """Test optimization initialization."""
+        simple_se2_graph.initialize_optimization()
+        # Should not raise an exception
+
+    def test_set_verbose(self, basic_se2_optimizer):
+        """Test setting verbose output."""
+        basic_se2_optimizer.set_verbose(True)
+        basic_se2_optimizer.set_verbose(False)
+        # Should not raise an exception
+
+
+class TestBlockSolvers:
+    """Test BlockSolver configurations."""
+
+    def test_block_solver_se2(self):
+        """Test BlockSolverSE2."""
+        solver = g2o.BlockSolverSE2(g2o.LinearSolverEigenSE2())
+        assert solver is not None
+
+    def test_block_solver_se3(self):
+        """Test BlockSolverSE3."""
+        solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
+        assert solver is not None
+
+    def test_block_solver_variable(self):
+        """Test variable dimension BlockSolverX."""
+        solver = g2o.BlockSolverX(g2o.LinearSolverEigenX())
+        assert solver is not None
+
+
+class TestLinearSolvers:
+    """Test various linear solver implementations."""
+
+    def test_linear_solver_eigen_se2(self):
+        """Test LinearSolverEigenSE2."""
+        solver = g2o.LinearSolverEigenSE2()
+        assert solver is not None
+
+    def test_linear_solver_eigen_se3(self):
+        """Test LinearSolverEigenSE3."""
+        solver = g2o.LinearSolverEigenSE3()
+        assert solver is not None
+
+    def test_linear_solver_eigen_x(self):
+        """Test LinearSolverEigenX (variable dimension)."""
+        solver = g2o.LinearSolverEigenX()
+        assert solver is not None
+
+
+class TestOptimizationAlgorithms:
+    """Test optimization algorithm implementations."""
+
+    def test_levenberg_marquardt(self, basic_se2_optimizer):
+        """Test Levenberg-Marquardt algorithm."""
+        assert basic_se2_optimizer.algorithm() is not None
+
+    def test_gauss_newton_creation(self):
+        """Test creating Gauss-Newton algorithm."""
+        solver = g2o.BlockSolverSE2(g2o.LinearSolverEigenSE2())
+        algo = g2o.OptimizationAlgorithmGaussNewton(solver)
+        assert algo is not None
+
+    def test_dogleg_creation(self):
+        """Test creating Dogleg algorithm."""
+        solver = g2o.BlockSolverSE2(g2o.LinearSolverEigenSE2())
+        algo = g2o.OptimizationAlgorithmDogleg(solver)
+        assert algo is not None
+
+
+class TestErrorComputation:
+    """Test error and chi2 computation."""
+
+    def test_compute_active_errors(self, simple_se2_graph):
+        """Test computing active errors."""
+        simple_se2_graph.compute_active_errors()
+        chi2 = simple_se2_graph.chi2()
+        assert chi2 >= 0
+
+    def test_chi2_before_optimization(self, simple_se2_graph):
+        """Test chi2 computation before optimization."""
+        simple_se2_graph.initialize_optimization()
+        simple_se2_graph.compute_active_errors()
+        initial_chi2 = simple_se2_graph.chi2()
+
+        simple_se2_graph.optimize(1)
+        simple_se2_graph.compute_active_errors()
+        final_chi2 = simple_se2_graph.chi2()
+
+        # Chi2 should improve or stay the same
+        assert final_chi2 <= initial_chi2 + 1e-6