├── tests ├── __init__.py ├── test_essmat_sdpa.py ├── testing_utils.py └── test_constraints.py ├── assets ├── teaser.png └── notation.png ├── .isort.cfg ├── nonmin_pose ├── __init__.py ├── models │ ├── essential_mat_zhao.py │ ├── essential_mat_gsalguero.py │ ├── c2p.py │ └── base.py ├── constraints │ └── constraint_manager.py └── utils.py ├── experiments ├── env_experiments.yml ├── accuracy_vs_noise.py ├── accuracy_vs_npoints.py ├── slack_variable_st.py ├── runtimes.py ├── accuracy_vs_translation_length.py ├── homog_vs_tlen.py ├── exp_utils.py └── real_data_strecha.py ├── .github └── workflows │ ├── static.yml │ └── build_and_publish_wheels.yml ├── src ├── sdp_zhao.cpp ├── sdpa.cpp └── COPYING ├── .gitignore ├── setup.py ├── pyproject.toml ├── README.md └── LICENSE /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /assets/teaser.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/javrtg/C2P/HEAD/assets/teaser.png -------------------------------------------------------------------------------- /assets/notation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/javrtg/C2P/HEAD/assets/notation.png -------------------------------------------------------------------------------- /.isort.cfg: -------------------------------------------------------------------------------- 1 | [settings] 2 | profile=black 3 | src_paths=nonmin_pose,experiments,tests 4 | skip_gitignore=True -------------------------------------------------------------------------------- /nonmin_pose/__init__.py: -------------------------------------------------------------------------------- 1 | from nonmin_pose.models.c2p import C2P, C2PFast 2 | from nonmin_pose.models.essential_mat_gsalguero import EssentialGSalguero 3 | from nonmin_pose.models.essential_mat_zhao import EssentialZhao 4 | -------------------------------------------------------------------------------- /experiments/env_experiments.yml: -------------------------------------------------------------------------------- 1 | name: c2p_exp 2 | channels: 3 | - conda-forge 4 | - anaconda 5 | - defaults 6 | dependencies: 7 | - python=3.10 8 | - pip 9 | - numpy 10 | - scipy 11 | - matplotlib 12 | - perfplot 13 | - pillow 14 | - tqdm 15 | - pip: 16 | - opencv-python-headless 17 | -------------------------------------------------------------------------------- /.github/workflows/static.yml: -------------------------------------------------------------------------------- 1 | # Simple workflow for deploying static content to GitHub Pages 2 | name: Deploy static content to Pages 3 | 4 | on: 5 | # Runs on pushes targeting the default branch 6 | push: 7 | branches: ["gh-pages"] 8 | 9 | # Allows you to run this workflow manually from the Actions tab 10 | workflow_dispatch: 11 | 12 | # Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages 13 | permissions: 14 | contents: read 15 | pages: write 16 | id-token: write 17 | 18 | # Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. 19 | # However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. 20 | concurrency: 21 | group: "pages" 22 | cancel-in-progress: false 23 | 24 | jobs: 25 | # Single deploy job since we're just deploying 26 | deploy: 27 | environment: 28 | name: github-pages 29 | url: ${{ steps.deployment.outputs.page_url }} 30 | runs-on: ubuntu-latest 31 | steps: 32 | - name: Checkout 33 | uses: actions/checkout@v4 34 | with: 35 | ref: gh-pages 36 | - name: Setup Pages 37 | uses: actions/configure-pages@v5 38 | - name: Upload artifact 39 | uses: actions/upload-pages-artifact@v3 40 | with: 41 | # Upload entire repository 42 | path: '.' 43 | - name: Deploy to GitHub Pages 44 | id: deployment 45 | uses: actions/deploy-pages@v4 46 | -------------------------------------------------------------------------------- /nonmin_pose/models/essential_mat_zhao.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, Union 2 | 3 | import numpy as np 4 | 5 | from nonmin_pose.constraints.constraint_manager import ConstraintManager 6 | from nonmin_pose.constraints.constraints import Parameter 7 | from nonmin_pose.models.base import NonMinRelPoseBase 8 | 9 | 10 | class EssentialZhao(NonMinRelPoseBase): 11 | """Essential matrix estimation using Zhao's method [1]. 12 | 13 | [1] An efficient solution to non-minimal case essential matrix estimation, J. Zhao. 14 | """ 15 | 16 | SDP_COMPUTES_POSE = False 17 | PARAMS = { 18 | "E": Parameter("E", 1, list(range(1, 10))), 19 | "t": Parameter("t", 1, list(range(10, 13))), 20 | } 21 | CONSTRAINTS = {"manif_def_left": None, "norm_t": None} 22 | 23 | def _get_params_and_manager(self, *args, **kwargs): 24 | params_list = list(self.PARAMS.values()) 25 | manager = ConstraintManager(params_list, self.CONSTRAINTS) 26 | return self.PARAMS, manager 27 | 28 | def retrieve_solution(self) -> Dict[str, Union[np.ndarray, bool]]: 29 | """Get the essential matrix and check its optimality.""" 30 | E, t = self.params["E"], self.params["t"] 31 | Eb, tb = E.block, t.block 32 | assert Eb == tb 33 | 34 | # get X = xx^\top. 35 | X = self.npt_problem.getResultYMat(Eb) 36 | Xe, Xt = X[:9, :9], X[9:, 9:] 37 | 38 | # check optimality. 39 | eps = self.cfg["th_rank_optimality"] 40 | _, svals_e, Vt_e = np.linalg.svd(Xe) 41 | _, svals_t, _ = np.linalg.svd(Xt) 42 | is_optimal = (svals_e > eps).sum() == 1 == (svals_t > eps).sum() 43 | 44 | E01 = Vt_e[0].reshape(3, 3) 45 | return {"E01": E01, "is_optimal": is_optimal} 46 | -------------------------------------------------------------------------------- /nonmin_pose/models/essential_mat_gsalguero.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, Union 2 | 3 | import numpy as np 4 | 5 | from nonmin_pose.constraints.constraint_manager import ConstraintManager 6 | from nonmin_pose.constraints.constraints import Parameter 7 | from nonmin_pose.models.base import NonMinRelPoseBase 8 | 9 | 10 | class EssentialGSalguero(NonMinRelPoseBase): 11 | """Essential matrix solver using García-Salguero's ADJ (adjugate) method [1]. 12 | 13 | [1] A Tighter Relaxation for the Relative Pose Problem Between Cameras, 14 | M. Garcia-Salguero, J. Briales and J. Gonzalez-Jimenez. 15 | """ 16 | 17 | SDP_COMPUTES_POSE = False 18 | PARAMS = { 19 | "E": Parameter("E", 1, list(range(1, 10))), 20 | "t": Parameter("t", 2, list(range(1, 4))), 21 | "q": Parameter("q", 2, list(range(4, 7))), 22 | } 23 | CONSTRAINTS = { 24 | "manif_def_left": [0], 25 | "manif_def_right": [0], 26 | "norm_t": None, 27 | "norm_q": None, 28 | "adjoint": None, 29 | "norm_e": None, 30 | } 31 | 32 | def _get_params_and_manager(self, *args, **kwargs): 33 | params_list = list(self.PARAMS.values()) 34 | manager = ConstraintManager(params_list, self.CONSTRAINTS) 35 | return self.PARAMS, manager 36 | 37 | def retrieve_solution(self) -> Dict[str, Union[np.ndarray, bool]]: 38 | E, t, q = self.params["E"], self.params["t"], self.params["q"] 39 | Eb, tb, qb = E.block, t.block, q.block 40 | assert Eb != tb == qb 41 | 42 | # get X = xx^\top. 43 | Xe = self.npt_problem.getResultYMat(Eb) 44 | Xtq = self.npt_problem.getResultYMat(tb) 45 | 46 | # check optimality. 47 | eps = self.cfg["th_rank_optimality"] 48 | _, svals_e, Vt_e = np.linalg.svd(Xe) 49 | svals_tq = np.linalg.svd(Xtq, compute_uv=False) 50 | is_optimal = (svals_e > eps).sum() == 1 == (svals_tq > eps).sum() 51 | 52 | E01 = Vt_e[0].reshape(3, 3) 53 | return {"E01": E01, "is_optimal": is_optimal} 54 | -------------------------------------------------------------------------------- /tests/test_essmat_sdpa.py: -------------------------------------------------------------------------------- 1 | """Simple tests to check whether the solvers work as expected.""" 2 | 3 | import numpy as np 4 | 5 | from nonmin_pose import ( 6 | C2P, 7 | C2PFast, 8 | EssentialGSalguero, 9 | EssentialZhao, 10 | sdp_zhao, # type: ignore 11 | ) 12 | from nonmin_pose.utils import compute_data_matrix_C 13 | from tests.testing_utils import SyntheticData 14 | 15 | CFG_DATASET = { 16 | "seed": 0, 17 | "min_depth": 4.0, 18 | "max_depth": 8.0, 19 | "focal": 800.0, 20 | } 21 | CFG_DATA = { 22 | "transl_magnitude": 1.0, 23 | "euler_ang_magnitude": 0.5, 24 | "max_npoints": 100, 25 | "noise_level": 0.0, 26 | } 27 | 28 | 29 | def epipolar_error(E, C): 30 | e = E.reshape(9, 1) 31 | return e.T @ C @ e 32 | 33 | 34 | def test_essmat_zhao(): 35 | dataset = SyntheticData(**CFG_DATASET) 36 | data = dataset.generate_data(**CFG_DATA) 37 | 38 | essmat_solver = EssentialZhao() 39 | sol = essmat_solver(data["f0"], data["f1"]) 40 | E01 = sol["E01"] 41 | 42 | C = compute_data_matrix_C(data["f0"], data["f1"]) 43 | error = epipolar_error(E01, C) 44 | assert error < 1e-6 45 | 46 | 47 | def test_essmat_gsalguero(): 48 | dataset = SyntheticData(**CFG_DATASET) 49 | data = dataset.generate_data(**CFG_DATA) 50 | 51 | essmat_solver = EssentialGSalguero() 52 | sol = essmat_solver(data["f0"], data["f1"]) 53 | E01 = sol["E01"] 54 | 55 | C = compute_data_matrix_C(data["f0"], data["f1"]) 56 | error = epipolar_error(E01, C) 57 | assert error < 1e-6 58 | 59 | 60 | def test_c2p(): 61 | dataset = SyntheticData(**CFG_DATASET) 62 | data = dataset.generate_data(**CFG_DATA) 63 | 64 | c2p_solver = C2P() 65 | sol = c2p_solver(data["f0"], data["f1"]) 66 | E01 = sol["E01"] 67 | 68 | C = compute_data_matrix_C(data["f0"], data["f1"]) 69 | error = epipolar_error(E01, C) 70 | assert error < 1e-6 71 | 72 | 73 | def test_c2p_fast(): 74 | dataset = SyntheticData(**CFG_DATASET) 75 | data = dataset.generate_data(**CFG_DATA) 76 | 77 | c2p_solver = C2PFast() 78 | sol = c2p_solver(data["f0"], data["f1"]) 79 | E01 = sol["E01"] 80 | 81 | C = compute_data_matrix_C(data["f0"], data["f1"]) 82 | error = epipolar_error(E01, C) 83 | assert error < 1e-6 84 | 85 | 86 | def test_sdp_zhao(): 87 | dataset = SyntheticData(**CFG_DATASET) 88 | data = dataset.generate_data(**CFG_DATA) 89 | 90 | C = compute_data_matrix_C(data["f0"], data["f1"]) 91 | 92 | X = sdp_zhao.solve(C) 93 | _, _, Vt_e = np.linalg.svd(X[:9, :9]) 94 | E01 = Vt_e[0].reshape(3, 3) 95 | 96 | error = epipolar_error(E01, C) 97 | assert error < 1e-6 98 | -------------------------------------------------------------------------------- /experiments/accuracy_vs_noise.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from tqdm import tqdm 6 | 7 | from exp_utils import SyntheticData, compute_error_metrics 8 | from nonmin_pose import C2P, C2PFast, EssentialGSalguero, EssentialZhao 9 | 10 | # magnitude of the translational offset of the second frame. 11 | TRANSL_MAGNITUDE = 2.0 12 | # angle bounds orientation of the second frame. 13 | EULER_ANG_MAGNITUDE = 0.5 14 | FOCAL_LENGTH = 800.0 15 | 16 | NREPEATS = 500 17 | NPOINTS = 1000 18 | NOISE_STEP = 0.5 19 | 20 | 21 | def main(dataset, methods, noise_levels, outdir): 22 | """Noise resilience experiment.""" 23 | r_errors = np.zeros((len(noise_levels), len(methods))) 24 | t_errors = np.zeros((len(noise_levels), len(methods))) 25 | 26 | for i, noise_level in enumerate(tqdm(noise_levels)): 27 | n = 0 28 | for j in range(NREPEATS): 29 | data = dataset.generate_data( 30 | transl_magnitude=TRANSL_MAGNITUDE, 31 | euler_ang_magnitude=EULER_ANG_MAGNITUDE, 32 | max_npoints=NPOINTS, 33 | noise_level=noise_level, 34 | ) 35 | # run each method. 36 | for k, method in enumerate(methods): 37 | pose_est = method(data["f0"], data["f1"]) 38 | r_err, t_err = compute_error_metrics(data, pose_est) 39 | # cumulative average. 40 | r_errors[i, k] = (r_err + n * r_errors[i, k]) / (n + 1) 41 | t_errors[i, k] = (t_err + n * t_errors[i, k]) / (n + 1) 42 | n += 1 43 | 44 | # plot results. 45 | fig, ax = plt.subplots(1, 2, figsize=(10, 5)) 46 | for i, method in enumerate(methods): 47 | ax[0].plot(noise_levels, r_errors[:, i], label=method.label, linewidth=2) 48 | ax[1].plot(noise_levels, t_errors[:, i], label=method.label, linewidth=2) 49 | 50 | xlim = (noise_levels.min(), noise_levels.max()) 51 | ax[0].set( 52 | xlabel="noise level (pixels)", 53 | ylabel="mean rot. error (deg)", 54 | xlim=xlim, 55 | ) 56 | ax[1].set( 57 | xlabel="noise level (pixels)", 58 | ylabel="mean tran. error (deg)", 59 | xlim=xlim, 60 | ) 61 | 62 | for axi in ax: 63 | axi.grid(True, linestyle="--", linewidth=0.5, color="gray") 64 | axi.legend() 65 | 66 | # save the plots 67 | filepath = outdir / f"noise_resilience_npoints{NPOINTS}" 68 | fig.savefig(filepath.with_suffix(".png"), bbox_inches="tight") 69 | fig.savefig(filepath.with_suffix(".pdf"), bbox_inches="tight") 70 | 71 | 72 | if __name__ == "__main__": 73 | dataset = SyntheticData(seed=0, focal=FOCAL_LENGTH) 74 | noise_levels = np.arange(0, 10 + NOISE_STEP, NOISE_STEP) 75 | 76 | # nonminimal models to evaluate. 77 | essmat_zhao = EssentialZhao() 78 | essmat_gsalguero = EssentialGSalguero() 79 | relpose_ours = C2P() 80 | relpose_ours_f = C2PFast() 81 | # labels for plotting. 82 | essmat_zhao.label = "Zhao" # type: ignore 83 | essmat_gsalguero.label = "García-Salguero" # type: ignore 84 | relpose_ours.label = "C2P" # type: ignore 85 | relpose_ours_f.label = "C2P-fast" # type: ignore 86 | 87 | # gather solvers. 88 | methods = [ 89 | essmat_zhao, 90 | essmat_gsalguero, 91 | relpose_ours_f, 92 | relpose_ours, 93 | ] 94 | 95 | # output folder. 96 | outdir = Path(__file__).parent / "results" / "accuracy_vs_noise" 97 | outdir.mkdir(parents=True, exist_ok=True) 98 | 99 | # run experiment 100 | main(dataset, methods, noise_levels, outdir) 101 | -------------------------------------------------------------------------------- /.github/workflows/build_and_publish_wheels.yml: -------------------------------------------------------------------------------- 1 | name: Build and deploy wheels 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - main 8 | release: 9 | types: 10 | - published 11 | 12 | 13 | env: 14 | MSYS2_ROOT: D:\a\_temp\msys64 15 | 16 | jobs: 17 | 18 | build_wheels: 19 | name: Build wheels on ${{ matrix.os }} 20 | runs-on: ${{ matrix.os }} 21 | strategy: 22 | matrix: 23 | os: [windows-latest, ubuntu-latest, macos-latest] 24 | 25 | steps: 26 | - name: Checkout 27 | uses: actions/checkout@v4 28 | 29 | - name: Provide gfortran (macOS) 30 | if: runner.os == 'macOS' 31 | run: | 32 | # https://github.com/AntoineD/pdfo/blob/build-wheels/.github/workflows/build_wheels.yml 33 | # https://github.com/actions/virtual-environments/issues/2524 34 | # https://github.com/cbg-ethz/dce/blob/master/.github/workflows/pkgdown.yaml 35 | sudo ln -s /usr/local/bin/gfortran-11 /usr/local/bin/gfortran 36 | sudo mkdir /usr/local/gfortran 37 | sudo ln -s /usr/local/Cellar/gcc@11/*/lib/gcc/11 /usr/local/gfortran/lib 38 | gfortran --version 39 | # ls /usr/local/gfortran/lib 40 | 41 | - name: Setup MSYS2 (Windows) 42 | if: runner.os == 'Windows' 43 | uses: msys2/setup-msys2@v2 44 | with: 45 | update: true 46 | msystem: 'MSYS' 47 | path-type: inherit 48 | install: >- 49 | base-devel 50 | mingw-w64-x86_64-toolchain 51 | mingw-w64-x86_64-openblas 52 | mingw-w64-x86_64-lapack 53 | 54 | - name: Build wheels on Windows 55 | if: runner.os == 'Windows' 56 | shell: msys2 {0} 57 | run: pipx run cibuildwheel==2.16.2 58 | env: 59 | MSYSTEM: MINGW64 60 | CIBW_ENVIRONMENT_WINDOWS: SDPA_DIR="${GITHUB_WORKSPACE}\\sdpa-7.3.17" MSYS2_ROOT=${MSYS2_ROOT} 61 | 62 | - name: Build wheels on Linux and macOS 63 | if: runner.os != 'Windows' 64 | run: pipx run cibuildwheel==2.16.2 65 | env: 66 | CIBW_ENVIRONMENT: SDPA_DIR="$(pwd)/sdpa-7.3.17" 67 | CIBW_ARCHS_MACOS: x86_64 arm64 68 | 69 | - name: Upload wheels as artifact 70 | uses: actions/upload-artifact@v3 71 | with: 72 | path: ./wheelhouse/*.whl 73 | 74 | build_sdist: 75 | name: Build source distribution 76 | runs-on: ubuntu-latest 77 | steps: 78 | - name: Checkout 79 | uses: actions/checkout@v4 80 | 81 | - name: Build source distribution 82 | run: pipx run build --sdist 83 | 84 | - name: Upload source distribution as artifact 85 | uses: actions/upload-artifact@v3 86 | with: 87 | path: dist/*.tar.gz 88 | 89 | publish-to-pypi: 90 | name: Publish to pypi.org 91 | if: github.event_name == 'release' 92 | 93 | needs: [build_wheels, build_sdist] 94 | runs-on: ubuntu-latest 95 | environment: pypi 96 | permissions: 97 | id-token: write # for trusted publishing 98 | 99 | steps: 100 | - uses: actions/download-artifact@v3 101 | with: 102 | name: artifact 103 | path: dist 104 | 105 | - uses: pypa/gh-action-pypi-publish@release/v1 106 | 107 | # publish-to-testpypi: 108 | # name: Publish to test.pypi.org 109 | # if: github.event_name == 'release' 110 | 111 | # needs: [build_wheels, build_sdist] 112 | # runs-on: ubuntu-latest 113 | # environment: testpypi 114 | # permissions: 115 | # id-token: write # for trusted publishing 116 | 117 | # steps: 118 | # - uses: actions/download-artifact@v3 119 | # with: 120 | # name: artifact 121 | # path: dist 122 | 123 | # - uses: pypa/gh-action-pypi-publish@release/v1 124 | # with: 125 | # repository-url: https://test.pypi.org/legacy/ 126 | -------------------------------------------------------------------------------- /src/sdp_zhao.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace py = pybind11; 6 | 7 | py::array_t solve(py::array_t C) 8 | { 9 | py::buffer_info buf = C.request(); 10 | if (buf.ndim != 2 || buf.shape[0] != 9 || buf.shape[1] != 9) 11 | { 12 | throw std::runtime_error("Expected a numpy ndarray of shape (9, 9)"); 13 | } 14 | double *ptr = static_cast(buf.ptr); 15 | 16 | SDPA npt_problem; 17 | npt_problem.setParameterType(SDPA::PARAMETER_DEFAULT); 18 | 19 | // number of constraints and blocks 20 | int mDIM = 7; 21 | int nBlock = 1; 22 | npt_problem.inputConstraintNumber(mDIM); 23 | npt_problem.inputBlockNumber(nBlock); 24 | npt_problem.inputBlockSize(1, 12); 25 | npt_problem.inputBlockType(1, SDPA::SDP); 26 | 27 | npt_problem.initializeUpperTriangleSpace(); 28 | 29 | for (int i = 1; i <= 6; i++) 30 | { 31 | npt_problem.inputCVec(i, 0); 32 | } 33 | npt_problem.inputCVec(7, -1); 34 | 35 | // Input F0 36 | for (int i = 0; i < 9; i++) 37 | { 38 | for (int j = i; j < 9; j++) 39 | { 40 | npt_problem.inputElement(0, 1, i + 1, j + 1, -ptr[i * 9 + j]); 41 | } 42 | } 43 | 44 | // Input F_1 -- F_7 45 | int e11 = 1; 46 | int e21 = 2; 47 | int e31 = 3; 48 | int e12 = 4; 49 | int e22 = 5; 50 | int e32 = 6; 51 | int e13 = 7; 52 | int e23 = 8; 53 | int e33 = 9; 54 | int t1 = 10; 55 | int t2 = 11; 56 | int t3 = 12; 57 | 58 | npt_problem.inputElement(1, 1, e11, e11, -1); 59 | npt_problem.inputElement(1, 1, e12, e12, -1); 60 | npt_problem.inputElement(1, 1, e13, e13, -1); 61 | npt_problem.inputElement(1, 1, t3, t3, 1); 62 | npt_problem.inputElement(1, 1, t2, t2, 1); 63 | 64 | npt_problem.inputElement(2, 1, e21, e21, -1); 65 | npt_problem.inputElement(2, 1, e22, e22, -1); 66 | npt_problem.inputElement(2, 1, e23, e23, -1); 67 | npt_problem.inputElement(2, 1, t1, t1, 1); 68 | npt_problem.inputElement(2, 1, t3, t3, 1); 69 | 70 | npt_problem.inputElement(3, 1, e31, e31, -1); 71 | npt_problem.inputElement(3, 1, e32, e32, -1); 72 | npt_problem.inputElement(3, 1, e33, e33, -1); 73 | npt_problem.inputElement(3, 1, t1, t1, 1); 74 | npt_problem.inputElement(3, 1, t2, t2, 1); 75 | 76 | npt_problem.inputElement(4, 1, e11, e21, -1); 77 | npt_problem.inputElement(4, 1, e12, e22, -1); 78 | npt_problem.inputElement(4, 1, e13, e23, -1); 79 | npt_problem.inputElement(4, 1, t1, t2, -1); 80 | 81 | npt_problem.inputElement(5, 1, e11, e31, -1); 82 | npt_problem.inputElement(5, 1, e12, e32, -1); 83 | npt_problem.inputElement(5, 1, e13, e33, -1); 84 | npt_problem.inputElement(5, 1, t1, t3, -1); 85 | 86 | npt_problem.inputElement(6, 1, e21, e31, -1); 87 | npt_problem.inputElement(6, 1, e22, e32, -1); 88 | npt_problem.inputElement(6, 1, e23, e33, -1); 89 | npt_problem.inputElement(6, 1, t2, t3, -1); 90 | 91 | npt_problem.inputElement(7, 1, t1, t1, -1); 92 | npt_problem.inputElement(7, 1, t2, t2, -1); 93 | npt_problem.inputElement(7, 1, t3, t3, -1); 94 | 95 | npt_problem.initializeUpperTriangle(); 96 | 97 | // perform optimization 98 | npt_problem.initializeSolve(); 99 | npt_problem.solve(); 100 | 101 | // extract solution 102 | double *X = npt_problem.getResultYMat(1); 103 | 104 | // corresponding ndarray. 105 | std::vector shape = {12, 12}; 106 | return py::array_t(shape, X); 107 | }; 108 | 109 | PYBIND11_MODULE(sdp_zhao, m1) 110 | { 111 | m1.doc() = "Zhao's non-minimal essential matrix solver"; 112 | 113 | m1.def( 114 | "solve", &solve, 115 | "Solve the SDP using Zhao's characterization.\n\n" 116 | "Args:\n" 117 | " C: (9, 9) data cost matrix.\n\n" 118 | "Returns:\n" 119 | " X: (12, 12) SDP's positive-semidefinite matrix solution."); 120 | }; 121 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | 162 | .vscode/ 163 | thirdparty/ 164 | # .github/ 165 | dist_backup/ 166 | 167 | experiments/**/*.png 168 | experiments/**/*.pdf 169 | experiments/**/*.mp4 170 | experiments/**/*.svg 171 | experiments/data/ 172 | -------------------------------------------------------------------------------- /experiments/accuracy_vs_npoints.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from tqdm import tqdm 6 | 7 | from exp_utils import SyntheticData, compute_error_metrics 8 | from nonmin_pose import C2P, C2PFast, EssentialGSalguero, EssentialZhao 9 | 10 | # magnitude of the translational offset of the second frame. 11 | TRANSL_MAGNITUDE = 2.0 12 | # angle bounds orientation of the second frame. 13 | EULER_ANG_MAGNITUDE = 0.5 14 | FOCAL_LENGTH = 800.0 15 | 16 | NREPEATS = 1000 17 | NOISE_LEVEL = 1.0 # pixels. 18 | 19 | 20 | def savefig(fig, outdir, npoints_levels): 21 | """Save figure.""" 22 | m, M = npoints_levels.min(), npoints_levels.max() 23 | filepath = str( 24 | outdir / f"acc_vs_npoints_noise{NOISE_LEVEL}_tmag{TRANSL_MAGNITUDE}_m{m}_M{M}" 25 | ) 26 | fig.savefig(filepath + ".png", bbox_inches="tight") 27 | fig.savefig(filepath + ".pdf", bbox_inches="tight") 28 | 29 | 30 | def plot_results(ax, r_errors, t_errors, methods, npoints_levels): 31 | """Plot results.""" 32 | for i, method in enumerate(methods): 33 | ax[0].plot(npoints_levels, r_errors[:, i], label=method.label, linewidth=2) 34 | ax[1].plot(npoints_levels, t_errors[:, i], label=method.label, linewidth=2) 35 | 36 | xlim = (npoints_levels.min(), npoints_levels.max()) 37 | # xticks = np.arange(xlim[0], xlim[1] + 1, 5) 38 | fs = 20 39 | ax[0].set( 40 | xlim=xlim, 41 | # xticks=xticks, 42 | ) 43 | ax[0].set_xlabel("#correspondences", fontsize=fs) 44 | ax[0].set_ylabel("mean rot. error (deg)", fontsize=fs) 45 | ax[1].set( 46 | xlim=xlim, 47 | # xticks=xticks, 48 | ) 49 | ax[1].set_xlabel("#correspondences", fontsize=fs) 50 | ax[1].set_ylabel("mean tran. error (deg)", fontsize=fs) 51 | 52 | for axi in ax: 53 | axi.grid(True, linestyle="--", linewidth=0.5, color="gray") 54 | axi.legend(prop=dict(size=18)) 55 | axi.tick_params(axis="both", which="major", labelsize=16) 56 | 57 | 58 | def main(dataset, methods, outdir): 59 | """Accuracy vs npoints experiment.""" 60 | regime1 = np.arange(12, 30 + 1, 1) 61 | regime2 = np.arange(100, 10_000 + 400, 400) 62 | 63 | for ri, regime in enumerate((regime1, regime2)): 64 | npoints_levels = regime 65 | r_errors = np.zeros((len(npoints_levels), len(methods))) 66 | t_errors = np.zeros((len(npoints_levels), len(methods))) 67 | 68 | for i, npoints in enumerate(tqdm(npoints_levels, desc=f"Regime {ri}")): 69 | n = 0 70 | for j in range(NREPEATS): 71 | data = dataset.generate_data( 72 | transl_magnitude=TRANSL_MAGNITUDE, 73 | euler_ang_magnitude=EULER_ANG_MAGNITUDE, 74 | max_npoints=npoints, 75 | noise_level=NOISE_LEVEL, 76 | ) 77 | 78 | # run each method. 79 | for k, method in enumerate(methods): 80 | pose_est = method(data["f0"], data["f1"]) 81 | r_err, t_err = compute_error_metrics(data, pose_est) 82 | # cumulative average. 83 | r_errors[i, k] = (r_err + n * r_errors[i, k]) / (n + 1) 84 | t_errors[i, k] = (t_err + n * t_errors[i, k]) / (n + 1) 85 | n += 1 86 | 87 | fig, ax = plt.subplots(1, 2, figsize=(20, 5)) 88 | plot_results(ax, r_errors, t_errors, methods, npoints_levels) 89 | if ri == 1: 90 | ax[1].set_xscale("log", base=2) 91 | ax[0].set_xscale("log", base=2) 92 | savefig(fig, outdir, npoints_levels) 93 | 94 | 95 | if __name__ == "__main__": 96 | seed = 0 97 | dataset = SyntheticData(seed=seed, focal=FOCAL_LENGTH) 98 | 99 | # nonminimal models to evaluate. 100 | essmat_zhao = EssentialZhao() 101 | essmat_gsalguero = EssentialGSalguero() 102 | relpose_ours = C2P() 103 | relpose_ours_f = C2PFast() 104 | # labels for plotting. 105 | essmat_zhao.label = "Zhao" # type: ignore 106 | essmat_gsalguero.label = "García-Salguero" # type: ignore 107 | relpose_ours.label = "C2P" # type: ignore 108 | relpose_ours_f.label = "C2P-fast" # type: ignore 109 | 110 | # gather solvers. 111 | methods = [ 112 | essmat_zhao, 113 | essmat_gsalguero, 114 | relpose_ours_f, 115 | relpose_ours, 116 | ] 117 | 118 | # output folder. 119 | outdir = Path(__file__).parent / "results" / "accuracy_vs_npoints" 120 | outdir.mkdir(parents=True, exist_ok=True) 121 | 122 | # run experiment 123 | main(dataset, methods, outdir) 124 | -------------------------------------------------------------------------------- /experiments/slack_variable_st.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from tqdm import tqdm 6 | 7 | from exp_utils import SyntheticData 8 | from nonmin_pose import C2P 9 | from nonmin_pose.utils import rot_given_Etq 10 | 11 | # angle bounds orientation of the second frame. 12 | EULER_ANG_MAGNITUDE = 0.5 13 | NREPEATS = 1000 14 | FOCAL_LENGTH = 800.0 15 | 16 | NPOINTS = 100 17 | NOISE_LEVEL = 0.5 # pixels. 18 | 19 | 20 | class C2PWrap(C2P): 21 | def retrieve_solution(self): 22 | X = self.npt_problem.getResultYMat(self.params["h"].block) 23 | 24 | _, svals, Vt = np.linalg.svd(X) 25 | idx = 0 26 | E01 = Vt[idx, :9].reshape(3, 3) 27 | t01 = Vt[idx, 9:12].reshape(3, 1) 28 | q = Vt[idx, 12:15].reshape(3, 1) 29 | h = Vt[idx, 15] 30 | E01, t01, q = (E01, t01, q) if h > 0 else (-E01, -t01, -q) 31 | 32 | sct = self.npt_problem.getResultYMat(2)[0, 0] 33 | 34 | is_pure_rot = sct < self.cfg["th_pure_rot_sdp"] 35 | if sct < self.cfg["th_pure_rot_noisefree_sdp"]: 36 | # improve numerical conditioning. 37 | _, _, Vt = np.linalg.svd(X[:15, :15]) 38 | E01_ = Vt[idx, :9].reshape(3, 3) 39 | t01_ = Vt[idx, 9:12].reshape(3, 1) 40 | q_ = Vt[idx, 12:15].reshape(3, 1) 41 | # correct sign. 42 | id_mx = np.abs(t01).argmax() 43 | E01, t01, q = ( 44 | (E01_, t01_, q_) 45 | if t01[id_mx, 0] * t01_[id_mx, 0] > 0 46 | else (-E01_, -t01_, -q_) 47 | ) 48 | 49 | # manifold projections. 50 | Ue, _, Vte = np.linalg.svd(E01) 51 | E01 = Ue[:, :2] @ Vte[:2] 52 | t01 = t01 / np.linalg.norm(t01) 53 | q = q / np.linalg.norm(q) 54 | 55 | R01 = rot_given_Etq(E01, t01, q) 56 | 57 | # check optimality. 58 | eps = self.cfg["th_rank_optimality"] 59 | is_optimal = (svals > eps).sum() <= 3 60 | return { 61 | "R01": R01, 62 | "t01": t01, 63 | "E01": E01, 64 | "is_optimal": is_optimal, 65 | "is_pure_rot": is_pure_rot, 66 | "sct": sct, 67 | } 68 | 69 | 70 | def create_boxplots(translation_lengths, slack_st_vals): 71 | nt = len(translation_lengths) 72 | fig, ax = plt.subplots() 73 | labels = translation_lengths.astype(str) 74 | 75 | ax.boxplot(slack_st_vals.T) 76 | 77 | samples_idx = np.random.choice(NREPEATS, min(50, NREPEATS), replace=False) 78 | x_jitter = np.random.uniform(-0.4 * 0.5, 0.4 * 0.5, samples_idx.shape) 79 | for i in range(1, nt + 1): 80 | xvals = i + x_jitter 81 | yvals = slack_st_vals[i - 1, samples_idx] 82 | ax.scatter(xvals, yvals, alpha=0.5, color="0.8", s=5) 83 | 84 | ax.set( 85 | yscale="log", 86 | xticklabels=labels, 87 | # xticks=group_centers, 88 | ) 89 | fs = 20 90 | ax.set_xlabel("translation length", fontsize=fs) 91 | ax.set_ylabel("slack variable $s_t^2$", fontsize=fs) 92 | ax.tick_params(axis="both", which="major", labelsize=16) 93 | return fig 94 | 95 | 96 | def main(dataset, method, translation_lengths, outdir): 97 | """Accuracy vs translation length experiment.""" 98 | slack_st_vals = np.zeros((len(translation_lengths), NREPEATS)) 99 | 100 | for i, translation_length in enumerate(tqdm(translation_lengths)): 101 | for j in range(NREPEATS): 102 | data = dataset.generate_data( 103 | scale_t=translation_length, 104 | euler_ang_magnitude=EULER_ANG_MAGNITUDE, 105 | max_npoints=NPOINTS, 106 | noise_level=NOISE_LEVEL, 107 | ) 108 | pose_est = method(data["f0"], data["f1"]) 109 | slack_st_vals[i, j] = pose_est["sct"] 110 | 111 | fig = create_boxplots(translation_lengths, slack_st_vals) 112 | # save the plots 113 | filepath = str(outdir / f"slack_st_noise{NOISE_LEVEL:.2f}") 114 | fig.savefig(filepath + ".png", bbox_inches="tight") 115 | fig.savefig(filepath + ".pdf", bbox_inches="tight") 116 | plt.close(fig) 117 | 118 | 119 | if __name__ == "__main__": 120 | seed = 0 121 | dataset = SyntheticData(seed=seed, focal=FOCAL_LENGTH) 122 | translation_lengths = np.array([0, 1e-3, 1e-2, 1e-1, 0.5, 1, 2, 3]) 123 | 124 | relpose_ours = C2PWrap() 125 | relpose_ours.label = "C2P" # type: ignore 126 | 127 | # output folder. 128 | outdir = Path(__file__).parent / "results" / "slack_variable_st" 129 | outdir.mkdir(parents=True, exist_ok=True) 130 | # run experiment 131 | main(dataset, relpose_ours, translation_lengths, outdir) 132 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """ This module is based on and modified from: 2 | https://github.com/sdpa-python/sdpa-python/blob/main/setup.py 3 | """ 4 | import os 5 | import platform 6 | import sys 7 | from distutils.command.build_ext import build_ext 8 | from pathlib import Path 9 | 10 | from pybind11.setup_helpers import Pybind11Extension as Extension # type: ignore 11 | from setuptools import setup 12 | 13 | # from setuptools.command.build_ext import build_ext 14 | 15 | ON_WINDOWS = platform.system() == "Windows" 16 | ON_MACOS = platform.system() == "Darwin" 17 | 18 | if ON_WINDOWS: 19 | if "mingw64\\bin" not in os.environ["PATH"]: 20 | raise RuntimeError( 21 | "On Windows, the binaries of MinGW are required to compile the extensions. " 22 | "Please, add the corresponding directory to your PATH environment variable." 23 | " For example, if you installed MinGW using MSYS2 with standard settings, " 24 | "C:\\msys64\\mingw64\\bin should be added to your PATH env. variable." 25 | ) 26 | 27 | 28 | USEGMP = False 29 | 30 | # Paths that depend on how dependencies like SDPA and MUMPS have been installed. 31 | SDPA_DIR = os.environ.get("SDPA_DIR", str(Path("path/to/folder/sdpa-7.3.17"))) 32 | # FIXME: If we are building a source dist. (python -m build --sidst), SDPA_DIR is not needed. 33 | # assert Path(SDPA_DIR).is_dir(), f"SDPA_DIR is not a folder: {SDPA_DIR}" 34 | 35 | MUMPS_DIR = os.path.join(SDPA_DIR, "mumps", "build") 36 | 37 | if ON_WINDOWS: 38 | MSYS2_ROOT = os.environ.get("MSYS2_ROOT", os.path.join("C:\\", "msys64")) 39 | if sys.maxsize > 2 * 32: 40 | # 64-bit Python 41 | MINGW_LIBS = os.path.join(MSYS2_ROOT, "mingw64", "lib") 42 | else: 43 | # 32-bit Python 44 | MINGW_LIBS = os.path.join(MSYS2_ROOT, "mingw32", "lib") 45 | # MINGW_LIBS = os.environ.get( 46 | # "MINGW_LIBS", 47 | # os.path.join("C:\\", "msys64", "mingw64", "lib"), 48 | # ) 49 | assert Path(SDPA_DIR).is_dir(), f"SDPA_DIR is not a folder: {SDPA_DIR}" 50 | assert Path(MINGW_LIBS).is_dir(), f"MINGW_LIBS is not a folder: {MINGW_LIBS}" 51 | 52 | # We use BLAS/LAPACK from Accelerate on MacOS, while on Linux/Windows we use OpenBLAS. 53 | BLAS_LAPACK_LIBS = [] if ON_MACOS else ["openblas", "gomp"] 54 | # gfortran on MacOS is installed and symlinked in a non-standard location. 55 | GFORTRAN_LIBS = "/usr/local/gfortran/lib" 56 | 57 | 58 | def pjoin(parent, children): 59 | return [os.path.join(parent, child) for child in children] 60 | 61 | 62 | libraries = ["sdpa", "dmumps", "mumps_common", "pord", "mpiseq"] + BLAS_LAPACK_LIBS 63 | library_dirs = [SDPA_DIR] + pjoin(MUMPS_DIR, ["lib", "libseq"]) 64 | include_dirs = [SDPA_DIR] + pjoin(MUMPS_DIR, ["include"]) 65 | 66 | if ON_MACOS: 67 | extra_objects = pjoin(GFORTRAN_LIBS, ["libgfortran.a", "libquadmath.a"]) 68 | else: 69 | libraries += ["gfortran", "quadmath"] 70 | extra_objects = [] 71 | 72 | if ON_WINDOWS: 73 | import distutils.cygwinccompiler 74 | 75 | distutils.cygwinccompiler.get_msvcr = lambda: [] 76 | library_dirs += [MINGW_LIBS] # type: ignore 77 | 78 | extra_link_args = ( 79 | ["-static"] if ON_WINDOWS else ["-framework", "Accelerate"] if ON_MACOS else [] 80 | ) 81 | 82 | ext_modules = [ 83 | Extension( 84 | "nonmin_pose.sdp_zhao", 85 | ["src/sdp_zhao.cpp"], 86 | include_dirs=include_dirs, 87 | library_dirs=library_dirs, 88 | libraries=libraries, 89 | extra_objects=extra_objects, 90 | extra_compile_args=["-DUSEGMP=1"] if USEGMP else ["-DUSEGMP=0"], 91 | extra_link_args=extra_link_args, 92 | cxx_std="11" if ON_MACOS else None, 93 | ), 94 | Extension( 95 | "nonmin_pose.sdpa", 96 | ["src/sdpa.cpp"], 97 | include_dirs=include_dirs, 98 | library_dirs=library_dirs, 99 | libraries=libraries, 100 | extra_objects=extra_objects, 101 | extra_compile_args=["-DUSEGMP=1"] if USEGMP else ["-DUSEGMP=0"], 102 | extra_link_args=extra_link_args, 103 | cxx_std="11" if ON_MACOS else None, 104 | ), 105 | ] 106 | 107 | 108 | class CustomBuildExtCommand(build_ext): 109 | def initialize_options(self): 110 | super().initialize_options() 111 | if ON_WINDOWS: 112 | # set compiler type to mingw32. 113 | self.compiler = "mingw32" 114 | 115 | 116 | if ON_WINDOWS: 117 | # Remove the MSVC specific flags that Pybind11 adds (we use MINGW compiler type). 118 | for ext in ext_modules: 119 | ext.extra_compile_args = [ 120 | x for x in ext.extra_compile_args if x not in ("/EHsc", "/bigobj") 121 | ] 122 | 123 | 124 | setup_args = { 125 | "ext_modules": ext_modules, 126 | "cmdclass": {"build_ext": CustomBuildExtCommand}, 127 | } 128 | setup(**setup_args) 129 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "wheel", "pybind11"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | # [tool.setuptools] 6 | # packages = ["nonmin_pose", "nonmin_pose.models", "nonmin_pose.constraints"] 7 | 8 | [tool.setuptools.packages.find] 9 | # to also install the modules under folders w/o __init__.py, we use the pattern pkg*: 10 | include = ["nonmin_pose*"] 11 | 12 | [project] 13 | name = "nonmin_pose" 14 | version = "0.0.1" 15 | authors = [ 16 | {name = "Javier Tirado Garin", email = "jtiradogarin@gmail.com"}, 17 | ] 18 | description = "Code corresponding to the paper 'From Correspondences to Pose: Non-minimal Certifiably Optimal Relative Pose without Disambiguation'" 19 | readme = "README.md" 20 | requires-python = ">=3.7" 21 | license = {file = "LICENSE"} 22 | dependencies = ["numpy"] 23 | 24 | [project.urls] 25 | "Homepage" = "https://github.com/javrtg/C2P" 26 | "Bug Tracker" = "https://github.com/javrtg/C2P/issues" 27 | 28 | [tool.cibuildwheel] 29 | build-verbosity = 3 30 | skip = ["cp36-*", "*_i686", "*musllinux*", "*-win32"] 31 | manylinux-x86_64-image = "manylinux2014" 32 | # manylinux-i686-image = "manylinux2014" 33 | # From https://github.com/Qiskit/qiskit-aer/blob/main/pyproject.toml: 34 | # We need to use pre-built versions of Numpy and Scipy in the tests; they have a 35 | # tendency to crash if they're installed from source by `pip install`, 36 | # so we force pip to use older ones without restricting any dependencies 37 | # that Numpy and Scipy might have. 38 | before-test = "pip install --only-binary=numpy,scipy numpy scipy" 39 | test-skip = ["pp*"] 40 | test-requires = ["pytest"] 41 | # to run the tests against the installed version whe use --import-mode=append. 42 | # https://docs.pytest.org/en/7.1.x/explanation/pythonpath.html#import-modes 43 | test-command = "pytest {project}/tests --import-mode=append" 44 | 45 | [tool.cibuildwheel.linux] 46 | before-all = [ 47 | "yum install -y openblas-devel lapack-devel wget", 48 | "curl -L -O 'https://downloads.sourceforge.net/project/sdpa/sdpa/sdpa_7.3.17.tar.gz'", 49 | "tar -zxf 'sdpa_7.3.17.tar.gz'", 50 | "cd sdpa-7.3.17", 51 | "./configure --prefix='$HOME/sdpa-7.3.17' --with-blas='-lopenblas' --with-lapack='-lopenblas'", 52 | "make" 53 | ] 54 | 55 | [[tool.cibuildwheel.overrides]] 56 | # Before Python 3.10, manylinux2010 is the most compatible: 57 | # https://iscinumpy.dev/post/cibuildwheel-2-2-0/ 58 | select = "cp3?-manylinux*" 59 | manylinux-x86_64-image = "manylinux2010" 60 | # manylinux-i686-image = "manylinux2010" 61 | before-all = [ 62 | "yum install -y openblas-devel lapack-devel wget", 63 | "curl -L -O 'https://downloads.sourceforge.net/project/sdpa/sdpa/sdpa_7.3.17.tar.gz'", 64 | "tar -zxf 'sdpa_7.3.17.tar.gz'", 65 | "cd sdpa-7.3.17", 66 | "sed -i '36s|-fallow-argument-mismatch||' './mumps/Makefile'", 67 | "./configure --prefix='$HOME/sdpa-7.3.17' --with-blas='-lopenblas' --with-lapack='-lopenblas'", 68 | "make" 69 | ] 70 | 71 | [tool.cibuildwheel.macos] 72 | before-all = [ 73 | "curl -L -O 'https://downloads.sourceforge.net/project/sdpa/sdpa/sdpa_7.3.17.tar.gz'", 74 | "tar -zxf 'sdpa_7.3.17.tar.gz'", 75 | "cd sdpa-7.3.17", 76 | # "./configure --prefix='$HOME/sdpa-7.3.17' --with-blas='-lopenblas' --with-lapack='-lopenblas'", 77 | "./configure --prefix='$HOME/sdpa-7.3.17'", 78 | "make" 79 | ] 80 | 81 | [tool.cibuildwheel.windows] 82 | before-all = [ 83 | "curl -L -O 'https://downloads.sourceforge.net/project/sdpa/sdpa/sdpa_7.3.17.tar.gz'", 84 | "tar -zxf sdpa_7.3.17.tar.gz", 85 | # remove -fallow-argument-mismatch flag from MUMPS Makefile 86 | # sed -i '36s|-fallow-argument-mismatch||' "./sdpa-7.3.17/mumps/Makefile" 87 | "ls", 88 | "cd ./sdpa-7.3.17", 89 | "sh ./configure --with-blas='-lopenblas' --with-lapack='-lopenblas'", 90 | "make" 91 | ] 92 | # If using powershell, add mingw64 binaries to path but in front of 93 | # chocolatey binaries (if present) to avoid using e.g. gcc from chocolatey 94 | # before-all = """ 95 | # powershell -Command \ 96 | # $newPath = '${env:MSYS2_PATH}\\mingw64\\bin'; \ 97 | # $insertBefore = 'C:\\ProgramData\\Chocolatey\\bin'; \ 98 | # $paths = $env:PATH -split ';'; \ 99 | # $position = [array]::IndexOf($paths, $insertBefore); \ 100 | # if ($position -ne -1) {$paths = $paths[0..($position - 1)] + $newPath + $paths[$position..($paths.Length - 1)]}; $env:PATH = $paths -join ';'; \ 101 | # echo $env:PATH 102 | # """ 103 | 104 | # [[tool.cibuildwheel.overrides]] 105 | # select = "*musllinux*" 106 | # before-all = [ 107 | # "apk add openblas-dev lapack-dev wget", 108 | # "curl -L -O 'https://downloads.sourceforge.net/project/sdpa/sdpa/sdpa_7.3.17.tar.gz'", 109 | # "tar -zxf 'sdpa_7.3.17.tar.gz'", 110 | # "cd sdpa-7.3.17", 111 | # "sed -i '36s|-fallow-argument-mismatch||' './mumps/Makefile'", 112 | # "./configure --prefix='$HOME/sdpa-7.3.17' --with-blas='-lopenblas' --with-lapack='-lopenblas'", 113 | # "make" 114 | # ] 115 | -------------------------------------------------------------------------------- /experiments/runtimes.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import cv2 4 | import numpy as np 5 | import perfplot 6 | 7 | from exp_utils import SyntheticData 8 | from nonmin_pose import C2P, C2PFast, EssentialGSalguero, EssentialZhao 9 | from nonmin_pose.utils import compute_data_matrix_C, decompose_essmat 10 | 11 | 12 | def zhao_midpoint(f0, f1): 13 | return ess_zhao(f0, f1, do_disambiguation=True, use_opencv=False) # type: ignore 14 | 15 | 16 | def zhao_triangulation(f0, f1): 17 | return ess_zhao(f0, f1, do_disambiguation=True, use_opencv=True) # type: ignore 18 | 19 | 20 | def salguero_midpoint(f0, f1): 21 | return ess_salguero(f0, f1, do_disambiguation=True, use_opencv=False) # type: ignore 22 | 23 | 24 | def salguero_triangulation(f0, f1): 25 | return ess_salguero(f0, f1, do_disambiguation=True, use_opencv=True) # type: ignore 26 | 27 | 28 | def c2p(f0, f1): 29 | return nonmin_relpose(f0, f1) 30 | 31 | 32 | def c2p_fast(f0, f1): 33 | return nonmin_relpose_fast(f0, f1) 34 | 35 | 36 | def sample_data(n): 37 | data = dataset.generate_data(max_npoints=n, noise_level=0.0) 38 | return data 39 | 40 | 41 | def monkeypatch_call( 42 | self, f0, f1, do_disambiguation=True, already_normalized=False, use_opencv=False 43 | ): 44 | """function for monkey-patching the __call__ method of the base class to include 45 | OpenCV's recoverPose() disambiguation, which triangulates the correspondences.""" 46 | sh0, sh1 = f0.shape, f1.shape 47 | assert sh0 == sh1 and len(sh0) == 2 and sh0[0] == 3 and sh0[1] >= 5 48 | if not already_normalized: 49 | f0 = f0 / np.linalg.norm(f0, axis=0) 50 | f1 = f1 / np.linalg.norm(f1, axis=0) 51 | C = compute_data_matrix_C(f0, f1) 52 | self.solveSDP(C, f0, f1) 53 | sol = self.retrieve_solution() 54 | if not self.SDP_COMPUTES_POSE: 55 | U, _, Vt = np.linalg.svd(sol["E01"]) 56 | sol["E01"] = U[:, :2] @ Vt[:2] 57 | if do_disambiguation and not use_opencv: 58 | ( 59 | sol["R01"], 60 | sol["t01"], 61 | sol["is_pure_rot"], 62 | sol["ret"], 63 | ) = decompose_essmat(U, Vt, f0, f1, self.cfg["th_pure_rot_post"]) 64 | elif do_disambiguation and use_opencv: 65 | f0 = f0[:2] / f0[2:] 66 | f1 = f1[:2] / f1[2:] 67 | sol = cv2.recoverPose(sol["E01"], f0.T, f1.T) 68 | return sol # type: ignore 69 | 70 | 71 | EssentialZhao.__call__ = monkeypatch_call # type: ignore 72 | EssentialGSalguero.__call__ = monkeypatch_call # type: ignore 73 | C2P.__call__ = monkeypatch_call # type: ignore 74 | C2PFast.__call__ = monkeypatch_call # type: ignore 75 | 76 | if __name__ == "__main__": 77 | dataset = SyntheticData(1) 78 | ess_zhao = EssentialZhao() 79 | ess_salguero = EssentialGSalguero() 80 | nonmin_relpose = C2P() 81 | nonmin_relpose_fast = C2PFast() 82 | 83 | labels = [ 84 | "zhao_midpoint", 85 | "salguero_midpoint", 86 | "zhao_triangulation", 87 | "salguero_triangulation", 88 | "C2P", 89 | "C2P-fast", 90 | ] 91 | n_to_test = [2**k for k in range(6, 17)] 92 | 93 | out = perfplot.bench( 94 | setup=lambda n: sample_data(n), 95 | kernels=[ 96 | lambda data: zhao_midpoint(data["f0"], data["f1"]), 97 | lambda data: salguero_midpoint(data["f0"], data["f1"]), 98 | lambda data: zhao_triangulation(data["f0"], data["f1"]), 99 | lambda data: salguero_triangulation(data["f0"], data["f1"]), 100 | lambda data: c2p(data["f0"], data["f1"]), 101 | lambda data: c2p_fast(data["f0"], data["f1"]), 102 | ], 103 | labels=labels, 104 | n_range=n_to_test, 105 | xlabel="#correspondences", 106 | # More optional arguments with their default values: 107 | # logx="auto", # set to True or False to force scaling 108 | # logy="auto", 109 | equality_check=None, # set to None to disable "correctness" assertion 110 | show_progress=True, 111 | # target_time_per_measurement=1.0, 112 | # max_time=None, # maximum time per measurement 113 | # time_unit="s", # set to one of ("auto", "s", "ms", "us", or "ns") to force plot units 114 | # relative_to=1, # plot the timings relative to one of the measurements 115 | # flops=lambda n: 3*n, # FLOPS plots 116 | ) 117 | 118 | print(f"n samples:\n{n_to_test}\n") 119 | for timing, label in zip(out.timings_s, labels): 120 | print(f"{label}:\n{timing}\n") 121 | 122 | out_dir = Path(__file__).parent / "results" / "runtimes" 123 | out_dir.mkdir(parents=True, exist_ok=True) 124 | filepath = str(out_dir / "runtimes") 125 | filepath_log = str(out_dir / "runtimes_log") 126 | 127 | for ext in [".png", ".pdf"]: 128 | out.save( 129 | filepath + ext, 130 | transparent=True, 131 | bbox_inches="tight", 132 | logy=False, 133 | ) 134 | out.save( 135 | filepath_log + ext, 136 | transparent=True, 137 | bbox_inches="tight", 138 | logy=True, 139 | ) 140 | -------------------------------------------------------------------------------- /experiments/accuracy_vs_translation_length.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from tqdm import tqdm 6 | 7 | from exp_utils import SyntheticData, compute_error_metrics 8 | from nonmin_pose import C2P, EssentialGSalguero, EssentialZhao 9 | 10 | # angle bounds orientation of the second frame. 11 | EULER_ANG_MAGNITUDE = 0.5 12 | NREPEATS = 1000 13 | FOCAL_LENGTH = 800.0 14 | 15 | NPOINTS = 100 16 | NOISE_LEVEL = 0.5 # 0.5 pixels. 17 | 18 | 19 | def create_boxplots(translation_lengths, r_errors, t_errors): 20 | nm = r_errors.shape[0] 21 | nt = len(translation_lengths) 22 | 23 | # plot results. 24 | fig, ax = plt.subplots(1, 2, figsize=(10, 5)) 25 | 26 | # style. 27 | colors = plt.get_cmap("turbo")(np.linspace(0.1, 0.9, nm)) # type: ignore 28 | labels = translation_lengths.astype(str) 29 | 30 | # position of the boxes for the first method. 31 | box_w = 0.5 32 | box_sep = 0.2 * box_w 33 | group_sep = box_w 34 | positions = np.arange( 35 | 0, 36 | nt * (nm * box_w + (nm - 1) * box_sep + group_sep), 37 | nm * box_w + (nm - 1) * box_sep + group_sep, 38 | ) 39 | group_centers = np.zeros(nt) # for cumulative average 40 | 41 | # add point samples to the boxplots to show the distribution. 42 | samples_idx = np.random.choice(NREPEATS, min(50, NREPEATS), replace=False) 43 | x_jitter = np.random.uniform(-0.4 * box_w, 0.4 * box_w, samples_idx.shape) 44 | idx = np.ones_like(samples_idx) 45 | 46 | for k, c in enumerate(colors): 47 | positions_ = positions + k * (box_w + box_sep) 48 | group_centers += positions_ 49 | 50 | ax[0].boxplot( 51 | r_errors[k].T, 52 | positions=positions_, 53 | widths=box_w, 54 | # labels=labels, 55 | patch_artist=True, 56 | boxprops=dict(color=c, facecolor="none"), 57 | capprops=dict(color=c), 58 | flierprops=dict(color=c, markeredgecolor=c), 59 | whiskerprops=dict(color=c), 60 | medianprops=dict(color="black"), 61 | ) 62 | 63 | ax[1].boxplot( 64 | t_errors[k].T, 65 | positions=positions_, 66 | widths=box_w, 67 | # labels=labels, 68 | patch_artist=True, 69 | boxprops=dict(color=c, facecolor="none"), 70 | capprops=dict(color=c), 71 | flierprops=dict(color=c, markeredgecolor=c), 72 | whiskerprops=dict(color=c), 73 | medianprops=dict(color="black"), 74 | ) 75 | 76 | for i, pos in enumerate(positions_): 77 | idx_i = i * idx 78 | samples_r_err = r_errors[k, idx_i, samples_idx] 79 | samples_t_err = t_errors[k, idx_i, samples_idx] 80 | 81 | xvals = pos + x_jitter 82 | 83 | ax[0].scatter(xvals, samples_r_err, alpha=0.5, color="0.8", s=5) 84 | ax[1].scatter(xvals, samples_t_err, alpha=0.5, color="0.8", s=5) 85 | 86 | group_centers /= nm 87 | ax[0].set( 88 | xlabel="translation length", 89 | ylabel="rotation error (deg)", 90 | xticks=group_centers, 91 | xticklabels=labels, 92 | ) 93 | ax[1].set( 94 | xlabel="translation length", 95 | ylabel="translation error (deg)", 96 | yscale="log", 97 | xticks=group_centers, 98 | xticklabels=labels, 99 | ) 100 | ax[1].legend( 101 | handles=[ 102 | ax[1].plot([], [], c=c, label=m.label)[0] for m, c in zip(methods, colors) 103 | ] 104 | ) 105 | return fig 106 | 107 | 108 | def main(dataset, methods, translation_lengths, outdir): 109 | """Accuracy vs translation length experiment.""" 110 | r_errors = np.zeros((len(methods), len(translation_lengths), NREPEATS)) 111 | t_errors = np.zeros((len(methods), len(translation_lengths), NREPEATS)) 112 | 113 | for i, translation_length in enumerate(tqdm(translation_lengths)): 114 | for j in range(NREPEATS): 115 | data = dataset.generate_data( 116 | scale_t=translation_length, 117 | euler_ang_magnitude=EULER_ANG_MAGNITUDE, 118 | max_npoints=NPOINTS, 119 | noise_level=NOISE_LEVEL, 120 | ) 121 | 122 | # run each method. 123 | for k, method in enumerate(methods): 124 | pose_est = method(data["f0"], data["f1"]) 125 | # error metrics. 126 | r_errors[k, i, j], t_errors[k, i, j] = compute_error_metrics( 127 | data, pose_est 128 | ) 129 | # plot and save results. 130 | fig = create_boxplots(translation_lengths, r_errors, t_errors) 131 | # save the plots 132 | filepath = str(outdir / f"acc_vs_translen_noise{NOISE_LEVEL}") 133 | fig.savefig(filepath + ".png", bbox_inches="tight") 134 | fig.savefig(filepath + ".pdf", bbox_inches="tight") 135 | plt.close(fig) 136 | 137 | 138 | if __name__ == "__main__": 139 | seed = 0 140 | dataset = SyntheticData(seed=seed, focal=FOCAL_LENGTH) 141 | translation_lengths = np.array([0, 1e-3, 1e-2, 1e-1, 0.5, 1, 2, 3]) 142 | 143 | # models to evaluate. 144 | essmat_zhao = EssentialZhao() 145 | essmat_gsalguero = EssentialGSalguero() 146 | relpose_ours = C2P() 147 | 148 | # prepare non-minimal methods. 149 | essmat_zhao.label = "Zhao" # type: ignore 150 | essmat_gsalguero.label = "García-Salguero" # type: ignore 151 | relpose_ours.label = "C2P" # type: ignore 152 | 153 | methods = [ 154 | essmat_zhao, 155 | essmat_gsalguero, 156 | relpose_ours, 157 | ] 158 | 159 | # output folder. 160 | outdir = Path(__file__).parent / "results" / "accuracy_vs_translation_length" 161 | outdir.mkdir(parents=True, exist_ok=True) 162 | 163 | # run experiment 164 | main(dataset, methods, translation_lengths, outdir) 165 | -------------------------------------------------------------------------------- /nonmin_pose/models/c2p.py: -------------------------------------------------------------------------------- 1 | from typing import Dict, Union 2 | 3 | import numpy as np 4 | 5 | from nonmin_pose.constraints.constraint_manager import ConstraintManager 6 | from nonmin_pose.constraints.constraints import Parameter 7 | from nonmin_pose.models.base import NonMinRelPoseBase 8 | from nonmin_pose.utils import rot_given_Etq 9 | 10 | 11 | class C2P(NonMinRelPoseBase): 12 | SDP_COMPUTES_POSE = True 13 | DEFAULT_PARAMS = { 14 | "E": Parameter("E", 1, list(range(1, 10))), 15 | "t": Parameter("t", 1, list(range(10, 13))), 16 | "q": Parameter("q", 1, list(range(13, 16))), 17 | "h": Parameter("h", 1, [16]), 18 | "sct": Parameter("sct", 2, [1]), 19 | "scr": Parameter("scr", 3, [1]), 20 | # "scr2": Parameter("scr2", 4, [1]), 21 | } 22 | DEFAULT_CONSTRAINTS = { 23 | "manif_def_left": [0], 24 | "manif_def_right": [0], 25 | "norm_t": None, 26 | "norm_q": None, 27 | "homogenization": None, 28 | "adjoint": None, 29 | "norm_e": None, 30 | "cheirality_translation_v2": None, 31 | "cheirality_rotation": None, 32 | # "right_null_space": None, 33 | # "left_null_space": None, 34 | # "cheirality_rotation_q": None, 35 | } 36 | 37 | def _get_params_and_manager(self, params=None, constraints=None): 38 | params = self.DEFAULT_PARAMS if params is None else {p.name: p for p in params} 39 | constraints = self.DEFAULT_CONSTRAINTS if constraints is None else constraints 40 | 41 | params_list = list(params.values()) 42 | manager = ConstraintManager(params_list, constraints, self.cfg["use_top_k"]) 43 | return params, manager 44 | 45 | def retrieve_solution(self) -> Dict[str, Union[np.ndarray, bool]]: 46 | X = self.npt_problem.getResultYMat(self.params["h"].block) 47 | 48 | _, svals, Vt = np.linalg.svd(X) 49 | idx = 0 50 | E01 = Vt[idx, :9].reshape(3, 3) 51 | t01 = Vt[idx, 9:12].reshape(3, 1) 52 | q = Vt[idx, 12:15].reshape(3, 1) 53 | h = Vt[idx, 15] 54 | E01, t01, q = (E01, t01, q) if h > 0 else (-E01, -t01, -q) 55 | 56 | sct = self.npt_problem.getResultYMat(2)[0, 0] 57 | 58 | is_pure_rot = sct < self.cfg["th_pure_rot_sdp"] 59 | if sct < self.cfg["th_pure_rot_noisefree_sdp"]: 60 | # improve numerical conditioning. 61 | _, _, Vt = np.linalg.svd(X[:15, :15]) 62 | E01_ = Vt[idx, :9].reshape(3, 3) 63 | t01_ = Vt[idx, 9:12].reshape(3, 1) 64 | q_ = Vt[idx, 12:15].reshape(3, 1) 65 | # correct sign. 66 | id_mx = np.abs(t01).argmax() 67 | E01, t01, q = ( 68 | (E01_, t01_, q_) 69 | if t01[id_mx, 0] * t01_[id_mx, 0] > 0 70 | else (-E01_, -t01_, -q_) 71 | ) 72 | 73 | # manifold projections. 74 | Ue, _, Vte = np.linalg.svd(E01) 75 | E01 = Ue[:, :2] @ Vte[:2] 76 | t01 = t01 / np.linalg.norm(t01) 77 | q = q / np.linalg.norm(q) 78 | 79 | R01 = rot_given_Etq(E01, t01, q) 80 | 81 | # check optimality. 82 | eps = self.cfg["th_rank_optimality"] 83 | is_optimal = (svals > eps).sum() <= 3 84 | return { 85 | "R01": R01, 86 | "t01": t01, 87 | "E01": E01, 88 | "is_optimal": is_optimal, 89 | "is_pure_rot": is_pure_rot, 90 | } 91 | 92 | 93 | class C2PFast(NonMinRelPoseBase): 94 | SDP_COMPUTES_POSE = True 95 | PARAMS = { 96 | "E": Parameter("E", 1, list(range(1, 10))), 97 | "t": Parameter("t", 1, list(range(10, 13))), 98 | "q": Parameter("q", 1, list(range(13, 16))), 99 | "h": Parameter("h", 1, [16]), 100 | "sct": Parameter("sct", 2, [1]), 101 | "scr": Parameter("scr", 3, [1]), 102 | } 103 | CONSTRAINTS = { 104 | "norm_t": None, 105 | "norm_q": None, 106 | "homogenization": None, 107 | "adjoint": None, 108 | "norm_e": None, 109 | "cheirality_translation_v2": None, 110 | "cheirality_rotation": None, 111 | } 112 | 113 | def _get_params_and_manager(self, *args, **kwargs): 114 | params_list = list(self.PARAMS.values()) 115 | manager = ConstraintManager(params_list, self.CONSTRAINTS) 116 | return self.PARAMS, manager 117 | 118 | def retrieve_solution(self) -> Dict[str, Union[np.ndarray, bool]]: 119 | X = self.npt_problem.getResultYMat(self.params["h"].block) 120 | 121 | _, svals, Vt = np.linalg.svd(X) 122 | idx = 0 123 | E01 = Vt[idx, :9].reshape(3, 3) 124 | t01 = Vt[idx, 9:12].reshape(3, 1) 125 | q = Vt[idx, 12:15].reshape(3, 1) 126 | h = Vt[idx, 15] 127 | E01, t01, q = (E01, t01, q) if h > 0 else (-E01, -t01, -q) 128 | 129 | sct = self.npt_problem.getResultYMat(2)[0, 0] 130 | 131 | is_pure_rot = sct < self.cfg["th_pure_rot_sdp"] 132 | if sct < self.cfg["th_pure_rot_noisefree_sdp"]: 133 | # improve numerical conditioning. 134 | _, _, Vt = np.linalg.svd(X[:15, :15]) 135 | E01_ = Vt[idx, :9].reshape(3, 3) 136 | t01_ = Vt[idx, 9:12].reshape(3, 1) 137 | q_ = Vt[idx, 12:15].reshape(3, 1) 138 | # correct sign. 139 | id_mx = np.abs(t01).argmax() 140 | E01, t01, q = ( 141 | (E01_, t01_, q_) 142 | if t01[id_mx, 0] * t01_[id_mx, 0] > 0 143 | else (-E01_, -t01_, -q_) 144 | ) 145 | 146 | # manifold projections. 147 | Ue, _, Vte = np.linalg.svd(E01) 148 | E01 = Ue[:, :2] @ Vte[:2] 149 | t01 = t01 / np.linalg.norm(t01) 150 | q = q / np.linalg.norm(q) 151 | 152 | R01 = rot_given_Etq(E01, t01, q) 153 | 154 | # check optimality. 155 | eps = self.cfg["th_rank_optimality"] 156 | is_optimal = (svals > eps).sum() <= 3 157 | return { 158 | "R01": R01, 159 | "t01": t01, 160 | "E01": E01, 161 | "is_optimal": is_optimal, 162 | "is_pure_rot": is_pure_rot, 163 | } 164 | -------------------------------------------------------------------------------- /nonmin_pose/constraints/constraint_manager.py: -------------------------------------------------------------------------------- 1 | from collections import defaultdict 2 | from typing import Dict, List, Optional, Union 3 | 4 | import numpy as np 5 | 6 | from nonmin_pose.constraints import constraints as cnt 7 | from nonmin_pose.constraints.constraints import Parameter 8 | 9 | ConstraintConfig = Union[Dict[str, Optional[List[int]]], Dict[str, None]] 10 | 11 | 12 | class ConstraintManager: 13 | """Manager of the metadata of constraints (blocks, values, indexes, etc.).""" 14 | 15 | CONSTRAINT_CLASSES = { 16 | "manif_def_left": cnt.ManifDefLeft, 17 | "manif_def_right": cnt.ManifDefRight, 18 | "norm_t": cnt.NormT, 19 | "norm_q": cnt.NormQ, 20 | "e_def_left": cnt.EDefLeft, 21 | "e_def_right": cnt.EDefRight, 22 | "e_def_left_right": cnt.EDefLeftRight, 23 | "homogenization": cnt.Homogenization, 24 | "adjoint": cnt.Adjoint, 25 | "norm_e": cnt.NormE, 26 | "right_null_space": cnt.RightNullSpace, 27 | "left_null_space": cnt.LeftNullSpace, 28 | "cheirality_translation": cnt.CheiralityTranslation, 29 | "cheirality_translation_v2": cnt.CheiralityTranslationV2, 30 | "cheirality_rotation": cnt.CheiralityRotation, 31 | "cheirality_rotation_q": cnt.CheiralityRotationQ, 32 | "cheirality_midpoint": cnt.CheiralityMidpoint, 33 | "orthogonality": cnt.Orthogonality, 34 | "determinant_r": cnt.DeterminantR, 35 | "t_q_definition": cnt.TQDefinition, 36 | "skew_t_q_definition": cnt.SkewTQDefinition, 37 | "convex_hull_so3": cnt.ConvexHullSO3, 38 | } 39 | 40 | DYNAMIC_CONSTRAINTS = { 41 | "cheirality_translation", 42 | "cheirality_translation_v2", 43 | "cheirality_rotation", 44 | "cheirality_rotation_q", 45 | "cheirality_midpoint", 46 | } 47 | 48 | def __init__( 49 | self, 50 | parameters: List[Parameter], 51 | constraints: ConstraintConfig, 52 | use_top_k: Optional[int] = None, 53 | ) -> None: 54 | """Constraint Manager 55 | 56 | Args: 57 | parameters: List of parameters. 58 | constraints: Dictionary of constraints. 59 | Keys are the names of the constraints and values are lists of 60 | indexes of the equations to be dropped in each constraint (a constraint 61 | can be composed of multiple equations). If None, all equations are used. 62 | use_top_k: If not None, only the top_k correspondences with highest weights 63 | are used to compute the dynamic coefficients. If None, all corresponden- 64 | ces are used. Default: None. 65 | """ 66 | if use_top_k is not None and use_top_k <= 0: 67 | raise ValueError(f"use_top_k must be positive. Got {use_top_k}") 68 | self.use_top_k = use_top_k 69 | 70 | # check parameters and get block indexes with their respective sizes. 71 | self.block_sizes = check_params_and_get_blocks(parameters) 72 | params = {p.name: p for p in parameters} 73 | 74 | # constraints with coefficients determined at runtime. 75 | self.dynamic_constraints = [] 76 | 77 | idx_first_el, idx_first_eq = 0, 0 78 | idx_, coeffs_, values_, blocks_, rows_, cols_ = [], [], [], [], [], [] 79 | 80 | for name, drop_eqs in constraints.items(): 81 | if name not in self.CONSTRAINT_CLASSES: 82 | raise ValueError(f"Unknown constraint: {name}") 83 | 84 | class_ = self.CONSTRAINT_CLASSES[name] 85 | constraint = class_(params, idx_first_el, idx_first_eq, drop_eqs) 86 | 87 | if name in self.DYNAMIC_CONSTRAINTS: 88 | self.dynamic_constraints.append(constraint) 89 | 90 | idx_.extend(constraint.constraint_idx) 91 | coeffs_.extend(constraint.coeffs) 92 | values_.extend(constraint.values) 93 | blocks_.extend(constraint.blocks) 94 | rows_.extend(constraint.rows) 95 | cols_.extend(constraint.cols) 96 | 97 | idx_first_el = constraint.idx_last_el 98 | idx_first_eq += len(constraint.values) 99 | 100 | self.constraint_idx = np.array(idx_) 101 | self.coeffs = np.array(coeffs_) 102 | self.values = np.array(values_) 103 | self.blocks = np.array(blocks_) 104 | self.rows = np.array(rows_) 105 | self.cols = np.array(cols_) 106 | 107 | self.n_blocks = len(self.block_sizes) 108 | self.n_constraints = len(self.values) 109 | 110 | self.has_dynamic_constraints = len(self.dynamic_constraints) > 0 111 | 112 | def compute_dynamic_coeffs( 113 | self, f0: np.ndarray, f1: np.ndarray, inliers_conf: Optional[np.ndarray] = None 114 | ): 115 | if not self.has_dynamic_constraints: 116 | return 117 | 118 | if self.use_top_k is not None: 119 | assert inliers_conf is not None, "weights must be provided when using top_k" 120 | if self.use_top_k < len(inliers_conf): 121 | idx = np.argpartition(inliers_conf, -self.use_top_k)[-self.use_top_k :] 122 | f0, f1 = f0[:, idx], f1[:, idx] 123 | 124 | for constraint in self.dynamic_constraints: 125 | constraint.compute_coeffs(self.coeffs, f0, f1) 126 | 127 | @property 128 | def available_constraints(self) -> List[str]: 129 | return list(self.CONSTRAINT_CLASSES) 130 | 131 | 132 | def check_params_and_get_blocks(params: List[Parameter]) -> Dict[int, int]: 133 | checked_blocks = defaultdict(list) 134 | for p in params: 135 | checked_blocks[p.block].extend(p.block_ids) 136 | 137 | for block, ids in checked_blocks.items(): 138 | if block < 1: 139 | raise ValueError(f"block must be positive. Got block: {block}.") 140 | 141 | if len(ids) != len(set(ids)): 142 | raise ValueError( 143 | f"Repeated block_ids for different parameters in block {block}" 144 | ) 145 | 146 | if list(sorted(ids)) != list(range(1, len(ids) + 1)): 147 | raise ValueError( 148 | "block_ids must be consecutive integers starting from 1. " 149 | f"Got {ids} for block {block}" 150 | ) 151 | 152 | blocks = dict(sorted({b: len(ids) for b, ids in checked_blocks.items()}.items())) 153 | return blocks 154 | -------------------------------------------------------------------------------- /experiments/homog_vs_tlen.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from tqdm import tqdm 6 | 7 | from exp_utils import SyntheticData, compute_error_metrics 8 | from nonmin_pose import C2P 9 | from nonmin_pose.utils import rot_given_Etq 10 | 11 | # angle bounds orientation of the second frame. 12 | EULER_ANG_MAGNITUDE = 0.5 13 | NREPEATS = 1000 14 | FOCAL_LENGTH = 800.0 15 | 16 | NPOINTS = 100 17 | NOISE_LEVEL = 10.0 # pixels. 18 | 19 | 20 | class C2PWrap(C2P): 21 | def retrieve_solution(self): 22 | X = self.npt_problem.getResultYMat(self.params["h"].block) 23 | _, svals, Vt = np.linalg.svd(X) 24 | idx = 0 25 | E01 = Vt[idx, :9].reshape(3, 3) 26 | t01 = Vt[idx, 9:12].reshape(3, 1) 27 | q = Vt[idx, 12:15].reshape(3, 1) 28 | h = Vt[idx, 15] 29 | E01, t01, q = (E01, t01, q) if h > 0 else (-E01, -t01, -q) 30 | 31 | sct = self.npt_problem.getResultYMat(2)[0, 0] 32 | is_pure_rot = sct < self.cfg["th_pure_rot_sdp"] 33 | 34 | if sct < self.cfg["th_pure_rot_noisefree_sdp"]: 35 | # improve numerical conditioning. 36 | _, _, Vt = np.linalg.svd(X[:15, :15]) 37 | E01_ = Vt[idx, :9].reshape(3, 3) 38 | t01_ = Vt[idx, 9:12].reshape(3, 1) 39 | q_ = Vt[idx, 12:15].reshape(3, 1) 40 | # correct sign. 41 | id_mx = np.abs(t01).argmax() 42 | E01_c, t01_c, q_c = ( 43 | (E01_, t01_, q_) 44 | if t01[id_mx, 0] * t01_[id_mx, 0] > 0 45 | else (-E01_, -t01_, -q_) 46 | ) 47 | else: 48 | E01_c, t01_c, q_c = E01, t01, q 49 | 50 | # manifold projections (without rot correction). 51 | Ue, _, Vte = np.linalg.svd(E01) 52 | E01 = Ue[:, :2] @ Vte[:2] 53 | t01 = t01 / np.linalg.norm(t01) 54 | q = q / np.linalg.norm(q) 55 | R01 = rot_given_Etq(E01, t01, q) 56 | 57 | # manifold projections (with maybe a rot correction). 58 | Ue, _, Vte = np.linalg.svd(E01_c) 59 | E01_c = Ue[:, :2] @ Vte[:2] 60 | t01_c = t01_c / np.linalg.norm(t01_c) 61 | q_c = q_c / np.linalg.norm(q_c) 62 | R01_c = rot_given_Etq(E01_c, t01_c, q_c) 63 | 64 | # check optimality. 65 | eps = self.cfg["th_rank_optimality"] 66 | is_optimal = (svals > eps).sum() <= 3 67 | return { 68 | "R01": R01, 69 | "t01": t01, 70 | "E01": E01, 71 | "R01_c": R01_c, 72 | "t01_c": t01_c, 73 | "E01_c": E01_c, 74 | "is_optimal": is_optimal, 75 | "is_pure_rot": is_pure_rot, 76 | "h": h, 77 | } 78 | 79 | 80 | def create_plot(translation_lengths, h_vals, r_errors, r_errors_c): 81 | nt = len(translation_lengths) 82 | x = np.arange(nt) 83 | labels = [ 84 | f"$10^{{{np.log10(i):.0f}}}$" if i > 0 else "0" for i in translation_lengths 85 | ] 86 | 87 | fig, ax1 = plt.subplots() 88 | ax2 = ax1.twinx() 89 | 90 | # bar chart of homogenization values. 91 | color_bars = [i / 255 for i in (167, 199, 231)] 92 | color_bars = [0.0, 0.0, 1.0] 93 | h_mean = h_vals.mean(1) 94 | h_std = h_vals.std(axis=1) 95 | ax1.bar( 96 | x, 97 | h_mean, 98 | # yerr=h_std, 99 | color=color_bars, 100 | alpha=0.5, 101 | # label="homogenization var. $h$", 102 | ) 103 | ax1.set_ylabel("homogenization var. $h$", color=color_bars, fontsize=20) 104 | ax1.tick_params(axis="y", labelcolor=color_bars) 105 | ax1.set_xticks(x) 106 | ax1.set_xticklabels(labels) 107 | ax1.tick_params(axis="both", which="major", labelsize=16) 108 | ax1.set_ylim(0, 1.0) 109 | 110 | # line plot of rotation errors. 111 | color_line = [i / 255 for i in (255, 150, 79)] 112 | color_line = [1.0, 0.0, 0.0] 113 | r_mean_c = r_errors_c.mean(axis=1) 114 | r_std_c = r_errors_c.std(axis=1) 115 | ax2.plot( 116 | x, 117 | r_mean_c, 118 | color=color_line, 119 | linestyle="--", 120 | label="num. improved rotation", 121 | lw=3, 122 | ) 123 | # ax2.fill_between( 124 | # x, 125 | # r_mean_c - 3 * r_std_c, 126 | # r_mean_c + 3 * r_std_c, 127 | # color=color_line, 128 | # alpha=0.5, 129 | # ) 130 | 131 | r_mean = r_errors.mean(axis=1) 132 | r_std = r_errors.std(axis=1) 133 | ax2.plot( 134 | x, 135 | r_mean, 136 | color=color_line, 137 | label="raw rotation", 138 | lw=3, 139 | ) 140 | # ax2.fill_between( 141 | # x, 142 | # r_mean - 3 * r_std, 143 | # r_mean + 3 * r_std, 144 | # color=color_line, 145 | # alpha=0.5, 146 | # ) 147 | 148 | if r_mean[0] / r_mean[-1] > 1e2: 149 | ax2.set_yscale("log") 150 | else: 151 | ax2.set_ylim(r_mean.min() * 0.9, r_mean.max() * 1.1) 152 | 153 | ax2.set_ylabel("rotation error (deg)", color=color_line, fontsize=20) 154 | ax2.tick_params(axis="y", labelcolor=color_line) 155 | ax2.tick_params(axis="y", which="major", labelsize=16) 156 | 157 | ax2.legend(fontsize=16, frameon=False) 158 | ax1.set_xlabel("relative scale of $\\mathbf{t}$ vs scene", fontsize=20) 159 | return fig 160 | 161 | 162 | def main(dataset, method, translation_lengths, outdir): 163 | """Accuracy vs translation length experiment.""" 164 | h_vals, r_errors, r_errors_c = np.zeros((3, len(translation_lengths), NREPEATS)) 165 | 166 | for i, translation_length in enumerate(tqdm(translation_lengths)): 167 | for j in range(NREPEATS): 168 | data = dataset.generate_data( 169 | scale_t=translation_length, 170 | euler_ang_magnitude=EULER_ANG_MAGNITUDE, 171 | max_npoints=NPOINTS, 172 | noise_level=NOISE_LEVEL, 173 | ) 174 | pose_est = method(data["f0"], data["f1"]) 175 | h_vals[i, j] = pose_est["h"] 176 | r_errors[i, j], _ = compute_error_metrics(data, pose_est) 177 | 178 | # errors in case of a rotation correction. 179 | pose_est["R01"] = pose_est["R01_c"] 180 | pose_est["t01"] = pose_est["t01_c"] 181 | pose_est["E01"] = pose_est["E01_c"] 182 | r_errors_c[i, j], _ = compute_error_metrics(data, pose_est) 183 | h_vals = np.abs(h_vals) 184 | 185 | # plot and save results. 186 | fig = create_plot(translation_lengths, h_vals, r_errors, r_errors_c) 187 | # fig = create_boxplots(translation_lengths, slack_st_vals) 188 | 189 | # save the plots 190 | filepath = str(outdir / f"homog_rotacc_vs_tlen_noise{NOISE_LEVEL:.2f}") 191 | fig.savefig(filepath + ".png", bbox_inches="tight") 192 | fig.savefig(filepath + ".pdf", bbox_inches="tight") 193 | 194 | plt.close(fig) 195 | 196 | 197 | if __name__ == "__main__": 198 | seed = 0 199 | dataset = SyntheticData(seed=seed, focal=FOCAL_LENGTH) 200 | translation_lengths = np.array([0, 1e-5, 1e-4, 1e-3, 1e-2, 1]) 201 | 202 | # models to evaluate. 203 | relpose_ours = C2PWrap() 204 | relpose_ours.label = "C2P" # type: ignore 205 | 206 | # output folder. 207 | outdir = Path(__file__).parent / "results" / "homog_vs_tlen" 208 | outdir.mkdir(parents=True, exist_ok=True) 209 | 210 | # run experiment 211 | main(dataset, relpose_ours, translation_lengths, outdir) 212 | -------------------------------------------------------------------------------- /nonmin_pose/models/base.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | from typing import Dict, List, Optional, Tuple, Union 3 | 4 | import numpy as np 5 | 6 | from nonmin_pose.constraints.constraint_manager import ( 7 | ConstraintConfig, 8 | ConstraintManager, 9 | ) 10 | from nonmin_pose.constraints.constraints import Parameter 11 | from nonmin_pose.sdpa import SDPA 12 | from nonmin_pose.utils import compute_data_matrix_C, decompose_essmat 13 | 14 | 15 | class NonMinRelPoseBase(ABC): 16 | """Non-minimal Essential matrix estimation using SDPA solver.""" 17 | 18 | DEFAULT_CFG = { 19 | # PARAMETER_STABLE_BUT_SLOW, PARAMETER_DEFAULT, PARAMETER_UNSTABLE_BUT_FAST 20 | "sdpa_param_type": SDPA.PARAMETER_DEFAULT, 21 | "th_rank_optimality": 1e-5, 22 | "th_pure_rot_post": 1 - 1e-8, # for Zhao's and Garcia-Salguero's methods. 23 | "th_pure_rot_sdp": 1e-3, # for C2P 24 | "th_pure_rot_noisefree_sdp": 1e-4, # for C2P 25 | # for computing the constraint coefficients that are determined at runtime. 26 | "use_top_k": None, 27 | } 28 | 29 | SDP_COMPUTES_POSE: bool 30 | 31 | def __init__( 32 | self, 33 | parameters: Optional[List[Parameter]] = None, 34 | constraints: Optional[ConstraintConfig] = None, 35 | cfg: Optional[Dict] = None, 36 | ): 37 | if cfg is None: 38 | cfg = {} 39 | self.cfg = {**self.DEFAULT_CFG, **cfg} 40 | self.params, self.cnt_man = self._get_params_and_manager( 41 | parameters, constraints 42 | ) 43 | # self.cnt_man = ConstraintManager(parameters, constraints) 44 | # self.params = {p.name: p for p in parameters} 45 | self._init_constant_costmat_params() 46 | self._init_solver() 47 | 48 | assert isinstance( 49 | self.SDP_COMPUTES_POSE, bool 50 | ), "SDP_COMPUTES_POSE class attribute must be defined and be a boolean." 51 | 52 | def __call__( 53 | self, 54 | f0: np.ndarray, 55 | f1: np.ndarray, 56 | w: Optional[np.ndarray] = None, 57 | already_unitary: bool = False, 58 | do_disambiguation: bool = True, 59 | inliers_conf: Optional[np.ndarray] = None, 60 | ) -> Dict[str, Union[np.ndarray, bool]]: 61 | """Non-minimal relative pose estimation. 62 | 63 | Args: 64 | f0: (3, n) bearing vectors in camera 0. 65 | f1: (3, n) bearing vectors in camera 1. 66 | w: (n,) array of weights for each residual. (default: None). 67 | already_unitary: True if the input coordinates are already unitary 68 | (default: False). 69 | do_disambiguation: True to decompose the essential matrix into relative 70 | rotation and translation. This argument is ignored when using C2P and is 71 | only used for the essential matrix solvers of Zhao and Garcia-Salguero. 72 | (default: True). 73 | 74 | Returns: 75 | sol: dict of solution parameters, with keys and values: 76 | - E01: (3, 3) essential matrix. 77 | - R01: (3, 3) rotation matrix. It is not returned for the methods of 78 | Zhao and G.Salguero et al. if do_disambiguation is False. 79 | - t01: (3, 1) translation vector. It is not returned for the methods of 80 | Zhao and G.Salguero et al. if do_disambiguation is False. 81 | - is_optimal: True if the solution is optimal. 82 | - is_pure_rot: True if a (near-)pure rotation is detected. It is not 83 | returned for the methods of Zhao and G.Salguero et al. if 84 | do_disambiguation is False. 85 | """ 86 | sh0, sh1 = f0.shape, f1.shape 87 | assert sh0 == sh1 and len(sh0) == 2 and sh0[0] == 3 and sh0[1] >= 5 88 | 89 | if not already_unitary: 90 | f0 = f0 / np.linalg.norm(f0, axis=0) 91 | f1 = f1 / np.linalg.norm(f1, axis=0) 92 | 93 | C = compute_data_matrix_C(f0, f1, w) 94 | self.solveSDP(C, f0, f1, inliers_conf) 95 | # retrieve estimated model, optimality and related measures. 96 | sol = self.retrieve_solution() 97 | 98 | if not self.SDP_COMPUTES_POSE: 99 | # Ensure E01 belongs to the normalized essential manifold. 100 | U, _, Vt = np.linalg.svd(sol["E01"]) 101 | sol["E01"] = U[:, :2] @ Vt[:2] # the same as U @ diag(1, 1, 0) @ Vt 102 | 103 | if do_disambiguation: 104 | ( 105 | sol["R01"], 106 | sol["t01"], 107 | sol["is_pure_rot"], 108 | ) = decompose_essmat(U, Vt, f0, f1, self.cfg["th_pure_rot_post"]) 109 | 110 | return sol 111 | 112 | def solveSDP( 113 | self, 114 | C: np.ndarray, 115 | f0: np.ndarray, 116 | f1: np.ndarray, 117 | inliers_conf: Optional[np.ndarray] = None, 118 | ) -> None: 119 | p = self.npt_problem 120 | cnt = self.cnt_man 121 | 122 | p.initializeUpperTriangleSpace() 123 | # constraint values. 124 | p.inputAllCVec(cnt.values) 125 | # nonzero elements of the data matrix C0. 126 | p.inputAllElements( 127 | self.C_constraints_idx, 128 | self.C_block_idx, 129 | self.C_row_idx_1based, 130 | self.C_col_idx_1based, 131 | # SDPA does max optimization, so we change the sign of C. 132 | -C[self.C_row_idx, self.C_col_idx], 133 | ) 134 | # compute dynamic coefficients (if any) and set all coeffs for each constraint. 135 | cnt.compute_dynamic_coeffs(f0, f1, inliers_conf) 136 | p.inputAllElements( 137 | cnt.constraint_idx, 138 | cnt.blocks, 139 | cnt.rows, 140 | cnt.cols, 141 | cnt.coeffs, 142 | ) 143 | p.initializeUpperTriangle(False) 144 | # solve SDP. 145 | p.initializeSolve() 146 | p.solve() 147 | 148 | def _init_solver(self): 149 | """Initialize SDPA solver.""" 150 | self.npt_problem = p = SDPA() 151 | p.setParameterType(self.cfg["sdpa_param_type"]) 152 | p.inputConstraintNumber(self.cnt_man.n_constraints) 153 | p.inputBlockNumber(self.cnt_man.n_blocks) 154 | for bi, size in self.cnt_man.block_sizes.items(): 155 | p.inputBlockSize(bi, size) 156 | # p.inputBlockType(bi, SDPA.SDP) 157 | for bi in self.cnt_man.block_sizes: 158 | p.inputBlockType(bi, SDPA.SDP) 159 | 160 | def _init_constant_costmat_params(self): 161 | """ "Set constant parameters related to the data matrix C. 162 | 163 | NOTE: We must define upper-triangular elements' info only. 164 | """ 165 | self.C_constraints_idx = np.zeros((45,), dtype=np.int32) 166 | self.C_block_idx = np.ones((45,), dtype=np.int32) 167 | self.C_row_idx, self.C_col_idx = np.triu_indices(9) 168 | self.C_row_idx_1based = self.C_row_idx + 1 169 | self.C_col_idx_1based = self.C_col_idx + 1 170 | 171 | @abstractmethod 172 | def _get_params_and_manager( 173 | self, p: Optional[List[Parameter]], c: Optional[ConstraintConfig] 174 | ) -> Tuple[Dict[str, Parameter], ConstraintManager]: 175 | """Get parameters and constraint manager.""" 176 | pass 177 | 178 | @abstractmethod 179 | def retrieve_solution(self) -> Dict[str, Union[np.ndarray, bool]]: 180 | """Retrieve essential matrix, optimality and related measures.""" 181 | pass 182 | -------------------------------------------------------------------------------- /tests/testing_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as R 3 | 4 | 5 | def skew(v): 6 | out = np.zeros((3, 3)) 7 | out[0, 1] = -v[2, 0] 8 | out[0, 2] = v[1, 0] 9 | out[1, 0] = v[2, 0] 10 | out[1, 2] = -v[0, 0] 11 | out[2, 0] = -v[1, 0] 12 | out[2, 1] = v[0, 0] 13 | return out 14 | 15 | 16 | class SyntheticData: 17 | """Data generation based on [1, Sec. 7.2.1] and [2]. 18 | 19 | [1] An Efficient Solution to Non-Minimal Case Essential Matrix Estimation, J.Zhao. 20 | [2] https://github.com/jizhaox/npt-pose/blob/master/src/create2D2DExperiment.cpp 21 | """ 22 | 23 | def __init__(self, seed=0, min_depth=4.0, max_depth=8.0, focal=800.0) -> None: 24 | self.rng = np.random.default_rng(seed) 25 | self.min_depth = min_depth 26 | self.max_depth = max_depth 27 | self.focal = focal # pixels 28 | 29 | def generate_data( 30 | self, 31 | transl_magnitude=2.0, 32 | euler_ang_magnitude=0.5, 33 | max_npoints=200, 34 | noise_level=0.0, 35 | scale_t=None, 36 | Rw1=None, 37 | tw1=None, 38 | ): 39 | """Generate synthetic data.""" 40 | # absolute camera poses (w.r.t. world "w" reference). 41 | Rw0, tw0 = self.cam0_absolute_pose 42 | Rw1, tw1 = self.set_cam1_absolute_pose( 43 | transl_magnitude, euler_ang_magnitude, scale_t, Rw1, tw1 44 | ) 45 | # relative pose such that p0 = R01 * p1 + t01. 46 | R01, t01, t01_unit = self.compute_relative_pose(Rw0, tw0, Rw1, tw1, scale_t) 47 | E01 = skew(t01_unit) @ R01 48 | f0_noisy, f1_noisy = self.generate_bearings( 49 | Rw0, tw0, Rw1, tw1, max_npoints, noise_level 50 | ) 51 | return { 52 | "f0": f0_noisy, 53 | "f1": f1_noisy, 54 | "R01": R01, 55 | "t01": t01, 56 | "t01_unit": t01_unit, 57 | "E01": E01, 58 | } 59 | 60 | def generate_bearings(self, Rw0, tw0, Rw1, tw1, max_npoints, noise_level): 61 | # generate 3D points sampling from a unit cube. 62 | pw = self.generate_absolute_3d_points(max_npoints) 63 | 64 | # transform points to each camera reference. 65 | p0 = Rw0.T @ pw - Rw0.T @ tw0 66 | p1 = Rw1.T @ pw - Rw1.T @ tw1 67 | 68 | # corresponding bearing vectors. 69 | f0 = p0 / np.linalg.norm(p0, axis=0) 70 | f1 = p1 / np.linalg.norm(p1, axis=0) 71 | 72 | # add noise to the bearing vectors. 73 | f0_noisy = self.add_noise_to_bearings(f0, max_npoints, noise_level) 74 | f1_noisy = self.add_noise_to_bearings(f1, max_npoints, noise_level) 75 | return f0_noisy, f1_noisy 76 | 77 | def generate_absolute_3d_points(self, max_npoints): 78 | """Sample 3D points sampling from a unit cube.""" 79 | unit_cube = self.rng.uniform(-0.5, 0.5, (3, max_npoints)) 80 | directions = unit_cube / np.linalg.norm(unit_cube, axis=0) 81 | magnitudes = self.rng.uniform(self.min_depth, self.max_depth, (1, max_npoints)) 82 | pw = magnitudes * directions 83 | return pw 84 | 85 | def add_noise_to_bearings(self, f, n, noise_level): 86 | """Add noise to each bearing vector assuming spherical cameras. 87 | 88 | The noise, in pixels, is added in the tangent plane of each bearing. The 89 | distance of each tangent plane is determined by the focal length of the camera. 90 | """ 91 | cols_idx = np.arange(n) 92 | 93 | max_args, min_args = np.abs(f).argmax(0), np.abs(f).argmin(0) 94 | max_vals, min_vals = f[max_args, cols_idx], f[min_args, cols_idx] 95 | 96 | # first perpendicular vector. 97 | ortho_a = np.zeros((3, n)) 98 | ortho_a[min_args, cols_idx] = 1.0 99 | ortho_a[max_args, cols_idx] = -min_vals / max_vals 100 | ortho_a = ortho_a / np.linalg.norm(ortho_a, axis=0) 101 | 102 | # second perpendicular vector. 103 | ortho_b = np.cross(f, ortho_a, axis=0) 104 | 105 | # add gaussian noise to each bearing. 106 | noise = self.rng.normal(0, noise_level, (2, n)) 107 | f_noisy = self.focal * f + noise[0] * ortho_a + noise[1] * ortho_b 108 | f_noisy = f_noisy / np.linalg.norm(f_noisy, axis=0) 109 | return f_noisy 110 | 111 | def set_cam1_absolute_pose( 112 | self, transl_magnitude, euler_ang_magnitude, scale_t, Rw1, tw1 113 | ): 114 | """camera 1 pose (w.r.t. world "w" reference).""" 115 | if Rw1 is None: 116 | euler_angles = self.rng.uniform( 117 | -euler_ang_magnitude, euler_ang_magnitude, (3,) 118 | ) 119 | Rw1 = R.from_euler("zyx", euler_angles).as_matrix() 120 | 121 | if tw1 is None: 122 | tw1 = transl_magnitude * self.rng.uniform(-1, 1, (3, 1)) 123 | 124 | if scale_t is not None: 125 | # set translation magnitude, useful e.g. for accuracy vs translation length. 126 | tw1 = tw1 / np.linalg.norm(tw1) * scale_t 127 | return Rw1, tw1 128 | 129 | def compute_relative_pose(self, Rw0, tw0, Rw1, tw1, scale_t): 130 | """Compute relative pose such that p0 = R01 * p1 + t01.""" 131 | R01 = Rw0.T @ Rw1 132 | t01 = Rw0.T @ (tw1 - tw0) 133 | if scale_t is None or scale_t > 0: 134 | t01_unit = t01 / np.linalg.norm(t01) 135 | else: 136 | # when there is pure rotation, any unit translation would satisfy the 137 | # epipolar constraint, e.g. we set it here to the x-axis unit vector. 138 | t01_unit = np.array([[1.0], [0], [0]]) 139 | return R01, t01, t01_unit 140 | 141 | @property 142 | def cam0_absolute_pose(self): 143 | """Camera 0 pose (w.r.t. world "w" reference).""" 144 | return np.eye(3), np.zeros((3, 1)) 145 | 146 | 147 | def sdpa2mat(constraint, block_sizes=[29], ndim=29): 148 | """Converts SDPA format to matrix form.""" 149 | con_idx, blocks, values, rows, cols, coeffs = ( 150 | constraint.constraint_idx, 151 | constraint.blocks, 152 | constraint.values, 153 | constraint.rows, 154 | constraint.cols, 155 | constraint.coeffs, 156 | ) 157 | n_constraints = len(values) 158 | assert (np.unique(con_idx) == np.arange(1, n_constraints + 1)).all() 159 | assert (np.unique(blocks) == np.arange(1, len(block_sizes) + 1)).all() 160 | assert ndim == sum(block_sizes) 161 | 162 | # initialize and fill matrices of constraints. 163 | As = np.zeros((n_constraints, ndim, ndim)) 164 | for block, constraint, row, col, coef in zip(blocks, con_idx, rows, cols, coeffs): 165 | # 0-based indexing. 166 | block, constraint, row, col = block - 1, constraint - 1, row - 1, col - 1 167 | rc_offset = sum(block_sizes[:block]) 168 | row += rc_offset 169 | col += rc_offset 170 | As[constraint, row, col] = coef 171 | return As 172 | 173 | 174 | def adjoint_of_3x3_mat(E): 175 | """Adjoint of a 3x3 matrix (valid for an essential matrix).""" 176 | assert E.shape == (3, 3) 177 | det = np.linalg.det 178 | det_minor00 = det(E[1:, 1:]) 179 | det_minor01 = -det(E[1:, ::2]) 180 | det_minor02 = det(E[1:, :2]) 181 | det_minor10 = -det(E[::2, 1:]) 182 | det_minor11 = det(E[::2, ::2]) 183 | det_minor12 = -det(E[::2, :2]) 184 | det_minor20 = det(E[:2, 1:]) 185 | det_minor21 = -det(E[:2, ::2]) 186 | det_minor22 = det(E[:2, :2]) 187 | 188 | # adjugate/adjoint is the *transpose* of the matrix of cofactors. 189 | adj = np.array( 190 | [ 191 | [det_minor00, det_minor10, det_minor20], 192 | [det_minor01, det_minor11, det_minor21], 193 | [det_minor02, det_minor12, det_minor22], 194 | ] 195 | ) 196 | return adj 197 | 198 | 199 | def so3_orbitope(R): 200 | """ 201 | [1 + r00 + r11 + r22, r21 - r12, r02 - r20, r10 - r01 ] 202 | [r21 - r12, 1 + r00 - r11 - r22, r10 + r01, r02 + r20 ] 203 | [r02 - r20, r10 + r01, 1 - r00 + r11 - r22, r21 + r12 ] 204 | [r10 - r01, r02 + r20, r21 + r12, 1 - r00 - r11 + r22] 205 | """ 206 | r00, r01, r02, r10, r11, r12, r20, r21, r22 = R.ravel() 207 | return np.array( 208 | [ 209 | [1 + r00 + r11 + r22, r21 - r12, r02 - r20, r10 - r01], 210 | [r21 - r12, 1 + r00 - r11 - r22, r10 + r01, r02 + r20], 211 | [r02 - r20, r10 + r01, 1 - r00 + r11 - r22, r21 + r12], 212 | [r10 - r01, r02 + r20, r21 + r12, 1 - r00 - r11 + r22], 213 | ] 214 | ) 215 | -------------------------------------------------------------------------------- /experiments/exp_utils.py: -------------------------------------------------------------------------------- 1 | from math import pi 2 | from typing import Dict, Optional, Sequence, Tuple, Union 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from matplotlib.patches import ConnectionPatch 7 | from scipy.spatial.transform import Rotation as R 8 | 9 | RAD2DEG = 180 / pi 10 | 11 | 12 | def skew(t): 13 | return np.array( 14 | [ 15 | [0, -t[2, 0], t[1, 0]], 16 | [t[2, 0], 0, -t[0, 0]], 17 | [-t[1, 0], t[0, 0], 0], 18 | ] 19 | ) 20 | 21 | 22 | class SyntheticData: 23 | """Data generation based on [1, Sec. 7.2.1] and [2]. 24 | 25 | [1] An Efficient Solution to Non-Minimal Case Essential Matrix Estimation, J.Zhao. 26 | [2] https://github.com/jizhaox/npt-pose/blob/master/src/create2D2DExperiment.cpp 27 | """ 28 | 29 | def __init__(self, seed=0, min_depth=4.0, max_depth=8.0, focal=800.0) -> None: 30 | self.rng = np.random.default_rng(seed) 31 | self.min_depth = min_depth 32 | self.max_depth = max_depth 33 | self.focal = focal # pixels 34 | 35 | def generate_data( 36 | self, 37 | transl_magnitude=2.0, 38 | euler_ang_magnitude=0.5, 39 | max_npoints=200, 40 | noise_level=2.5, 41 | scale_t=None, 42 | ): 43 | """Generate synthetic data.""" 44 | # absolute camera poses (w.r.t. world "w" reference). 45 | Rw0, tw0 = self.cam0_absolute_pose 46 | Rw1, tw1 = self.set_cam1_absolute_pose( 47 | transl_magnitude, euler_ang_magnitude, scale_t 48 | ) 49 | # relative pose such that p0 = R01 * p1 + t01. 50 | R01, t01, t01_unit = self.compute_relative_pose(Rw0, tw0, Rw1, tw1, scale_t) 51 | E01 = skew(t01_unit) @ R01 52 | f0_noisy, f1_noisy = self.generate_bearings( 53 | Rw0, tw0, Rw1, tw1, max_npoints, noise_level 54 | ) 55 | return { 56 | "f0": f0_noisy, 57 | "f1": f1_noisy, 58 | "R01": R01, 59 | "t01": t01, 60 | "t01_unit": t01_unit, 61 | "E01": E01, 62 | } 63 | 64 | def generate_bearings(self, Rw0, tw0, Rw1, tw1, max_npoints, noise_level): 65 | # generate 3D points sampling from a unit cube. 66 | pw = self.generate_absolute_3d_points(max_npoints) 67 | 68 | # transform points to each camera reference. 69 | p0 = Rw0.T @ pw - Rw0.T @ tw0 70 | p1 = Rw1.T @ pw - Rw1.T @ tw1 71 | 72 | # corresponding bearing vectors. 73 | f0 = p0 / np.linalg.norm(p0, axis=0) 74 | f1 = p1 / np.linalg.norm(p1, axis=0) 75 | 76 | # add noise to the bearing vectors. 77 | f0_noisy = self.add_noise_to_bearings(f0, max_npoints, noise_level) 78 | f1_noisy = self.add_noise_to_bearings(f1, max_npoints, noise_level) 79 | return f0_noisy, f1_noisy 80 | 81 | def generate_absolute_3d_points(self, max_npoints): 82 | """Sample 3D points sampling from a unit cube.""" 83 | unit_cube = self.rng.uniform(-0.5, 0.5, (3, max_npoints)) 84 | directions = unit_cube / np.linalg.norm(unit_cube, axis=0) 85 | magnitudes = self.rng.uniform(self.min_depth, self.max_depth, (1, max_npoints)) 86 | pw = magnitudes * directions 87 | return pw 88 | 89 | def add_noise_to_bearings(self, f, n, noise_level): 90 | """Add noise to each bearing vector assuming spherical cameras. 91 | 92 | The noise, in pixels, is added in the tangent plane of each bearing. The 93 | distance of each tangent plane is determined by the focal length of the camera. 94 | """ 95 | cols_idx = np.arange(n) 96 | 97 | max_args, min_args = np.abs(f).argmax(0), np.abs(f).argmin(0) 98 | max_vals, min_vals = f[max_args, cols_idx], f[min_args, cols_idx] 99 | 100 | # first perpendicular vector. 101 | ortho_a = np.zeros((3, n)) 102 | ortho_a[min_args, cols_idx] = 1.0 103 | ortho_a[max_args, cols_idx] = -min_vals / max_vals 104 | ortho_a = ortho_a / np.linalg.norm(ortho_a, axis=0) 105 | 106 | # second perpendicular vector. 107 | ortho_b = np.cross(f, ortho_a, axis=0) 108 | 109 | # add gaussian noise to each bearing. 110 | noise = self.rng.normal(0, noise_level, (2, n)) 111 | f_noisy = self.focal * f + noise[0] * ortho_a + noise[1] * ortho_b 112 | f_noisy = f_noisy / np.linalg.norm(f_noisy, axis=0) 113 | return f_noisy 114 | 115 | def set_cam1_absolute_pose(self, transl_magnitude, euler_ang_magnitude, scale_t): 116 | """camera 1 pose (w.r.t. world "w" reference).""" 117 | euler_angles = self.rng.uniform(-euler_ang_magnitude, euler_ang_magnitude, (3,)) 118 | Rw1 = R.from_euler("zyx", euler_angles).as_matrix() 119 | tw1 = transl_magnitude * self.rng.uniform(-1, 1, (3, 1)) 120 | 121 | if scale_t is not None: 122 | # set translation magnitude, useful e.g. for accuracy vs translation length. 123 | tw1 = tw1 / np.linalg.norm(tw1) * scale_t 124 | return Rw1, tw1 125 | 126 | def compute_relative_pose(self, Rw0, tw0, Rw1, tw1, scale_t): 127 | """Compute relative pose such that p0 = R01 * p1 + t01.""" 128 | R01 = Rw0.T @ Rw1 129 | t01 = Rw0.T @ (tw1 - tw0) 130 | if scale_t is None or scale_t > 0: 131 | t01_unit = t01 / np.linalg.norm(t01) 132 | else: 133 | # when there is pure rotation, any unit translation would satisfy the 134 | # epipolar constraint, e.g. we set it here to the x-axis unit vector. 135 | t01_unit = np.array([[1.0], [0], [0]]) 136 | return R01, t01, t01_unit 137 | 138 | @property 139 | def cam0_absolute_pose(self): 140 | """Camera 0 pose (w.r.t. world "w" reference).""" 141 | return np.eye(3), np.zeros((3, 1)) 142 | 143 | 144 | def compute_error_metrics( 145 | data_gt: Dict[str, np.ndarray], data_est: Dict[str, np.ndarray] 146 | ) -> Tuple[Union[np.float64, float], Union[np.float64, float]]: 147 | """Compute the rotation and translation errors.""" 148 | if data_est.get("fail", False): 149 | return 180.0, 180.0 150 | R01_true, t01_true = data_gt["R01"], data_gt["t01"] 151 | R01_est, t01_est = data_est["R01"], data_est["t01"] 152 | return _rotation_error(R01_true, R01_est), _translation_error(t01_true, t01_est) 153 | 154 | 155 | def _rotation_error(R01_true, R01_est) -> np.float64: 156 | """Rotation error in degrees.""" 157 | # NOTE: 158 | # R01_true.ravel().dot(R01_est.ravel()) is the same as trace(R01_true.T @ R01_est) 159 | return RAD2DEG * np.arccos( 160 | (0.5 * (R01_true.ravel().dot(R01_est.ravel()) - 1)).clip(-1, 1) 161 | ) 162 | 163 | 164 | def _translation_error(t01_true, t01_est) -> Union[np.float64, float]: 165 | """Translation error in degrees.""" 166 | t_true_norm = np.linalg.norm(t01_true) 167 | if t_true_norm == 0: 168 | return 90.0 169 | den = t_true_norm * np.linalg.norm(t01_est) 170 | return RAD2DEG * np.arccos((t01_true[:, 0].dot(t01_est[:, 0]) / den).clip(-1, 1)) 171 | 172 | 173 | def plot_boxplots( 174 | ax, data: np.ndarray, x_labels: Sequence[str], sample_points=True, **kwargs 175 | ): 176 | nd, nl = data.shape 177 | assert nl == len(x_labels), "number of data's columns and labels must be the same" 178 | 179 | ax.boxplot(data, **kwargs) 180 | 181 | if sample_points: 182 | samples_idx = np.random.choice(nd, min(50, nd), replace=False) 183 | x_jitter = np.random.uniform(-0.4 * 0.5, 0.4 * 0.5, samples_idx.shape) 184 | for i in range(1, nl + 1): 185 | xvals = i + x_jitter 186 | yvals = data[samples_idx, i - 1] 187 | ax.scatter(xvals, yvals, alpha=0.5, color="0.8", s=5) 188 | 189 | ax.set(xticklabels=x_labels) 190 | 191 | 192 | def plot_matches( 193 | fig: plt.Figure, # type:ignore 194 | ax0: plt.Axes, 195 | ax1: plt.Axes, 196 | kps0: np.ndarray, 197 | kps1: np.ndarray, 198 | kwargs_points: Optional[Dict] = None, 199 | kwargs_lines: Optional[Dict] = None, 200 | ): 201 | """Plot pairwise matches between two images.""" 202 | kw_points = {"s": 10.0, "c": [(0.0, 1.0, 0.0)]} 203 | kw_points.update(kwargs_points or {}) 204 | kw_lines = {"lw": 2.0, "color": (0.0, 1.0, 0.0), "alpha": 0.5} 205 | kw_lines.update(kwargs_lines or {}) 206 | 207 | # 2d points. 208 | ax0.scatter(kps0[0], kps0[1], **kw_points) 209 | ax1.scatter(kps1[0], kps1[1], **kw_points) 210 | # matches. 211 | for i in range(kps0.shape[1]): 212 | fig.add_artist( 213 | ConnectionPatch( 214 | xyA=kps0[:, i], # type:ignore 215 | xyB=kps1[:, i], # type:ignore 216 | axesA=ax0, 217 | axesB=ax1, 218 | coordsA=ax0.transData, 219 | coordsB=ax1.transData, 220 | **kw_lines, 221 | ) 222 | ) 223 | return fig 224 | -------------------------------------------------------------------------------- /src/sdpa.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace py = pybind11; 6 | 7 | void processVec( 8 | SDPA &self, 9 | const py::array_t &vec, 10 | void (SDPA::*inputVec)(int, double)) 11 | { 12 | py::buffer_info buf = vec.request(); 13 | const double *ptr = static_cast(buf.ptr); 14 | 15 | if (buf.ndim != 1) 16 | { 17 | throw std::runtime_error("cvec should be a 1-dimensional array."); 18 | } 19 | 20 | const int mDIM = self.getConstraintNumber(); 21 | if (buf.shape[0] != mDIM) 22 | { 23 | throw std::runtime_error("cvec should have the same length as the number of constraints."); 24 | } 25 | 26 | for (int i = 1; i <= mDIM; i++) 27 | { 28 | (self.*inputVec)(i, ptr[i - 1]); 29 | } 30 | }; 31 | 32 | void processInitMat( 33 | SDPA &self, 34 | const py::array_t &arr_l, 35 | const py::array_t &arr_i, 36 | const py::array_t &arr_j, 37 | const py::array_t &arr_values, 38 | void (SDPA::*inputInitMat)(int, int, int, double)) 39 | { 40 | py::buffer_info buf_l = arr_l.request(); 41 | py::buffer_info buf_i = arr_i.request(); 42 | py::buffer_info buf_j = arr_j.request(); 43 | py::buffer_info buf_values = arr_values.request(); 44 | 45 | if (buf_l.ndim != 1 || buf_i.ndim != 1 || buf_j.ndim != 1 || buf_values.ndim != 1) 46 | { 47 | throw std::runtime_error("All arrays should be 1-dimensional."); 48 | } 49 | 50 | const int len_l = buf_l.shape[0]; 51 | const int len_i = buf_i.shape[0]; 52 | const int len_j = buf_j.shape[0]; 53 | const int len_values = buf_values.shape[0]; 54 | 55 | if (len_l != len_i || len_l != len_j || len_l != len_values) 56 | { 57 | throw std::runtime_error("All arrays should have the same length."); 58 | } 59 | 60 | const int *ptr_l = static_cast(buf_l.ptr); 61 | const int *ptr_i = static_cast(buf_i.ptr); 62 | const int *ptr_j = static_cast(buf_j.ptr); 63 | const double *ptr_values = static_cast(buf_values.ptr); 64 | 65 | for (int idx = 0; idx < len_l; idx++) 66 | { 67 | (self.*inputInitMat)(ptr_l[idx], ptr_i[idx], ptr_j[idx], ptr_values[idx]); 68 | } 69 | }; 70 | 71 | PYBIND11_MODULE(sdpa, m) 72 | { 73 | py::class_ sdpa_(m, "SDPA"); 74 | 75 | // see http://www.gnu-darwin.org/distfiles/sdpa/sdpa.6.2.0.manual.pdf 76 | sdpa_.def(py::init<>()) 77 | .def("setParameterType", &SDPA::setParameterType) 78 | .def("inputConstraintNumber", &SDPA::inputConstraintNumber) 79 | .def("inputBlockNumber", &SDPA::inputBlockNumber) 80 | .def("inputBlockSize", &SDPA::inputBlockSize) 81 | .def("inputBlockType", &SDPA::inputBlockType) 82 | .def("inputCVec", &SDPA::inputCVec) 83 | .def("inputElement", &SDPA::inputElement) 84 | .def( 85 | "inputAllCVec", 86 | [](SDPA &self, const py::array_t &cvec) 87 | { 88 | processVec(self, cvec, &SDPA::inputCVec); 89 | }, 90 | py::arg("cvec")) 91 | .def( 92 | "inputAllElements", 93 | [](SDPA &self, 94 | const py::array_t &arr_k, 95 | const py::array_t &arr_l, 96 | const py::array_t &arr_i, 97 | const py::array_t &arr_j, 98 | const py::array_t &arr_values) 99 | { 100 | py::buffer_info buf_k = arr_k.request(); 101 | py::buffer_info buf_l = arr_l.request(); 102 | py::buffer_info buf_i = arr_i.request(); 103 | py::buffer_info buf_j = arr_j.request(); 104 | py::buffer_info buf_values = arr_values.request(); 105 | 106 | if (buf_k.ndim != 1 || buf_l.ndim != 1 || buf_i.ndim != 1 || buf_j.ndim != 1 || buf_values.ndim != 1) 107 | { 108 | throw std::runtime_error("All arrays should be 1-dimensional."); 109 | } 110 | 111 | const int len_k = buf_k.shape[0]; 112 | const int len_l = buf_l.shape[0]; 113 | const int len_i = buf_i.shape[0]; 114 | const int len_j = buf_j.shape[0]; 115 | const int len_values = buf_values.shape[0]; 116 | 117 | if (len_k != len_l || len_k != len_i || len_k != len_j || len_k != len_values) 118 | { 119 | throw std::runtime_error("All arrays should have the same length."); 120 | } 121 | 122 | const int *ptr_k = static_cast(buf_k.ptr); 123 | const int *ptr_l = static_cast(buf_l.ptr); 124 | const int *ptr_i = static_cast(buf_i.ptr); 125 | const int *ptr_j = static_cast(buf_j.ptr); 126 | const double *ptr_values = static_cast(buf_values.ptr); 127 | 128 | for (int idx = 0; idx < len_k; idx++) 129 | { 130 | self.inputElement(ptr_k[idx], ptr_l[idx], ptr_i[idx], ptr_j[idx], ptr_values[idx]); 131 | } 132 | }, 133 | py::arg("constraint_indices"), py::arg("block_indices"), 134 | py::arg("row_indices"), py::arg("col_indices"), py::arg("values")) 135 | .def("initializeUpperTriangleSpace", &SDPA::initializeUpperTriangleSpace) 136 | .def("initializeUpperTriangle", &SDPA::initializeUpperTriangle) 137 | .def("initializeSolve", &SDPA::initializeSolve) 138 | .def("solve", &SDPA::solve) 139 | // Section 10.3 140 | .def( 141 | "getResultXVec", 142 | [](SDPA &self) -> py::array_t 143 | { 144 | // raw pointer to the primal vector. 145 | double *primal_vec_els = self.getResultXVec(); 146 | // its shape is determined by the number of constraints. 147 | const int mDIM = self.getConstraintNumber(); 148 | const std::vector shape = {mDIM}; 149 | // corresponding ndarray without copying. 150 | return py::array_t(shape, primal_vec_els); 151 | }) 152 | .def( 153 | "getResultXMat", 154 | [](SDPA &self, const int l) -> py::array_t 155 | { 156 | // raw pointer to the l-th block of the primal matrix. 157 | double *primal_mat_els = self.getResultXMat(l); 158 | const int blocksize = self.getBlockSize(l); 159 | const std::vector shape = {blocksize, blocksize}; 160 | return py::array_t(shape, primal_mat_els); 161 | }, 162 | py::arg("block")) 163 | .def( 164 | "getResultYMat", 165 | [](SDPA &self, const int l) -> py::array_t 166 | { 167 | // raw pointer to the l-th block of the dual matrix. 168 | double *dual_mat_els = self.getResultYMat(l); 169 | const int blocksize = self.getBlockSize(l); 170 | const std::vector shape = {blocksize, blocksize}; 171 | return py::array_t(shape, dual_mat_els); 172 | }, 173 | py::arg("block")) 174 | 175 | .def("getPrimalObj", &SDPA::getPrimalObj) 176 | .def("getDualObj", &SDPA::getDualObj) 177 | .def("getPrimalError", &SDPA::getPrimalError) 178 | .def("getDualError", &SDPA::getDualError) 179 | .def("getIteration", &SDPA::getIteration) 180 | .def("getDualityGap", &SDPA::getDualityGap) 181 | 182 | .def("getConstraintNumber", &SDPA::getConstraintNumber) 183 | .def("getBlockNumber", &SDPA::getBlockNumber) 184 | .def("getBlockSize", &SDPA::getBlockSize, py::arg("block")) 185 | 186 | // Section 10.4 187 | .def("inputInitXVec", &SDPA::inputInitXVec) 188 | .def("inputInitXMat", &SDPA::inputInitXMat) 189 | .def("inputInitYMat", &SDPA::inputInitYMat) 190 | .def( 191 | "inputInitAllXVec", 192 | [](SDPA &self, const py::array_t &xvec) 193 | { 194 | processVec(self, xvec, &SDPA::inputInitXVec); 195 | }, 196 | py::arg("xvec")) 197 | .def( 198 | "inputInitAllXMat", 199 | [](SDPA &self, 200 | const py::array_t &arr_l, 201 | const py::array_t &arr_i, 202 | const py::array_t &arr_j, 203 | const py::array_t &arr_values) 204 | { 205 | processInitMat(self, arr_l, arr_i, arr_j, arr_values, &SDPA::inputInitXMat); 206 | }, 207 | py::arg("block_indices"), py::arg("row_indices"), py::arg("col_indices"), py::arg("values")) 208 | .def( 209 | "inputInitAllYMat", 210 | [](SDPA &self, 211 | const py::array_t &arr_l, 212 | const py::array_t &arr_i, 213 | const py::array_t &arr_j, 214 | const py::array_t &arr_values) 215 | { 216 | processInitMat(self, arr_l, arr_i, arr_j, arr_values, &SDPA::inputInitYMat); 217 | }, 218 | py::arg("block_indices"), py::arg("row_indices"), py::arg("col_indices"), py::arg("values")) 219 | .def("terminate", &SDPA::terminate); 220 | 221 | py::enum_(sdpa_, "ParameterType") 222 | .value("PARAMETER_DEFAULT", SDPA::ParameterType::PARAMETER_DEFAULT) 223 | .value("PARAMETER_UNSTABLE_BUT_FAST", SDPA::ParameterType::PARAMETER_UNSTABLE_BUT_FAST) 224 | .value("PARAMETER_STABLE_BUT_SLOW", SDPA::ParameterType::PARAMETER_STABLE_BUT_SLOW) 225 | .export_values(); 226 | 227 | py::enum_(sdpa_, "ConeType") 228 | .value("SDP", SDPA::ConeType::SDP) 229 | .value("SOCP", SDPA::ConeType::SOCP) 230 | .value("LP", SDPA::ConeType::LP) 231 | .export_values(); 232 | }; -------------------------------------------------------------------------------- /nonmin_pose/utils.py: -------------------------------------------------------------------------------- 1 | from typing import Optional, Tuple 2 | 3 | import numpy as np 4 | 5 | _W = np.array( 6 | [ 7 | [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]], 8 | [[0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]], 9 | ] 10 | ) 11 | 12 | 13 | def skew(a): 14 | """Skew-symmetric matrix from a 3D vector (array) of shape (3, 1).""" 15 | return np.array( 16 | [ 17 | [0, -a[2, 0], a[1, 0]], 18 | [a[2, 0], 0, -a[0, 0]], 19 | [-a[1, 0], a[0, 0], 0], 20 | ] 21 | ) 22 | 23 | 24 | def compute_data_matrix_C( 25 | f0: np.ndarray, f1: np.ndarray, w: Optional[np.ndarray] = None 26 | ) -> np.ndarray: 27 | """Compute the data matrix C from the bearing vectors. 28 | 29 | Args: 30 | f0: (3, n) bearing vectors in camera 0. 31 | f1: (3, n) bearing vectors in camera 1. 32 | w: (n,) array of weights for each epipolar residual. (default: None) 33 | 34 | Returns: 35 | C: (9, 9) data matrix. 36 | """ 37 | assert w is None or w.ndim == 1, "w must be a 1D array." 38 | 39 | n = f0.shape[1] 40 | f0_kron_f1 = (n**-0.5) * (f0[:, None] * f1).reshape(9, n) 41 | 42 | if w is None: 43 | return f0_kron_f1 @ f0_kron_f1.T 44 | return w * f0_kron_f1 @ f0_kron_f1.T 45 | 46 | 47 | def sdpa2mat(constraint, block_sizes): 48 | """Convert SDPA format to matrix form. 49 | 50 | Example usage: 51 | # instantiate the constraint manager with some parameters and constraints. 52 | cons = ConstraintManager(params, constraints) 53 | # function call. 54 | As = sdpa2mat(cons, list(cons.block_sizes.values())) 55 | 56 | Args: 57 | constraint: constraint manager instance. 58 | block_sizes: list of block sizes. 59 | 60 | Returns: 61 | As: (n_constraints, ndim, ndim) array of constraint matrices. 62 | """ 63 | con_idx, blocks, values, rows, cols, coeffs = ( 64 | constraint.constraint_idx, 65 | constraint.blocks, 66 | constraint.values, 67 | constraint.rows, 68 | constraint.cols, 69 | constraint.coeffs, 70 | ) 71 | n_constraints = len(values) 72 | assert (np.unique(con_idx) == np.arange(1, n_constraints + 1)).all() 73 | assert (np.unique(blocks) == np.arange(1, len(block_sizes) + 1)).all() 74 | # dimensionality. 75 | ndim = sum(block_sizes) 76 | # initialize and fill matrices of constraints. 77 | As = np.zeros((n_constraints, ndim, ndim)) 78 | for block, constraint, row, col, coef in zip(blocks, con_idx, rows, cols, coeffs): 79 | # 0-based indexing. 80 | block, constraint, row, col = block - 1, constraint - 1, row - 1, col - 1 81 | rc_offset = sum(block_sizes[:block]) 82 | row += rc_offset 83 | col += rc_offset 84 | As[constraint, row, col] = coef 85 | return As 86 | 87 | 88 | def rot_given_Etq(E, t, q): 89 | """Recover relative rotation given the essential matrix and the relative translation 90 | 91 | Assuming that we have tight solutions for E ∈ ME and t, q ∈ S2, we can directly 92 | recover the rotation matrix R ∈ SO(3) without requiring disambiguation. Recall that 93 | a (normalized) essential matrix satisfies E = [t] R, which depends linearly on R. 94 | Given that rank(E) = 2, this definition provides six linearly independent 95 | constraints for solving R (its nine parameters). Thus, three additional independent 96 | equations are needed. Notably, t lies in the nullspace of [t], allowing us to find 97 | the remaining linear equations from the definition q = R^Tt. Consequently, the 98 | elements of R can be computed as the solution of this linear system of equations. 99 | Since our solution is empirically tight, the estimates E, t and q belong to their 100 | respective spaces, implying that the solution R belongs to SO(3). While not 101 | theoretically necessary, for better numerical accuracy, we: 102 | 1) Consider the linearly dependent equations stemming from the definitions: 103 | E = [t] R, E = R [q], q = R^T t and t = R q, 104 | Thus, in essence, we estimate R through minimizing the (potentially 105 | negligible) square errors from the four above definitions. 106 | Fortunately, there is a fast closed-form solution to this problem, given by: 107 | R = t q^T - 0.5 * ([t] E + E [q]) 108 | This follows from forming the the corresponding normal equations, and 109 | realizing that their LHS and RHS are given by: 110 | 2 I_9 and 2 t q^T - [t] E - E[q], respectively. 111 | 2) Project the resulting matrix R to SO(3) by classical means. 112 | 113 | Args: 114 | E: (3, 3) essential matrix. 115 | t: (3, 1) relative translation. 116 | q: (3, 1) *rotated* relative translation i.e. q = R^T t. 117 | 118 | Returns: 119 | R: (3, 3) relative rotation. 120 | """ 121 | R = t @ q.T - 0.5 * (skew(t) @ E + E @ skew(q)) 122 | # project to SO(3). 123 | Ur, _, Vtr = np.linalg.svd(R) 124 | # TODO: This check ensuring proper rotations (avoiding reflections) should 125 | # only be needed when the solution is not certified as optimal. 126 | Vtr[2] = -Vtr[2] if np.linalg.det(Ur) * np.linalg.det(Vtr) < 0 else Vtr[2] 127 | R = Ur @ Vtr 128 | return R 129 | 130 | 131 | def decompose_essmat( 132 | U: np.ndarray, 133 | Vt: np.ndarray, 134 | f0: np.ndarray, 135 | f1: np.ndarray, 136 | th_pure_rotation: float = 1 - 1e-8, 137 | ) -> Tuple[np.ndarray, np.ndarray, bool]: 138 | """Decompose the essential matrix into relative rotation and (normalized) 139 | translation. 140 | 141 | The extraction of the 4 possible relative pose factorizations given an essential 142 | matrix, follows the approach explained in [1, Sec. 9.6.2]. 143 | To select the best pose candidate, we check the sign of the factor that 144 | multiplies each bearing vector. This factor must be positive since a bearing 145 | vector is equivalent to $f_i := X_i / ||X_i||$, where $X_i$ is the corresponding 146 | 3D point. Thus, to recover the 3D point, we multiply $f$ with an estimated 147 | scalar factor that *must* be positive. This constraint is independent of the 148 | camera model used (pinhole, fisheye, omnidirectional etc.), thus the camera 149 | model is not a limiting factor for this approach. 150 | To compute the scalar factor (the norm of X_i), we use the classic midpoint 151 | method (see e.g. [2]). However, instead of explicitly computing (triangulating) 152 | the 3D points, we just compute the sign of the scalar factors (lambdas). As a 153 | result, we save some computation. Specifically, we avoid computing: 154 | 1) the term (sin angle(f0, R01@f1))^2 = || f0 x R01@f1 ||^2, for each point, and 155 | 2) the XY coordinates of each 3D point. 156 | 157 | [1]: Multiple View Geometry in Computer Vision, Hartley and Zisserman, 2003. 158 | [2]: Triangulation: why optimize?, Lee and Civera, 2019. 159 | 160 | Args: 161 | U: (3, 3) left singular vectors of the essential matrix. 162 | Vt: (3, 3) right singular vectors of the essential matrix. 163 | f0: (3, n) bearing vectors in camera 0. 164 | f1: (3, n) bearing vectors in camera 1. 165 | th_pure_rotation: threshold for checking if the motion is a pure rotation. 166 | 167 | Returns: 168 | R: (3, 3) rotation matrix. 169 | t: (3, 1) translation vector. 170 | is_pure_rotation: True if a (near-)pure rotation is detected. 171 | """ 172 | # avoid reflection (ensure rotation) when decomposing the essential matrix. 173 | Vt[2] = -Vt[2] if np.linalg.det(U) * np.linalg.det(Vt) < 0 else Vt[2] 174 | 175 | Ra, Rb = U @ _W @ Vt # (2, 3, 3) 176 | ta, tb = U[:, 2:], -U[:, 2:] 177 | 178 | # check if it is a pure rotation. 179 | is_pure_rotation, choice = check_pure_rotation( 180 | f0, np.stack((Ra, Rb)) @ f1, th_pure_rotation 181 | ) 182 | 183 | # (Ra, ta) 184 | Raf1 = Ra @ f1 185 | lambda0_rhs = ( 186 | np.cross((Raf1).T, f0.T)[:, None] @ np.cross((Raf1).T, ta.T)[..., None] 187 | ) 188 | lambda1_rhs = np.cross((Raf1).T, f0.T)[:, None] @ np.cross(f0.T, ta.T)[..., None] 189 | npos_aa = ((lambda0_rhs > 0) & (lambda1_rhs > 0)).sum() 190 | 191 | # (Rb, ta) 192 | Rbf1 = Rb @ f1 193 | lambda0_rhs = ( 194 | np.cross((Rbf1).T, f0.T)[:, None] @ np.cross((Rbf1).T, ta.T)[..., None] 195 | ) 196 | lambda1_rhs = np.cross((Rbf1).T, f0.T)[:, None] @ np.cross(f0.T, ta.T)[..., None] 197 | npos_ba = ((lambda0_rhs > 0) & (lambda1_rhs > 0)).sum() 198 | 199 | # (Ra, tb) 200 | lambda0_rhs = ( 201 | np.cross((Raf1).T, f0.T)[:, None] @ np.cross((Raf1).T, tb.T)[..., None] 202 | ) 203 | lambda1_rhs = np.cross((Raf1).T, f0.T)[:, None] @ np.cross(f0.T, tb.T)[..., None] 204 | npos_ab = ((lambda0_rhs > 0) & (lambda1_rhs > 0)).sum() 205 | 206 | # (Rb, tb) 207 | lambda0_rhs = ( 208 | np.cross((Rbf1).T, f0.T)[:, None] @ np.cross((Rbf1).T, tb.T)[..., None] 209 | ) 210 | lambda1_rhs = np.cross((Rbf1).T, f0.T)[:, None] @ np.cross(f0.T, tb.T)[..., None] 211 | npos_bb = ((lambda0_rhs > 0) & (lambda1_rhs > 0)).sum() 212 | 213 | npos_tpos = np.r_[npos_aa, npos_ba] 214 | npos_tneg = np.r_[npos_ab, npos_bb] 215 | 216 | if is_pure_rotation and (npos_tpos[choice] == npos_tneg[choice] == 0): 217 | # Pure rotation with perfect bearings alignment by just rotating them. 218 | R01 = Ra if choice == 0 else Rb 219 | return R01, ta, is_pure_rotation 220 | 221 | if is_pure_rotation: 222 | # Pure rotation with imperfect bearings alignment. Choose the translation 223 | # candidate that satisfies the most the positive-norm bearings' constraint. 224 | t01 = ta if npos_tpos[choice] >= npos_tneg[choice] else tb 225 | R01 = Ra if choice == 0 else Rb 226 | return R01, t01, is_pure_rotation 227 | 228 | # Otherwise, select the candidate that satisfies the most the positive-norm 229 | # bearings' constraint. 230 | choice, npos = max( 231 | enumerate((npos_tpos[0], npos_tpos[1], npos_tneg[0], npos_tneg[1])), 232 | key=lambda x: x[1], 233 | ) 234 | 235 | t01 = ta if choice < 2 else tb 236 | R01 = Rb if choice % 2 else Ra 237 | 238 | return R01, t01, is_pure_rotation 239 | 240 | 241 | def check_pure_rotation( 242 | f0: np.ndarray, Rf1: np.ndarray, th: float = 1 - 1e-8 243 | ) -> Tuple[bool, int]: 244 | """Rotationally-invariant metric for checking if the motion is a pure rotation 245 | 246 | If the motion is a pure rotation, then f0 is the same as R01 @ f1. 247 | To test this, we use the metric: 248 | f0^T (R01 @ f1), with f0 and f1 in R3 and R01 in SO3. 249 | This metric is rotationally invariant since: 250 | f0^T (R01 @ f1) = (R @ f0)^T (R @ (R01 @ f1)), for any R in SO3. 251 | As such, it only depends on the magnitude of the translation vector t01. 252 | For the threshold "th", we found these empirical **approximate** 253 | relations: 254 | | threshold "th" | translation magnitude w.r.t. scene | 255 | | 1 - 1e-14 | 1e-7 | 256 | | 1 - 1e-12 | 1e-6 | 257 | | 1 - 1e-10 | 1e-5 | 258 | | 1 - 1e-8 | 1e-4 | 259 | | 1 - 1e-6 | 1e-3 | 260 | 261 | Args: 262 | f0: (3, n) bearings in camera 0. 263 | Rf1: (2, 3, n) bearings in camera 1, rotated by the candidates R01cand. 264 | 265 | Returns: 266 | is_pure_rotation: True if the motion is a pure rotation. 267 | choice: 0 or 1, depending on which rotation candidate is the correct motion. 268 | """ 269 | metric = np.einsum("dn, cdn -> c", f0, Rf1) / f0.shape[1] 270 | choice, metric = max(enumerate((metric[0], metric[1])), key=lambda x: x[1]) 271 | return metric > th, choice 272 | -------------------------------------------------------------------------------- /tests/test_constraints.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from nonmin_pose.constraints import constraints 4 | from nonmin_pose.constraints.constraints import Parameter 5 | from tests.testing_utils import ( 6 | SyntheticData, 7 | adjoint_of_3x3_mat, 8 | sdpa2mat, 9 | skew, 10 | so3_orbitope, 11 | ) 12 | 13 | CFG_DATASET = { 14 | "seed": 0, 15 | "min_depth": 4.0, 16 | "max_depth": 8.0, 17 | "focal": 800.0, 18 | } 19 | CFG_DATA = { 20 | "transl_magnitude": 1.0, 21 | "euler_ang_magnitude": 0.5, 22 | "max_npoints": 100, 23 | "noise_level": 0.0, 24 | } 25 | 26 | 27 | def create_parameters(): 28 | params = [ 29 | Parameter("E", 1, list(range(1, 10))), 30 | Parameter("t", 1, list(range(10, 13))), 31 | Parameter("q", 1, list(range(13, 16))), 32 | Parameter("h", 1, [16]), 33 | Parameter("R", 1, list(range(17, 26))), 34 | Parameter("sct", 1, [26]), 35 | Parameter("scr", 1, [27]), 36 | Parameter("scr2", 1, [28]), 37 | Parameter("scm1", 1, [29]), 38 | Parameter("scm2", 1, [30]), 39 | Parameter("Zc", 1, list(range(31, 47))), 40 | ] 41 | return {p.name: p for p in params} 42 | 43 | 44 | def sample_data(): 45 | dataset = SyntheticData(**CFG_DATASET) 46 | data = dataset.generate_data(**CFG_DATA) 47 | h, sct, scr, scr2, scm1, scm2 = 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 48 | q = data["R01"].T @ data["t01_unit"] 49 | x = np.concatenate( 50 | ( 51 | data["E01"].ravel(), 52 | data["t01_unit"].ravel(), 53 | q.ravel(), 54 | [h], 55 | data["R01"].ravel(), 56 | [sct, scr, scr2, scm1, scm2], 57 | so3_orbitope(data["R01"]).ravel(), 58 | ) 59 | ) 60 | return x[:, None], data 61 | 62 | 63 | def gather_errors(x, A, constraint, constraint_num, is_inequality): 64 | values = constraint.values 65 | if is_inequality: 66 | cond_sdpa_sdpa = np.allclose(values, np.zeros_like(values)) 67 | cond_data_sdpa = np.allclose((x.T @ A @ x).squeeze(), constraint_num) 68 | else: 69 | cond_sdpa_sdpa = np.allclose((x.T @ A @ x).squeeze(), values) 70 | cond_data_sdpa = np.allclose(constraint_num, values) 71 | 72 | errors = [] 73 | if not cond_sdpa_sdpa: 74 | if is_inequality: 75 | errors.append("SDPA coefficients are not zero.") 76 | else: 77 | errors.append("SDPA coefficients lead to different SDPA values.") 78 | if not cond_data_sdpa: 79 | errors.append( 80 | "SDPA values are different than those derived from data." 81 | f"\n{(x.T @ A @ x).squeeze()}\n{constraint_num}" 82 | ) 83 | success = len(errors) == 0 84 | err_msg = "Errors:\n{}".format("\n".join(errors)) 85 | return success, err_msg 86 | 87 | 88 | def obtain_errors(constraint_class, x, constraint_num, f0=None, f1=None): 89 | params = create_parameters() 90 | constraint = constraint_class(params, 0, 0, None) 91 | 92 | is_inequality = constraint.__class__.__name__.startswith("Cheirality") 93 | if is_inequality: 94 | constraint.compute_coeffs(constraint.coeffs, f0, f1) 95 | 96 | A = sdpa2mat(constraint, block_sizes=[len(x)], ndim=len(x)) 97 | 98 | errors = gather_errors(x, A, constraint, constraint_num, is_inequality) 99 | return errors 100 | 101 | 102 | def test_manif_def_left(): 103 | x, data = sample_data() 104 | 105 | E01, t01_unit = data["E01"], data["t01_unit"] 106 | constraint_num = E01 @ E01.T - skew(t01_unit) @ skew(t01_unit).T 107 | constraint_num = constraint_num.ravel()[[0, 4, 8, 1, 2, 5]] 108 | 109 | success, err_msg = obtain_errors(constraints.ManifDefLeft, x, constraint_num) 110 | assert success, err_msg 111 | 112 | 113 | def test_manif_def_right(): 114 | x, data = sample_data() 115 | 116 | E01 = data["E01"] 117 | q = data["R01"].T @ data["t01_unit"] 118 | constraint_num = E01.T @ E01 - skew(q).T @ skew(q) 119 | constraint_num = constraint_num.ravel()[[0, 4, 8, 1, 2, 5]] 120 | 121 | success, err_msg = obtain_errors(constraints.ManifDefRight, x, constraint_num) 122 | assert success, err_msg 123 | 124 | 125 | def test_normt(): 126 | x, data = sample_data() 127 | 128 | t01_unit = data["t01_unit"] 129 | constraint_num = t01_unit.T @ t01_unit 130 | 131 | success, err_msg = obtain_errors(constraints.NormT, x, constraint_num) 132 | assert success, err_msg 133 | 134 | 135 | def test_normq(): 136 | x, data = sample_data() 137 | 138 | q = data["R01"].T @ data["t01_unit"] 139 | constraint_num = q.T @ q 140 | 141 | success, err_msg = obtain_errors(constraints.NormQ, x, constraint_num) 142 | assert success, err_msg 143 | 144 | 145 | def test_e_def_left(): 146 | x, data = sample_data() 147 | 148 | E01, t01, R01 = data["E01"], data["t01_unit"], data["R01"] 149 | constraint_num = (E01 - skew(t01) @ R01).ravel() 150 | 151 | success, err_msg = obtain_errors(constraints.EDefLeft, x, constraint_num) 152 | assert success, err_msg 153 | 154 | 155 | def test_e_def_right(): 156 | x, data = sample_data() 157 | 158 | E01, R01 = data["E01"], data["R01"] 159 | q = R01.T @ data["t01_unit"] 160 | constraint_num = (E01 - R01 @ skew(q)).ravel() 161 | 162 | success, err_msg = obtain_errors(constraints.EDefRight, x, constraint_num) 163 | assert success, err_msg 164 | 165 | 166 | def test_e_def_left_right(): 167 | x, data = sample_data() 168 | 169 | R01, t01 = data["R01"], data["t01_unit"] 170 | q = R01.T @ t01 171 | constraint_num = (skew(t01) @ R01 - R01 @ skew(q)).ravel() 172 | 173 | success, err_msg = obtain_errors(constraints.EDefLeftRight, x, constraint_num) 174 | assert success, err_msg 175 | 176 | 177 | def test_homogenization(): 178 | x, _ = sample_data() 179 | constraint_num = 1.0 180 | success, err_msg = obtain_errors(constraints.Homogenization, x, constraint_num) 181 | assert success, err_msg 182 | 183 | 184 | def test_adjoint(): 185 | x, data = sample_data() 186 | 187 | E01, t01 = data["E01"], data["t01_unit"] 188 | q = data["R01"].T @ t01 189 | adjoint = adjoint_of_3x3_mat(E01) 190 | constraint_num = (adjoint - q @ t01.T).T.ravel() 191 | 192 | success, err_msg = obtain_errors(constraints.Adjoint, x, constraint_num) 193 | assert success, err_msg 194 | 195 | 196 | def test_norm_e(): 197 | x, data = sample_data() 198 | 199 | E01 = data["E01"] 200 | constraint_num = E01.ravel().dot(E01.ravel()) 201 | 202 | success, err_msg = obtain_errors(constraints.NormE, x, constraint_num) 203 | assert success, err_msg 204 | 205 | 206 | def test_right_null_space(): 207 | x, data = sample_data() 208 | 209 | E01 = data["E01"] 210 | q = data["R01"].T @ data["t01_unit"] 211 | constraint_num = (E01 @ q).ravel() 212 | 213 | success, err_msg = obtain_errors(constraints.RightNullSpace, x, constraint_num) 214 | assert success, err_msg 215 | 216 | 217 | def test_left_null_space(): 218 | x, data = sample_data() 219 | 220 | E01, t01 = data["E01"], data["t01_unit"] 221 | constraint_num = (t01.T @ E01).ravel() 222 | 223 | success, err_msg = obtain_errors(constraints.LeftNullSpace, x, constraint_num) 224 | assert success, err_msg 225 | 226 | 227 | def test_cheirality_translation(): 228 | x, data = sample_data() 229 | 230 | t01, R01 = data["t01_unit"], data["R01"] 231 | f0, f1 = data["f0"], data["f1"] 232 | q = R01.T @ t01 233 | f0_agg, f1_agg = f0.sum(1, keepdims=True), f1.sum(1, keepdims=True) 234 | 235 | constraint_num = (f0_agg.T @ R01 @ q - t01.T @ R01 @ f1_agg).ravel() 236 | 237 | success, err_msg = obtain_errors( 238 | constraints.CheiralityTranslation, x, constraint_num, f0, f1 239 | ) 240 | assert success, err_msg 241 | 242 | 243 | def test_cheirality_translation_v2(): 244 | x, data = sample_data() 245 | 246 | t01 = data["t01_unit"] 247 | f0, f1 = data["f0"], data["f1"] 248 | q = data["R01"].T @ t01 249 | f0_agg, f1_agg = f0.mean(1, keepdims=True), f1.mean(1, keepdims=True) 250 | 251 | constraint_num = (f0_agg.T @ t01 - q.T @ f1_agg).ravel() 252 | 253 | success, err_msg = obtain_errors( 254 | constraints.CheiralityTranslationV2, x, constraint_num, f0, f1 255 | ) 256 | assert success, err_msg 257 | 258 | 259 | def test_cheirality_rotation(): 260 | x, data = sample_data() 261 | 262 | E01, t01 = data["E01"], data["t01_unit"] 263 | f0, f1 = data["f0"], data["f1"] 264 | n = f0.shape[1] 265 | 266 | E_skewt = E01.T @ skew(t01) 267 | constraint_num = ( 268 | sum((f1i[None] @ E_skewt @ f0i[:, None])[0, 0] for f0i, f1i in zip(f0.T, f1.T)) 269 | / n 270 | ) 271 | 272 | success, err_msg = obtain_errors( 273 | constraints.CheiralityRotation, x, constraint_num, f0, f1 274 | ) 275 | assert success, err_msg 276 | 277 | 278 | def test_cheirality_rotation_q(): 279 | x, data = sample_data() 280 | 281 | E01 = data["E01"] 282 | q = data["R01"].T @ data["t01_unit"] 283 | f0, f1 = data["f0"], data["f1"] 284 | 285 | E_skewq = E01 @ skew(q) 286 | constraint_num = sum( 287 | (f0i[None] @ E_skewq @ f1i[:, None])[0, 0] for f0i, f1i in zip(f0.T, f1.T) 288 | ) 289 | 290 | success, err_msg = obtain_errors( 291 | constraints.CheiralityRotationQ, x, constraint_num, f0, f1 292 | ) 293 | assert success and constraint_num <= 0, err_msg 294 | 295 | 296 | def test_cheirality_midpoint(): 297 | x, data = sample_data() 298 | 299 | t01, R01 = data["t01_unit"], data["R01"] 300 | f0, f1 = data["f0"], data["f1"] 301 | q = R01.T @ t01 302 | 303 | # fmt:off 304 | sum0 = sum( 305 | ( 306 | -(t01.T @ R01 @ f1i[:, None]) 307 | + (f0i[None] @ R01 @ f1i[:, None]) * (f0i[None] @ t01) 308 | )[0, 0] 309 | for f0i, f1i in zip(f0.T, f1.T) 310 | ) 311 | sum1 = sum( 312 | ( 313 | (f0i[None] @ R01 @ q) 314 | - (f1i[None] @ q) * (f0i[None] @ R01 @ f1i[:, None]) 315 | )[0, 0] 316 | for f0i, f1i in zip(f0.T, f1.T) 317 | ) 318 | # fmt:on 319 | constraint_num = np.array([sum0, sum1]) 320 | 321 | success, err_msg = obtain_errors( 322 | constraints.CheiralityMidpoint, x, constraint_num, f0, f1 323 | ) 324 | assert success, err_msg 325 | 326 | 327 | def test_orthogonality(): 328 | x, data = sample_data() 329 | 330 | R01 = data["R01"] 331 | 332 | constraint_num = np.concatenate( 333 | ( 334 | (R01 @ R01.T).ravel()[[0, 4, 8, 1, 2, 5]], 335 | (R01.T @ R01).ravel()[[4, 8, 1, 2, 5]], 336 | ) 337 | ) 338 | success, err_msg = obtain_errors(constraints.Orthogonality, x, constraint_num) 339 | assert success, err_msg 340 | 341 | 342 | def test_determinant_R(): 343 | x, data = sample_data() 344 | 345 | R01 = data["R01"] 346 | constraint_num = (R01 - adjoint_of_3x3_mat(R01).T).ravel() 347 | 348 | success, err_msg = obtain_errors(constraints.DeterminantR, x, constraint_num) 349 | assert success, err_msg 350 | 351 | 352 | def test_t_q_definition(): 353 | x, data = sample_data() 354 | 355 | t01, R01 = data["t01_unit"], data["R01"] 356 | q = R01.T @ t01 357 | constraint_num = np.concatenate((t01 - R01 @ q, q - R01.T @ t01))[:, 0] 358 | 359 | success, err_msg = obtain_errors(constraints.TQDefinition, x, constraint_num) 360 | assert success, err_msg 361 | 362 | 363 | def test_skew_t_q_definition(): 364 | x, data = sample_data() 365 | 366 | E01, t01, R01 = data["E01"], data["t01_unit"], data["R01"] 367 | q = R01.T @ t01 368 | constraint_num = np.concatenate( 369 | ((skew(t01) - E01 @ R01.T).ravel(), (skew(q) - R01.T @ E01).ravel()) 370 | ) 371 | 372 | success, err_msg = obtain_errors(constraints.SkewTQDefinition, x, constraint_num) 373 | assert success, err_msg 374 | 375 | 376 | def test_convex_hull_so3(): 377 | x, data = sample_data() 378 | 379 | R01 = data["R01"] 380 | orbitope = so3_orbitope(R01) 381 | min_eig = np.linalg.eigvalsh(orbitope).min() 382 | constraint_num = np.eye(4).ravel()[[0, 1, 2, 3, 5, 6, 7, 10, 11, 15]] 383 | 384 | success, err_msg = obtain_errors(constraints.ConvexHullSO3, x, constraint_num) 385 | assert success and min_eig >= -1e-12, err_msg 386 | -------------------------------------------------------------------------------- /experiments/real_data_strecha.py: -------------------------------------------------------------------------------- 1 | import os 2 | import shutil 3 | import urllib.request 4 | from argparse import ArgumentParser 5 | from math import atan, cos 6 | from pathlib import Path 7 | 8 | import cv2 9 | import matplotlib.pyplot as plt 10 | import numpy as np 11 | from PIL import Image 12 | from pyopengv import pyopengv as gv 13 | from tqdm import tqdm 14 | 15 | from exp_utils import compute_error_metrics, plot_boxplots, plot_matches, skew 16 | from nonmin_pose import C2P, C2PFast, EssentialGSalguero, EssentialZhao 17 | 18 | SEQUENCE = "castle-P19" 19 | 20 | SEQ_MAPS = { 21 | "fountain-P11": "fountain_dense", 22 | "Herz-Jesus-P8": "herzjesu_dense", 23 | "entry-P10": "castle_entry_dense", 24 | "castle-P19": "castle_dense", 25 | "Herz-Jesus-P25": "herzjesu_dense_large", 26 | "castle-P30": "castle_dense_large", 27 | } 28 | 29 | 30 | def do_request(url, file_name, do_uncompress=False, do_clean=False): 31 | """Download the file from `url` and save it locally under `file_name`. 32 | ref: https://stackoverflow.com/a/7244263/14559854 33 | """ 34 | with urllib.request.urlopen(url) as response, open(file_name, "wb") as out_file: 35 | shutil.copyfileobj(response, out_file) 36 | if do_uncompress: 37 | shutil.unpack_archive(file_name, Path(file_name).parent) 38 | if do_clean: 39 | os.remove(file_name) 40 | 41 | 42 | def download_data(data_dir: Path, sequence: str, remove_tar: bool = True): 43 | data_dir.mkdir(parents=True, exist_ok=True) 44 | seq_name = SEQ_MAPS[sequence] 45 | base_url = ( 46 | "https://documents.epfl.ch/groups/c/cv/cvlab-unit/www/data/multiview/data/" 47 | ) 48 | seq_url = base_url + f"{seq_name}/urd/" 49 | print("Downloading and extracting data...") 50 | # .png images. 51 | file = f"{seq_name}_images.tar.gz" 52 | do_request(seq_url + file, str(data_dir / file), True, do_clean=remove_tar) 53 | # camera files (containing K, R, t). 54 | file = f"{seq_name}_cameras.tar.gz" 55 | do_request(seq_url + file, str(data_dir / file), True, do_clean=remove_tar) 56 | # Projection matrices: K * (R | t). 57 | file = f"{seq_name}_p.tar.gz" 58 | do_request(seq_url + file, str(data_dir / file), True, do_clean=remove_tar) 59 | print("Files downloaded and extracted!") 60 | 61 | 62 | def read_data(data_dir: Path, check_with_pmats: bool = False): 63 | """Return images, calibration matrices, rotation matrices and translation vectors. 64 | 65 | Table with Strecha's file .png.camera format: 66 | ---------------------------------------------------------------------------------- 67 | | Lines | Content | 68 | |-------|------------------------------------------------------------------------| 69 | | 1-3 | K: 3X3 calib matrix | 70 | | 4 | row with 3 zeros | 71 | | 5-7 | Rwc: 3X3 rotation rotating points from cam. to world reference systems | 72 | | | i.e. pw = Rwc pc | 73 | | 8 | twc: translation vector, again from cam. to world reference system | 74 | | 9 | width and height (in pixels) of the image | 75 | ---------------------------------------------------------------------------------- 76 | As such, the projection matrix, P, projecting points from world to homogeneous 77 | image coordinates is given by: 78 | P = K * [Rwc^T | -Rwc^T t] 79 | 80 | Args: 81 | data_dir (Path): Path to the directory containing the data. 82 | 83 | Returns: 84 | ims: ndarray (n, h, w, 3) with the n images. 85 | Ks: ndarray (n, 3, 3) with the n calibration matrices. 86 | Rwcs: ndarray (n, 3, 3) with the n rotation matrices (cam. to world coords.). 87 | twcs: ndarray (n, 3, 1) with the n translation vectors (cam. to world coords.). 88 | """ 89 | # Strecha's naming convention for the sequences is "{seq}-P{number_of_images}". 90 | n = int(data_dir.name.rpartition("P")[2]) 91 | data_dir = data_dir / SEQ_MAPS[data_dir.name] / "urd" 92 | assert data_dir.is_dir(), f"Data directory {data_dir} does not exist." 93 | 94 | # images. 95 | ims = np.stack( 96 | [ 97 | np.array(Image.open(p), dtype=np.uint8) 98 | for p in sorted(data_dir.glob("*.png")) 99 | ] 100 | ) 101 | assert n == len(ims), f"Expected {n} images, got {ims.shape[0]}." 102 | 103 | # calibration matrices and extrinsic params. 104 | Rwcs = np.empty((n, 3, 3)) 105 | twcs = np.empty((n, 3, 1)) 106 | Ks = np.zeros((n, 3, 3)) 107 | Ks[:, 2, 2] = 1.0 108 | 109 | for i, p in enumerate(sorted(data_dir.glob("*.png.camera"))): 110 | with open(p, "r") as f: 111 | lines = [line.split() for line in f.readlines()] 112 | # calibration matrix. 113 | Ks[i, 0, 0] = lines[0][0] # fx 114 | Ks[i, 0, 2] = lines[0][2] # cx 115 | Ks[i, 1, 1] = lines[1][1] # fy 116 | Ks[i, 1, 2] = lines[1][2] # cy 117 | # rotation matrices. 118 | Rwcs[i] = lines[4:7] 119 | # translation vectors. 120 | twcs[i, :, 0] = lines[7] 121 | 122 | if check_with_pmats: 123 | # read projection matrices. 124 | Ps = np.empty((n, 3, 4)) 125 | for i, p in enumerate(sorted(data_dir.glob("*.png.P"))): 126 | with open(p, "r") as f: 127 | Ps[i] = [line.split() for line in f.readlines()] 128 | assert i == n - 1, f"Expected {n} projection matrices, got {i + 1}." # type: ignore 129 | # ensure that they are consistent with the read Ks, Rwcs and twcs. 130 | Ps_to_test = Ks @ np.concatenate( 131 | (Rwcs.transpose(0, 2, 1), -Rwcs.transpose(0, 2, 1) @ twcs), axis=2 132 | ) 133 | assert ( 134 | np.max(np.abs(Ps - Ps_to_test)) 135 | < 0.2 # we use 0.2 since elements in .png.P files have 1 decimal place. 136 | ), "Inconsistent projection matrices." 137 | 138 | return ims, Ks, Rwcs, twcs 139 | 140 | 141 | def extract_local_features(ims: np.ndarray): 142 | """Extract local features (2D points + desc.) from the images using DoG + SIFT.""" 143 | 144 | def rootsift(descs): 145 | descs = descs / (descs.sum(1, keepdims=True)) # L1 normalization 146 | descs = np.sqrt(descs) # now is L2 normalized: ||sqrt(x_l1)|| = sum(x_l1) = 1 147 | # but for greater precision, we ensure it is L2 normalized. 148 | descs /= np.linalg.norm(descs, axis=1, keepdims=True) 149 | return descs 150 | 151 | assert ims.ndim == 4 and ims.shape[3] == 3, "Expected color images." 152 | gray_ims = [cv2.cvtColor(im, cv2.COLOR_RGB2GRAY) for im in ims] 153 | local_feat_extractor = cv2.SIFT_create() # pyright: ignore[reportGeneralTypeIssues] 154 | 155 | print("Extracting local features...") 156 | kps_per_im, descs_per_im = [], [] 157 | for im in tqdm(gray_ims, desc="Extracting local features", leave=False): 158 | kps, descs = local_feat_extractor.detectAndCompute(im, None) 159 | descs_per_im.append(rootsift(descs)) 160 | # get 2d coordinates. 161 | kps_per_im.append(np.array([kp.pt for kp in kps])) 162 | print("Done!") 163 | return kps_per_im, descs_per_im 164 | 165 | 166 | def bearings_from_2d_points(kps_per_im, Ks): 167 | """Return the unit bearing vectors corresponding to the 2D points in each image.""" 168 | assert len(kps_per_im) == len(Ks), "Expected same number of images and Ks." 169 | bearings_per_im = [] 170 | for kps, K in zip(kps_per_im, Ks): 171 | # 2d points in normalized plane. 172 | kps = cv2.undistortPoints(kps, K, None)[:, 0].T # type:ignore (2, n) 173 | # corresponding bearings. 174 | kps = np.concatenate((kps, np.ones_like(kps[:1])), axis=0) 175 | bearings = kps / np.linalg.norm(kps, axis=0, keepdims=True) 176 | bearings_per_im.append(bearings) 177 | return bearings_per_im 178 | 179 | 180 | class StrechaDataset: 181 | def __init__( 182 | self, 183 | data_dir: Path, 184 | sequence: str = "castle-P19", 185 | step: int = 1, 186 | download: bool = False, 187 | remove_tar: bool = True, 188 | ): 189 | self.step = step 190 | if download: 191 | download_data(data_dir, sequence, remove_tar) 192 | 193 | self.ims, self.Ks, Rwcs, twcs = read_data(data_dir, check_with_pmats=True) 194 | self._len = len(self.ims) 195 | 196 | self.kps_per_im, self.descs_per_im = extract_local_features(self.ims) 197 | self.bearings_per_im = bearings_from_2d_points(self.kps_per_im, self.Ks) 198 | 199 | # get relative poses. 200 | Rw0, Rw1 = Rwcs[:-step], Rwcs[step:] 201 | tw0, tw1 = twcs[:-step], twcs[step:] 202 | self.R01s = Rw0.transpose(0, 2, 1) @ Rw1 203 | self.t01s = Rw0.transpose(0, 2, 1) @ (tw1 - tw0) 204 | 205 | self.last_idx = self._len - self.step 206 | 207 | def __len__(self): 208 | return self._len 209 | 210 | def __getitem__(self, idx): 211 | """Return data for the idx-th image pair: relative pose, calib mats and matches""" 212 | assert ( 213 | idx < self._len - self.step 214 | ), f"Index {idx} out of bounds with step {self.step}." 215 | 216 | d0, d1 = self.descs_per_im[idx], self.descs_per_im[idx + self.step] 217 | matches = pairwise_matching(d0, d1) 218 | idx0, idx1 = matches[:, 0], matches[:, 1] 219 | 220 | data = { 221 | "f0": self.bearings_per_im[idx][:, idx0], # (3,n) 222 | "f1": self.bearings_per_im[idx + self.step][:, idx1], # (3,n) 223 | "kps0": self.kps_per_im[idx][idx0], # (n,2) 224 | "kps1": self.kps_per_im[idx + self.step][idx1], # (n,2) 225 | "R01": self.R01s[idx], 226 | "t01": self.t01s[idx], 227 | "K0": self.Ks[idx], 228 | "K1": self.Ks[idx + self.step], 229 | } 230 | return data 231 | 232 | def plot_matches(self, idx, kps0_m, kps1_m, ransac_mask, out_dir: Path): 233 | fig, ax = plt.subplots(1, 2) 234 | 235 | ax[0].imshow(self.ims[idx]) 236 | ax[1].imshow(self.ims[idx + self.step]) 237 | 238 | # all keypoints (including those that are not matched). 239 | kps0, kps1 = self.kps_per_im[idx], self.kps_per_im[idx + self.step] 240 | ax[0].scatter(kps0[:, 0], kps0[:, 1], c="b", s=1, alpha=0.5) 241 | ax[1].scatter(kps1[:, 0], kps1[:, 1], c="b", s=1, alpha=0.5) 242 | 243 | # plot all (pre and post ransac) matches. 244 | kw_pre_p = {"s": 1.0, "c": [(1.0, 0.0, 0.0)]} 245 | kw_pre_l = {"lw": 0.1, "color": (1.0, 0.0, 0.0)} 246 | plot_matches(fig, ax[0], ax[1], kps0_m.T, kps1_m.T, kw_pre_p, kw_pre_l) 247 | # plot ransac inliers. 248 | kps0_m, kps1_m = kps0_m[ransac_mask], kps1_m[ransac_mask] 249 | kw_post_p = {"s": 1.0, "c": [(0.0, 1.0, 0.0)]} 250 | kw_post_l = {"lw": 0.1, "color": (0.0, 1.0, 0.0), "alpha": 1.0} 251 | plot_matches(fig, ax[0], ax[1], kps0_m.T, kps1_m.T, kw_post_p, kw_post_l) 252 | 253 | # save. 254 | out_dir = out_dir / "matches" 255 | out_dir.mkdir(parents=True, exist_ok=True) 256 | fname = str(out_dir / f"{idx}_{idx+self.step}.png") 257 | ax[0].set_axis_off() 258 | ax[1].set_axis_off() 259 | fig.tight_layout(pad=0.5) 260 | fig.savefig(fname, bbox_inches="tight", pad_inches=0.0) 261 | 262 | plt.close(fig) 263 | 264 | 265 | def pairwise_matching(d0, d1, ratio_th: float = 0.8): 266 | """Pairwise matching of SIFT descriptors with Lowe's ratio test and MNN check.""" 267 | ind0 = np.arange(len(d0)) # (n0,) 268 | mask0 = np.ones(len(d0), dtype=bool) 269 | # similarity matrix. 270 | sim = d0 @ d1.T 271 | # nearest neighbours for each descriptor in image 0 and 1. 272 | nn0 = np.argpartition(sim, -2 if ratio_th else -1, axis=1) # (n0, n1) 273 | nn1 = np.argpartition(sim.T, -2 if ratio_th else -1, axis=1) # (n1, n0) 274 | if ratio_th: 275 | # Lowe's ratio test. 276 | dist = 2.0 * (1.0 - sim) 277 | mask0 &= dist[ind0, nn0[:, -1]] <= ratio_th**2 * dist[ind0, nn0[:, -2]] 278 | # MNN check. 279 | mask0 &= ind0 == nn1[nn0[ind0, -1], -1] 280 | # final matched idx. 281 | matches = np.stack((ind0[mask0], nn0[mask0, -1]), axis=1) # (n, 2) 282 | return matches 283 | 284 | 285 | def filter_outliers_opengv(data, method="NISTER", th=5.0): 286 | """Outlier filtering with OpenGV's 3D based ransac. 287 | 288 | The most important parameter is the angular threshold, which is explained in: 289 | https://laurentkneip.github.io/opengv/page_how_to_use.html#sec_ransac 290 | """ 291 | threshold = 1.0 - cos(atan(th / 2_700)) 292 | max_iters = 1_000 293 | prob = 0.99 294 | 295 | f0, f1 = data["f0"], data["f1"] 296 | Rt01 = gv.relative_pose_ransac( 297 | b1=f0.T, 298 | b2=f1.T, 299 | algo_name=method, 300 | threshold=threshold, 301 | iterations=max_iters, 302 | probability=prob, 303 | ) 304 | R01 = Rt01[:3, :3].copy() 305 | t01 = Rt01[:3, 3:] / np.linalg.norm(Rt01[:3, 3]) 306 | 307 | # (estimated) bearings with the estimated relative pose. 308 | p3d_0 = gv.triangulation_triangulate2(f0.T, f1.T, t01, R01).T # (3, n) 309 | f0_reproj = p3d_0 / np.linalg.norm(p3d_0, axis=0, keepdims=True) 310 | f1_reproj = R01.T @ p3d_0 - R01.T @ t01 311 | f1_reproj /= np.linalg.norm(f1_reproj, axis=0, keepdims=True) 312 | 313 | # angular threshold to select inliers (transformed to be \in [0, 2]). 314 | ang_error0 = 1.0 - np.einsum("dn,dn->n", f0, f0_reproj) 315 | ang_error1 = 1.0 - np.einsum("dn,dn->n", f1, f1_reproj) 316 | inliers_mask = (ang_error0 + ang_error1) <= threshold 317 | 318 | f0_inliers = f0[:, inliers_mask] 319 | f1_inliers = f1[:, inliers_mask] 320 | 321 | pose_est = {"R01": R01, "t01": t01, "E01": skew(t01) @ R01} 322 | return pose_est, f0_inliers, f1_inliers, inliers_mask 323 | 324 | 325 | def main(args, methods): 326 | assert len(methods) > 0, "No methods to evaluate." 327 | # the dataset {will be downloaded / is expected} at this location. 328 | data_dir = Path(__file__).parent / "data" / "strecha" / args.sequence 329 | results_dir = Path(__file__).parent / "results" / "strecha" / args.sequence 330 | dataset = StrechaDataset(data_dir, args.sequence, step=1, download=args.download) 331 | ransac_fun = filter_outliers_opengv 332 | 333 | r_errors = np.zeros((len(methods), dataset.last_idx)) 334 | t_errors = np.zeros((len(methods), dataset.last_idx)) 335 | 336 | for i in tqdm(range(dataset.last_idx), desc="Evaluating methods"): 337 | # tentative matches + ground-truth. 338 | data = dataset[i] 339 | # filter potential outliers with ransac. 340 | _, f0_inliers, f1_inliers, inliers_mask = ransac_fun(data) 341 | dataset.plot_matches(i, data["kps0"], data["kps1"], inliers_mask, results_dir) 342 | data["f0"] = f0_inliers 343 | data["f1"] = f1_inliers 344 | 345 | for k, method in enumerate(methods): 346 | pose_est = method(f0_inliers, f1_inliers) 347 | r_errors[k, i], t_errors[k, i] = compute_error_metrics(data, pose_est) 348 | 349 | print([method.label for method in methods]) 350 | print(r_errors.mean(1), np.median(r_errors, 1)) 351 | print(t_errors.mean(1), np.median(t_errors, 1)) 352 | 353 | # plot boxplots. 354 | fig, ax = plt.subplots(1, 2, layout="constrained", figsize=(15, 6)) 355 | x_labels = [method.label for method in methods] 356 | plot_boxplots(ax[0], r_errors.T, x_labels, sample_points=False, showfliers=False) 357 | plot_boxplots(ax[1], t_errors.T, x_labels, sample_points=False, showfliers=False) 358 | 359 | fs = 20 360 | ax[0].set_ylabel("rotation error (deg)", fontsize=fs) 361 | ax[1].set_ylabel("translation error (deg)", fontsize=fs) 362 | for axi in ax: 363 | # axi.set_xlabel("method", fontsize=fs) 364 | axi.tick_params(axis="both", which="major", labelsize=16) 365 | 366 | fname = str(results_dir / "boxplots") 367 | fig.savefig(fname + ".png", bbox_inches="tight") 368 | fig.savefig(fname + ".pdf", bbox_inches="tight") 369 | 370 | 371 | if __name__ == "__main__": 372 | parser = ArgumentParser() 373 | parser.add_argument("--download", action="store_true") 374 | parser.add_argument("--sequence", type=str, default=SEQUENCE) 375 | args = parser.parse_args() 376 | 377 | # nonminimal models to evaluate. 378 | essmat_zhao = EssentialZhao() 379 | essmat_gsalguero = EssentialGSalguero() 380 | relpose_ours = C2P() 381 | relpose_ours_f = C2PFast() 382 | # plot labels. 383 | essmat_zhao.label = "Zhao" # type: ignore 384 | essmat_gsalguero.label = "G-Salg." # type: ignore 385 | relpose_ours.label = "C2P" # type: ignore 386 | relpose_ours_f.label = "C2P-fast." # type: ignore 387 | 388 | # gather solvers. 389 | methods = [ 390 | essmat_zhao, 391 | essmat_gsalguero, 392 | relpose_ours, 393 | relpose_ours_f, 394 | ] 395 | 396 | main(args, methods) 397 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

From Correspondences to Pose:
Non-minimal Certifiably Optimal Relative Pose
without Disambiguation

3 |

Javier Tirado-Garín    Javier Civera
4 | I3A, University of Zaragoza

5 | 6 |

Relative pose directly from matches, without additional steps for disambiguation and pure rotation checks.

7 |

8 | arXiv | 9 | Video | 10 | Project Page 11 |

12 |
13 | 14 | ## Installation 15 | 16 | For ease of installation, we recommend installing the package via PyPi. We provide wheels for Linux, MacOS, and Windows, which also install the necessary Python bindings for the Semidefinite Programming (SDP) solver [SDPA](https://sdpa.sourceforge.net/). The project can be installed by executing: 17 | ```shell 18 | pip install nonmin-pose 19 | ``` 20 | 21 | ## Usage 22 | 23 | ### C2P and C2P-fast 24 | 25 | Instances of C2P and C2P-fast can be created as follows: 26 | ```python 27 | from nonmin_pose import C2P, C2PFast 28 | 29 | configuration = { 30 | # threshold for the singular values of X to check if rank(X)\in[1,3] (condition for tightness), 31 | # where X is the SDP-solution submatrix corresponding to E, t, q and h. 32 | "th_rank_optimality": 1e-5, # default 33 | # threshold for the slack variable "st" for detecting (near-)pure rotations. 34 | "th_pure_rot_sdp": 1e-3, # default 35 | # threshold for "st" used for detecting noise-free pure rotations 36 | # and improving the numerical accuracy. 37 | "th_pure_rot_noisefree_sdp": 1e-4, # default 38 | } 39 | 40 | solver = C2P(cfg=configuration) 41 | solver_fast = C2PFast(cfg=configuration) 42 | ``` 43 | 44 | Given a set of `n` correspondences (pairs of unit bearing vectors), represented as two numpy ndarrays, `f0` and `f1`, each of shape `(3, n)`, all solvers can be called and the solution estimates can be recovered as follows: 45 | 46 | ```python 47 | solution = solver(f0, f1) 48 | 49 | # essential matrix 50 | E01 = solution["E01"] 51 | # relative rotation and translation 52 | R01 = solution["R01"] 53 | t01 = solution["t01"] 54 | # certificate 55 | is_optimal = solution["is_optimal"] 56 | # pure rotation check 57 | is_pure_rot = solution["is_pure_rot"] 58 | ``` 59 | 60 |
61 | [Click to see docstring and optional arguments] 62 | 63 | ```python 64 | """Non-minimal relative pose estimation. 65 | 66 | Args: 67 | f0: (3, n) bearing vectors in camera 0. 68 | f1: (3, n) bearing vectors in camera 1. 69 | w: (n,) array of weights for each residual. (default: None). 70 | already_unitary: True if the input coordinates are already unitary 71 | (default: False). 72 | do_disambiguation: True to decompose the essential matrix into relative 73 | rotation and translation. This argument is ignored when using C2P and is 74 | only used for the essential matrix solvers of Zhao and Garcia-Salguero. 75 | (default: True). 76 | 77 | Returns: 78 | sol: dict of solution parameters, with keys and values: 79 | - E01: (3, 3) essential matrix. 80 | - R01: (3, 3) rotation matrix. It is not returned for the methods of 81 | Zhao and G.Salguero et al. if do_disambiguation is False. 82 | - t01: (3, 1) translation vector. It is not returned for the methods of 83 | Zhao and G.Salguero et al. if do_disambiguation is False. 84 | - is_optimal: True if the solution is optimal. 85 | - is_pure_rot: True if a (near-)pure rotation is detected. It is not 86 | returned for the methods of Zhao and G.Salguero et al. if 87 | do_disambiguation is False. 88 | """ 89 | ``` 90 | 91 |
92 | 93 | #### Notation used in the code 94 | 95 | Notation 96 | 97 | * (Algebraic) Epipolar errors are defined as 98 | ```math 99 | \mathbf{f}_{0}^{\top} \mathbf{E}_{01} \mathbf{f}_1 100 | ``` 101 | * The relative rotation and translation, $\mathbf{R} _{01}$ and $\mathbf{t} _{01}$, map a 3D point expressed in cam {1} to cam {0} as 102 | ```math 103 | \lambda_0 \mathbf{f}_0 = \lambda_1 \mathbf{R}_{01} \mathbf{f}_1 + \mathbf{t}_{01} 104 | ``` 105 | where $\lambda_0,\lambda_1\in\mathbb{R}$ represent the norm of the 3D point when expressed in cam {0} and cam {1}, respectively. 106 | 107 | 108 | ### Zhao's [[2]](#2) and García-Salguero et al.'s [[3]](#3) methods 109 | 110 | We also provide Python classes for the methods of J. Zhao [[2]](#2) and M. García-Salguero et al. [[3]](#3), which can be instantiated as follows: 111 | ```python 112 | from nonmin_pose import EssentialZhao, EssentialGSalguero 113 | 114 | configuration = { 115 | # threshold on the singular values of of the diagonal blocks of X, with X the SDP-solution, to check if they are rank-1 (condition for tightness). 116 | "th_rank_optimality": 1e-5, # default 117 | # threshold for the posterior step used for detecting (near-)pure rotations. 118 | "th_pure_rot_post": 1 - 1e-8, # default 119 | } 120 | 121 | solver = EssentialZhao(cfg=configuration) 122 | solver = EssentialGSalguero(cfg=configuration) 123 | ``` 124 | 125 | ## Evaluation 126 | 127 | To reproduce the experiments reported in the paper, please first clone the repository 128 | ```shell 129 | git clone https://github.com/javrtg/C2P.git 130 | cd C2P/experiments 131 | ``` 132 | and then install the [dependencies](experiments/env_experiments.yml), e.g. using conda: 133 | ```shell 134 | conda env create -f env_experiments.yml 135 | conda activate c2p_exp 136 | ``` 137 | 138 | Finally, run the following commands depending on the experiment to be reproduced. All results will be automatically stored under a subfolder named `results`. 139 | 140 | #### Accuracy vs number of correspondences 141 | 142 | ```shell 143 | python accuracy_vs_npoints.py 144 | ``` 145 | 146 | #### Accuracy vs noise levels 147 | 148 | ```shell 149 | python accuracy_vs_noise.py 150 | ``` 151 | 152 | #### Accuracy vs translation magnitude 153 | 154 | ```shell 155 | python accuracy_vs_translation_length.py 156 | ``` 157 | 158 | #### Runtimes 159 | 160 | ```shell 161 | python runtimes.py 162 | ``` 163 | 164 | #### Real-data experiment 165 | 166 | For running this experiment, the official Python bindings of [`OpenGV`](https://laurentkneip.github.io/opengv/) are needed. Please follow the [installation guide](https://laurentkneip.github.io/opengv/page_installation.html) to install the library. Please place the resulting `.so` or `.pyd` under the `Lib/site-packages/pyopengv` folder of your python distribution along an `__init__.py` file similar to 167 | ```python 168 | # __init__.py under Lib/site-packages/pyopengv 169 | from . import pyopengv 170 | ``` 171 | This way, `pyopengv` can be imported from anywhere as 172 | ```python 173 | from pyopengv import pyopengv 174 | ``` 175 | 176 |
177 | [Installation details if using Windows] 178 | 179 | On Windows, installing the library with Visual Studio may be very slow. In case this becomes a problem, below we describe a potentially faster alternative by using the **MinGW64** toolchain. 180 | 181 | MinGW64 provides gcc, making the compilation process akin to a Linux environment. We detail below the steps to install the toolchain and build (py)opengv from source. 182 | > :warning: Warning 183 | > There is a specific issue that needs to be solved first. OpenGV defines `struct timeval` and `gettimeofday` for Windows systems since they are native only to UNIX-like systems. Since MinGW already provides these, conflicts can occur during the build process. To prevent this, we need to modify L33 in [test/time_measurement.cpp](https://github.com/laurentkneip/opengv/blob/91f4b19c73450833a40e463ad3648aae80b3a7f3/test/time_measurement.cpp#L33) and L37 in [test/time_measurement.hpp](https://github.com/laurentkneip/opengv/blob/91f4b19c73450833a40e463ad3648aae80b3a7f3/test/time_measurement.hpp#L37) to change in both: 184 | > ```c++ 185 | > #ifdef WIN32 186 | > ``` 187 | > to 188 | > ```c++ 189 | > #if defined(WIN32) && !defined(__MINGW32__) 190 | > ``` 191 | 192 |
193 | 194 | 195 | ### Setting up MinGW64 toolchain via MSYS2 196 | 197 | 198 | 199 | To install the MinGW64 toolchain, a recommended approach is to first install [MSYS2](https://www.msys2.org/). MSYS2 comes with a package manager, `pacman`, useful for installing and managing additional dependencies like `cmake` and `eigen`. Alternatively, we can install these dependencies using other methods, such as within a [conda](https://docs.conda.io/projects/miniconda/en/latest/miniconda-install.html) or [mamba](https://mamba.readthedocs.io/en/latest/mamba-installation.html#mamba-install) environment. 200 | 201 | following [MSYS2 docs](https://www.msys2.org/docs/updating/), after installing MSYS2, we need to launch the `MSYS2 MSYS` shell and update the package database by running: 202 | ```shell 203 | pacman -Suy 204 | ``` 205 | As the [docs](https://www.msys2.org/docs/updating/) say, we may be prompted to close all terminals if core packages are updated: 206 | ```shell 207 | :: To complete this update all MSYS2 processes including this terminal will be closed. 208 | Confirm to proceed [Y/n] 209 | ``` 210 | If prompted, we need to close the terminal, reopen it, and run `pacman -Suy` again to update remaining packages. 211 | 212 | Next, we need to install the `gcc` and `g++` compilers using: 213 | ```shell 214 | pacman -S --needed base-devel mingw-w64-x86_64-toolchain 215 | ``` 216 |
217 | 218 |
219 | 220 | 221 | ### Installing dependencies 222 | 223 | 224 | 225 | Just as an example, below it is explained how to install the dependencies on a conda/mamba environment, but there are other alternatives, such as installing them in the `MSYS2 MSYS` shell. 226 | 227 | Open a conda prompt and run the following commands: 228 | ```cmd 229 | # create fresh environment to build opengv (replace 3.10 to the desired python version). 230 | (base) C:\random\path> mamba create -n opengv python=3.10 cmake ninja eigen -y 231 | 232 | # activate the new environment 233 | (base) C:\random\path> mamba activate opengv 234 | (opengv) C:\random\path> 235 | ``` 236 | 237 |
238 | 239 |
240 | 241 | 242 | ### Building OpenGV 243 | 244 | 245 | 246 | First, to have access to the compilers, we need to add the MINGW64 binaries directory to the the `PATH` environment variable. If MSYS2 was installed using default options, this directory is typically `C:\msys64\mingw64\bin`. 247 | We can temporarily modify the `PATH` variable as follows: 248 | ```cmd 249 | (opengv) C:\random\path> set PATH=%PATH%;C:\msys64\mingw64\bin 250 | ``` 251 | 252 | Finally, to build OpenGV: 253 | ```cmd 254 | # navigate to the cloned opengv repository. 255 | (opengv) C:\random\path> cd \path\to\opengv 256 | 257 | # create build directory. 258 | (opengv) \path\to\opengv> mkdir build && cd build 259 | 260 | # Build using cmake and ninja (adjust flags as needed): 261 | (opengv) \path\to\opengv> cmake -G Ninja .. -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DBUILD_PYTHON=ON && ninja 262 | ``` 263 |
264 | 265 | The build process should take ~3 mins and the resulting compiled libraries will be stored in the folder `\path\to\opengv\build\lib`. 266 | 267 | > :warning: Warning 268 | > Since the Python extension isn't statically compiled, the compiled `.pyd` module (e.g. `pyopengv.cp310-win_amd64.pyd`) will rely on several MinGW `.dll` files at runtime. Thus, before importing `pyopengv`, we need to add the MinGW binaries directory to the DLL search path in Python. This can be done for for Python >= 3.8 as follows: 269 | > ```python 270 | > import os 271 | > with os.add_dll_directory("C:\\msys64\\mingw64\\bin"): 272 | > import pyopengv 273 | > ``` 274 | > Alternatively, and as mentioned earlier, to automate this, we can move the `.pyd` module to a manually created `pyopengv` folder within the `Lib/site-packages` directory of our Python interpreter. Inside this `pyopengv` folder, besides having the `.pyd` file, we need to create a `__init__.py` file that contains something like: 275 | > ```python 276 | > import os 277 | > from pathlib import Path 278 | > 279 | > # constant for MinGW64 directory path. 280 | > MINGW64_PATH = Path("C:\\msys64\\mingw64\\bin") 281 | > 282 | > 283 | > def import_pyopengv(): 284 | > """Import pyopengv, with optional modification to DLL search path.""" 285 | > try: 286 | > # check if the MinGW64 directory exists 287 | > if MINGW64_PATH.is_dir(): 288 | > # augment .dll search path to include MinGW64's bin directory. 289 | > with os.add_dll_directory(str(MINGW64_PATH)): 290 | > from . import pyopengv 291 | > else: 292 | > from . import pyopengv 293 | > return pyopengv 294 | > except ImportError as e: 295 | > raise ImportError(f"Failed to import pyopengv: {e}") 296 | > 297 | > 298 | > import_pyopengv() 299 | > 300 | > # clean up the namespace. 301 | > del os, Path, import_pyopengv, MINGW64_PATH 302 | > ``` 303 | > After this, `pyopengv` can be imported, from any directory, as follows: 304 | > ```python 305 | > from pyopengv import pyopengv 306 | > ``` 307 | 308 |
309 | 310 | Finally, by executing the following command, the experiment will be run and when using the flag `--download`, the necessary data will be automatically downloaded and placed under the folder `experiments/data`. 311 | ```shell 312 | python experiments/real_data_strecha.py --download 313 | ``` 314 | 315 | ## Build from source 316 | 317 | [`SDPA`](https://sdpa.sourceforge.net/) is necessary to solve the SDP relaxation. We recommend following the [SDPA for Python installation guide](https://sdpa-python.github.io/docs/installation/) (without multiprecision requirements) which provides specific instructions for [Linux](https://sdpa-python.github.io/docs/installation/linux.html) | [Windows](https://sdpa-python.github.io/docs/installation/windows.html) | [MacOS](https://sdpa-python.github.io/docs/installation/macos.html). Please note that instead of using [SDPA-python](https://github.com/sdpa-python/sdpa-python), we have created thin bindings to the base SDPA's C++ library to gain more control over the solver's parameters. As such, the steps related to `sdpa-python` in the guide, can be safely skipped. 318 | 319 | After installing SDPA, we need to compile the bindings. Please modify the global variables in [`setup.py`](setup.py) to match the library paths of your operating system. Then, the project can be installed in development mode by executing: 320 | ```shell 321 | cd 322 | pip install -e . 323 | ``` 324 | 325 | ## Acknowledgements and license 326 | 327 | This work is inspired by the following works: 328 | 329 | [1] *A certifiably globally optimal solution to the non-minimal relative pose problem*, J. Briales, L. Kneip, J. Gonzalez-Jimenez, 2018 ([link](https://openaccess.thecvf.com/content_cvpr_2018/html/Briales_A_Certifiably_Globally_CVPR_2018_paper.html)). 330 | 331 | [2] 332 | *An Efficient Solution to Non-Minimal Case Essential Matrix Estimation*, J. Zhao, 2022 ([link](https://arxiv.org/abs/1903.09067)). 333 | 334 | [3] 335 | *A tighter relaxation for the relative pose problem between cameras*, M. García-Salguero, J. Briales, J. Gonzalez-Jimenez, 2022 ([link](https://link.springer.com/article/10.1007/s10851-022-01085-z)). 336 | 337 | 338 | This work provides and uses python bindings to [SDPA](https://sdpa.sourceforge.net/): 339 | 340 | [4] *Implementation and evaluation of SDPA 6.0 (SemiDefinite Programming Algorithm 6.0)*, M. Yamashita, K. Fujisawa, and M. Kojima, Optimization Methods and Software 18, 491-505, 2003. 341 | 342 | [5] *A high-performance software package for semidefinite programs: SDPA 7*, M. Yamashita, K. Fujisawa, K. Nakata, M. Nakata, M. Fukuda, K. Kobayashi, and K. Goto, Research Report B-460 Dept. of Mathematical and Computing Science, Tokyo Institute of Technology, Tokyo, Japan, September, 2010. 343 | 344 | SDPA is licensed under the [GNU Lesser General Public License v2.0](src/COPYING). As such this project is licensed under the [GNU Lesser General Public License v3.0](./LICENSE). 345 | 346 | ## Citation 347 | 348 | If you find this work useful, please consider citing: 349 | ```bibtex 350 | @article{tirado2024c2p, 351 | title={From Correspondences to Pose: Non-minimal Certifiably Optimal Relative Pose without Disambiguation}, 352 | author={Tirado-Gar{\'\i}n, Javier and Civera, Javier}, 353 | journal={CVPR}, 354 | year={2024} 355 | } 356 | ``` 357 | 358 | 403 | 404 | 405 | -------------------------------------------------------------------------------- /src/COPYING: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc. 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Library General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | 294 | Copyright (C) 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License 307 | along with this program; if not, write to the Free Software 308 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 309 | 310 | 311 | Also add information on how to contact you by electronic and paper mail. 312 | 313 | If the program is interactive, make it output a short notice like this 314 | when it starts in an interactive mode: 315 | 316 | Gnomovision version 69, Copyright (C) year name of author 317 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 318 | This is free software, and you are welcome to redistribute it 319 | under certain conditions; type `show c' for details. 320 | 321 | The hypothetical commands `show w' and `show c' should show the appropriate 322 | parts of the General Public License. Of course, the commands you use may 323 | be called something other than `show w' and `show c'; they could even be 324 | mouse-clicks or menu items--whatever suits your program. 325 | 326 | You should also get your employer (if you work as a programmer) or your 327 | school, if any, to sign a "copyright disclaimer" for the program, if 328 | necessary. Here is a sample; alter the names: 329 | 330 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 331 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 332 | 333 | , 1 April 1989 334 | Ty Coon, President of Vice 335 | 336 | This General Public License does not permit incorporating your program into 337 | proprietary programs. If your program is a subroutine library, you may 338 | consider it more useful to permit linking proprietary applications with the 339 | library. If this is what you want to do, use the GNU Library General 340 | Public License instead of this License. 341 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . --------------------------------------------------------------------------------