From 15f70fca0b83c196c659b0a1dbfc394d4085a3f4 Mon Sep 17 00:00:00 2001 From: B1ueber2y Date: Tue, 7 Jan 2025 11:02:04 +0100 Subject: [PATCH 1/9] specify clang-format version --- scripts/clang_format.sh | 32 +++++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/scripts/clang_format.sh b/scripts/clang_format.sh index 7fd6696..65dc27f 100644 --- a/scripts/clang_format.sh +++ b/scripts/clang_format.sh @@ -1,5 +1,35 @@ #!/usr/bin/env bash +# Find clang-format +tools=' + clang-format-8 + clang-format +' + +clang_format='' +for tool in ${tools}; do + if type -p "${tool}" > /dev/null; then + clang_format=$tool + break + fi +done + +if [ -z "$clang_format" ]; then + echo "Could not locate clang-format" + exit 1 +fi +echo "Found clang-format: $(which ${clang_format})" + +# Check version +version_string=$($clang_format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') +expected_version_string='19.1.0' +if [[ "$version_string" =~ "$expected_version_string" ]]; then + echo "clang-format version '$version_string' matches '$expected_version_string'" +else + echo "clang-format version '$version_string' doesn't match '$expected_version_string'" + exit 1 +fi + # Get all C++ files checked into the repo under src/ root_folder=$(git rev-parse --show-toplevel) all_files=$( \ @@ -11,4 +41,4 @@ echo "Formatting ${num_files} files" cd $root_folder clang-format -i --style=file $all_files -cd - \ No newline at end of file +cd - From b6de275e438f24a56d87dba489190a159c5d69cb Mon Sep 17 00:00:00 2001 From: B1ueber2y Date: Tue, 7 Jan 2025 11:02:24 +0100 Subject: [PATCH 2/9] add ruff for python formatting --- ruff.toml | 17 + scripts/python.sh | 26 + setup.py | 22 +- solver_py/scale_and_shift.py | 248 ++++- solver_py/shift_and_scale_shared_focal.py | 909 ++++++++++++++++-- solver_py/shift_and_scale_two_focal.py | 1036 +++++++++++++++++++-- 6 files changed, 2089 insertions(+), 169 deletions(-) create mode 100644 ruff.toml create mode 100644 scripts/python.sh diff --git a/ruff.toml b/ruff.toml new file mode 100644 index 0000000..da11434 --- /dev/null +++ b/ruff.toml @@ -0,0 +1,17 @@ +line-length = 80 + +[lint] +select = [ + # pycodestyle + "E", + # Pyflakes + "F", + # pyupgrade + "UP", + # flake8-bugbear + "B", + # flake8-simplify + "SIM", + # isort + "I", +] diff --git a/scripts/python.sh b/scripts/python.sh new file mode 100644 index 0000000..7dd575c --- /dev/null +++ b/scripts/python.sh @@ -0,0 +1,26 @@ +#!/usr/bin/env bash + +# This script runs the ruff Python formatter on the whole repository. + +# Check version +version_string=$(ruff --version | sed -E 's/^.*(\d+\.\d+-.*).*$/\1/') +expected_version_string='0.6.7' +if [[ "$version_string" =~ "$expected_version_string" ]]; then + echo "ruff version '$version_string' matches '$expected_version_string'" +else + echo "ruff version '$version_string' doesn't match '$expected_version_string'" + exit 1 +fi + +# Get all C++ files checked into the repo, excluding submodules +root_folder=$(git rev-parse --show-toplevel) +all_files=$( \ + git ls-tree --full-tree -r --name-only HEAD . \ + | grep "^.*\(\.py\)$" \ + | sed "s~^~$root_folder/~") +num_files=$(echo $all_files | wc -w) +echo "Formatting ${num_files} files" + +# shellcheck disable=SC2086 +ruff format --config ${root_folder}/ruff.toml ${all_files} +ruff check --config ${root_folder}/ruff.toml ${all_files} diff --git a/setup.py b/setup.py index f11ad28..6c395cf 100644 --- a/setup.py +++ b/setup.py @@ -34,7 +34,11 @@ def build_extension(self, ext: CMakeExtension) -> None: # Using this requires trailing slash for auto-detection & inclusion of # auxiliary "native" libs - debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug + debug = ( + int(os.environ.get("DEBUG", 0)) + if self.debug is None + else self.debug + ) cfg = "Debug" if debug else "Release" # CMake lets you override the generator - we need to check this. @@ -53,7 +57,9 @@ def build_extension(self, ext: CMakeExtension) -> None: # Adding CMake arguments set as environment variable # (needed e.g. to build for ARM OSx on conda-forge) if "CMAKE_ARGS" in os.environ: - cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item] + cmake_args += [ + item for item in os.environ["CMAKE_ARGS"].split(" ") if item + ] # In this example, we pass in the version to C++. You might not need to. # cmake_args += [f"-DEXAMPLE_VERSION_INFO={self.distribution.get_version()}"] @@ -69,7 +75,9 @@ def build_extension(self, ext: CMakeExtension) -> None: else: # Single config generators are handled "normally" - single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) + single_config = any( + x in cmake_generator for x in {"NMake", "Ninja"} + ) # CMake allows an arch-in-generator style for backward compatibility contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) @@ -91,7 +99,9 @@ def build_extension(self, ext: CMakeExtension) -> None: # Cross-compile support for macOS - respect ARCHFLAGS if set archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", "")) if archs: - cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))] + cmake_args += [ + "-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs)) + ] # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level # across all generators. @@ -121,11 +131,11 @@ def build_extension(self, ext: CMakeExtension) -> None: version="0.0.1", author="Yifan Yu", author_email="markyu98@outlook.com", - description='''Solvers and estimators described in the paper "Relative Pose Estimation through Affine Corrections of Monocular Depth Priors".''', + description="""Solvers and estimators described in the paper "Relative Pose Estimation through Affine Corrections of Monocular Depth Priors".""", long_description="", packages=find_packages(), ext_modules=[CMakeExtension("madpose")], cmdclass={"build_ext": CMakeBuild}, zip_safe=False, python_requires=">=3.7", -) \ No newline at end of file +) diff --git a/solver_py/scale_and_shift.py b/solver_py/scale_and_shift.py index e45f3e6..50e9f44 100644 --- a/solver_py/scale_and_shift.py +++ b/solver_py/scale_and_shift.py @@ -1,5 +1,6 @@ -import numpy as np import madpose +import numpy as np + def solve_shift_and_scale(x1, x2, d1, d2): # Estimates scale and shift @@ -12,36 +13,198 @@ def solve_shift_and_scale(x1, x2, d1, d2): # Compute coefficients coeffs = np.zeros((18,)) - coeffs[0] = 2*x2[0].dot(x2[1]) - x2[0].dot(x2[0]) - x2[1].dot(x2[1]) - coeffs[1] = x1[0].dot(x1[0]) + x1[1].dot(x1[1]) - 2*x1[0].dot(x1[1]) - coeffs[2] = 2*(d2[0]+d2[1])*x2[0].dot(x2[1]) - 2*d2[0]*x2[0].dot(x2[0]) - 2*d2[1]*x2[1].dot(x2[1]) - coeffs[3] = 2*d2[0]*d2[1]*x2[0].dot(x2[1]) - d2[0]*d2[0]*x2[0].dot(x2[0]) - d2[1]*d2[1]*x2[1].dot(x2[1]) - coeffs[4] = 2*d1[0]*x1[0].dot(x1[0]) + 2*d1[1]*x1[1].dot(x1[1]) - 2*(d1[0]+d1[1])*x1[0].dot(x1[1]) - coeffs[5] = d1[0]*d1[0]*x1[0].dot(x1[0]) + d1[1]*d1[1]*x1[1].dot(x1[1]) - 2*d1[0]*d1[1]*x1[0].dot(x1[1]) - coeffs[6] = 2*x2[0].dot(x2[2]) - x2[0].dot(x2[0]) - x2[2].dot(x2[2]) - coeffs[7] = x1[0].dot(x1[0]) + x1[2].dot(x1[2]) - 2*x1[0].dot(x1[2]) - coeffs[8] = 2*(d2[0]+d2[2])*x2[0].dot(x2[2]) - 2*d2[0]*x2[0].dot(x2[0]) - 2*d2[2]*x2[2].dot(x2[2]) - coeffs[9] = 2*d2[0]*d2[2]*x2[0].dot(x2[2]) - d2[0]*d2[0]*x2[0].dot(x2[0]) - d2[2]*d2[2]*x2[2].dot(x2[2]) - coeffs[10] = 2*d1[0]*x1[0].dot(x1[0]) + 2*d1[2]*x1[2].dot(x1[2]) - 2*(d1[0]+d1[2])*x1[0].dot(x1[2]) - coeffs[11] = d1[0]*d1[0]*x1[0].dot(x1[0]) + d1[2]*d1[2]*x1[2].dot(x1[2]) - 2*d1[0]*d1[2]*x1[0].dot(x1[2]) - coeffs[12] = 2*x2[1].dot(x2[2]) - x2[1].dot(x2[1]) - x2[2].dot(x2[2]) - coeffs[13] = x1[1].dot(x1[1]) + x1[2].dot(x1[2]) - 2*x1[1].dot(x1[2]) - coeffs[14] = 2*(d2[1]+d2[2])*x2[1].dot(x2[2]) - 2*d2[1]*x2[1].dot(x2[1]) - 2*d2[2]*x2[2].dot(x2[2]) - coeffs[15] = 2*d2[1]*d2[2]*x2[1].dot(x2[2]) - (d2[1]**2)*x2[1].dot(x2[1]) - (d2[2]**2)*x2[2].dot(x2[2]) - coeffs[16] = 2*d1[1]*x1[1].dot(x1[1]) + 2*d1[2]*x1[2].dot(x1[2]) - 2*(d1[1]+d1[2])*x1[1].dot(x1[2]) - coeffs[17] = d1[1]*d1[1]*x1[1].dot(x1[1]) + d1[2]*d1[2]*x1[2].dot(x1[2]) - 2*d1[1]*d1[2]*x1[1].dot(x1[2]) - + coeffs[0] = 2 * x2[0].dot(x2[1]) - x2[0].dot(x2[0]) - x2[1].dot(x2[1]) + coeffs[1] = x1[0].dot(x1[0]) + x1[1].dot(x1[1]) - 2 * x1[0].dot(x1[1]) + coeffs[2] = ( + 2 * (d2[0] + d2[1]) * x2[0].dot(x2[1]) + - 2 * d2[0] * x2[0].dot(x2[0]) + - 2 * d2[1] * x2[1].dot(x2[1]) + ) + coeffs[3] = ( + 2 * d2[0] * d2[1] * x2[0].dot(x2[1]) + - d2[0] * d2[0] * x2[0].dot(x2[0]) + - d2[1] * d2[1] * x2[1].dot(x2[1]) + ) + coeffs[4] = ( + 2 * d1[0] * x1[0].dot(x1[0]) + + 2 * d1[1] * x1[1].dot(x1[1]) + - 2 * (d1[0] + d1[1]) * x1[0].dot(x1[1]) + ) + coeffs[5] = ( + d1[0] * d1[0] * x1[0].dot(x1[0]) + + d1[1] * d1[1] * x1[1].dot(x1[1]) + - 2 * d1[0] * d1[1] * x1[0].dot(x1[1]) + ) + coeffs[6] = 2 * x2[0].dot(x2[2]) - x2[0].dot(x2[0]) - x2[2].dot(x2[2]) + coeffs[7] = x1[0].dot(x1[0]) + x1[2].dot(x1[2]) - 2 * x1[0].dot(x1[2]) + coeffs[8] = ( + 2 * (d2[0] + d2[2]) * x2[0].dot(x2[2]) + - 2 * d2[0] * x2[0].dot(x2[0]) + - 2 * d2[2] * x2[2].dot(x2[2]) + ) + coeffs[9] = ( + 2 * d2[0] * d2[2] * x2[0].dot(x2[2]) + - d2[0] * d2[0] * x2[0].dot(x2[0]) + - d2[2] * d2[2] * x2[2].dot(x2[2]) + ) + coeffs[10] = ( + 2 * d1[0] * x1[0].dot(x1[0]) + + 2 * d1[2] * x1[2].dot(x1[2]) + - 2 * (d1[0] + d1[2]) * x1[0].dot(x1[2]) + ) + coeffs[11] = ( + d1[0] * d1[0] * x1[0].dot(x1[0]) + + d1[2] * d1[2] * x1[2].dot(x1[2]) + - 2 * d1[0] * d1[2] * x1[0].dot(x1[2]) + ) + coeffs[12] = 2 * x2[1].dot(x2[2]) - x2[1].dot(x2[1]) - x2[2].dot(x2[2]) + coeffs[13] = x1[1].dot(x1[1]) + x1[2].dot(x1[2]) - 2 * x1[1].dot(x1[2]) + coeffs[14] = ( + 2 * (d2[1] + d2[2]) * x2[1].dot(x2[2]) + - 2 * d2[1] * x2[1].dot(x2[1]) + - 2 * d2[2] * x2[2].dot(x2[2]) + ) + coeffs[15] = ( + 2 * d2[1] * d2[2] * x2[1].dot(x2[2]) + - (d2[1] ** 2) * x2[1].dot(x2[1]) + - (d2[2] ** 2) * x2[2].dot(x2[2]) + ) + coeffs[16] = ( + 2 * d1[1] * x1[1].dot(x1[1]) + + 2 * d1[2] * x1[2].dot(x1[2]) + - 2 * (d1[1] + d1[2]) * x1[1].dot(x1[2]) + ) + coeffs[17] = ( + d1[1] * d1[1] * x1[1].dot(x1[1]) + + d1[2] * d1[2] * x1[2].dot(x1[2]) + - 2 * d1[1] * d1[2] * x1[1].dot(x1[2]) + ) + # Setup expanded equation system - coeff_ind0 = [0, 6, 12, 1, 7, 13, 2, 8, 0, 6, 12, 14, 6, 0, 12, 1, 7, 13, 3, 9, 2, 8, 14, 15, 4, 10, 7, - 1, 16, 13, 8, 2, 6, 12, 0, 14, 9, 3, 8, 14, 2, 15, 3, 9, 15, 4, 10, 16, 7, 13, 1, 5, 11, 10, 4, 17, 16] + coeff_ind0 = [ + 0, + 6, + 12, + 1, + 7, + 13, + 2, + 8, + 0, + 6, + 12, + 14, + 6, + 0, + 12, + 1, + 7, + 13, + 3, + 9, + 2, + 8, + 14, + 15, + 4, + 10, + 7, + 1, + 16, + 13, + 8, + 2, + 6, + 12, + 0, + 14, + 9, + 3, + 8, + 14, + 2, + 15, + 3, + 9, + 15, + 4, + 10, + 16, + 7, + 13, + 1, + 5, + 11, + 10, + 4, + 17, + 16, + ] coeff_ind1 = [11, 17, 5, 9, 15, 3, 5, 11, 17, 10, 16, 4, 11, 5, 17] - ind0 = [0, 1, 9, 12, 13, 21, 24, 25, 26, 28, 29, 33, 39, 42, 47, 50, 52, 53, 60, 61, 62, 64, 65, 69, 72, 73, 75, 78, 81, 83, 87, - 90, 91, 92, 94, 95, 99, 102, 103, 104, 106, 107, 110, 112, 113, 122, 124, 125, 127, 128, 130, 132, 133, 135, 138, 141, 143] + ind0 = [ + 0, + 1, + 9, + 12, + 13, + 21, + 24, + 25, + 26, + 28, + 29, + 33, + 39, + 42, + 47, + 50, + 52, + 53, + 60, + 61, + 62, + 64, + 65, + 69, + 72, + 73, + 75, + 78, + 81, + 83, + 87, + 90, + 91, + 92, + 94, + 95, + 99, + 102, + 103, + 104, + 106, + 107, + 110, + 112, + 113, + 122, + 124, + 125, + 127, + 128, + 130, + 132, + 133, + 135, + 138, + 141, + 143, + ] ind1 = [7, 8, 10, 19, 20, 22, 26, 28, 29, 31, 32, 34, 39, 42, 47] C0 = np.zeros((12, 12)) C1 = np.zeros((12, 4)) - C0[np.unravel_index(ind0, (12, 12), 'F')] = coeffs[coeff_ind0] - C1[np.unravel_index(ind1, (12, 4), 'F')] = coeffs[coeff_ind1] + C0[np.unravel_index(ind0, (12, 12), "F")] = coeffs[coeff_ind0] + C1[np.unravel_index(ind1, (12, 4), "F")] = coeffs[coeff_ind1] # Linear elimination C2 = np.linalg.solve(C0, C1) @@ -70,13 +233,12 @@ def find_transform(X1, X2): X1m = X1 - m1 X2m = X2 - m2 u, s, vt = np.linalg.svd(X2m.T @ X1m) - R = u @ np.diag([1.0, 1.0, np.linalg.det(u@vt)]) @ vt + R = u @ np.diag([1.0, 1.0, np.linalg.det(u @ vt)]) @ vt t = m2 - R @ m1 return R, t def test_solver(): - # Setup instance (with positive depths) while True: x1 = np.c_[np.random.randn(3, 2), np.ones((3,))] @@ -118,11 +280,19 @@ def test_solver(): print(R_est @ X1.T + t_est[:, None] - X2.T) err_R = np.linalg.norm(R - R_est) - err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) - print(f'posescaleoffsets, residual={err_a}, rotation={err_R}, translation={err_t}') + err_t = np.linalg.norm( + t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) + ) + print( + f"posescaleoffsets, residual={err_a}, rotation={err_R}, translation={err_t}" + ) for k, (a1, b1, a2, b2) in enumerate(sols + sols_mono): - err = np.abs(a2 - a2_gt / a1_gt) + np.abs(b1 - b1_gt / a1_gt) + np.abs(b2 - b2_gt / a1_gt) + err = ( + np.abs(a2 - a2_gt / a1_gt) + + np.abs(b1 - b1_gt / a1_gt) + + np.abs(b2 - b2_gt / a1_gt) + ) d1_corr = a1 * d1 + b1 d2_corr = a2 * d2 + b2 @@ -132,10 +302,14 @@ def test_solver(): R_est, t_est = find_transform(X1, X2) - err_R = np.linalg.norm(R-R_est) - err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) - print(f'solution={k}, residual={err}, rotation={err_R}, translation={err_t}') - + err_R = np.linalg.norm(R - R_est) + err_t = np.linalg.norm( + t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) + ) + print( + f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}" + ) + -if __name__ == '__main__': - test_solver() \ No newline at end of file +if __name__ == "__main__": + test_solver() diff --git a/solver_py/shift_and_scale_shared_focal.py b/solver_py/shift_and_scale_shared_focal.py index ce8c184..ae200a0 100644 --- a/solver_py/shift_and_scale_shared_focal.py +++ b/solver_py/shift_and_scale_shared_focal.py @@ -1,5 +1,6 @@ -import numpy as np import madpose +import numpy as np + def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): # Estimates scale, shift and a shared focal length @@ -24,65 +25,836 @@ def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): # Compute coefficients coeffs = np.zeros((32,)) - coeffs[0] = 2*x2[0,0]*x2[1,0] + 2*x2[0,1]*x2[1,1] - (x2[0,0]**2) - (x2[0,1]**2) - (x2[1,0]**2) - (x2[1,1]**2) - coeffs[1] = (x1[0,0]**2) - 2*x1[0,1]*x1[1,1] - 2*x1[0,0]*x1[1,0] + (x1[0,1]**2) + (x1[1,0]**2) + (x1[1,1]**2) - coeffs[2] = 2*d2[0]*x2[0,0]*x2[1,0] - 2*d2[0]*(x2[0,1]**2) - 2*d2[1]*(x2[1,0]**2) - 2*d2[1]*(x2[1,1]**2) - 2*d2[0]*(x2[0,0]**2) + 2*d2[1]*x2[0,0]*x2[1,0] + 2*d2[0]*x2[0,1]*x2[1,1] + 2*d2[1]*x2[0,1]*x2[1,1] - coeffs[3] = 2*d2[0]*d2[1]*x2[0,0]*x2[1,0] - (d2[0]**2)*(x2[0,1]**2) - (d2[1]**2)*(x2[1,0]**2) - (d2[1]**2)*(x2[1,1]**2) - (d2[0]**2)*(x2[0,0]**2) + 2*d2[0]*d2[1]*x2[0,1]*x2[1,1] - coeffs[4] = 2*d1[0]*(x1[0,0]**2) + 2*d1[0]*(x1[0,1]**2) + 2*d1[1]*(x1[1,0]**2) + 2*d1[1]*(x1[1,1]**2) - 2*d1[0]*x1[0,0]*x1[1,0] - 2*d1[1]*x1[0,0]*x1[1,0] - 2*d1[0]*x1[0,1]*x1[1,1] - 2*d1[1]*x1[0,1]*x1[1,1] - coeffs[5] = 2*d2[0]*d2[1] - (d2[0]**2) - (d2[1]**2) - coeffs[6] = (d1[0]**2)*(x1[0,0]**2) + (d1[0]**2)*(x1[0,1]**2) + (d1[1]**2)*(x1[1,0]**2) + (d1[1]**2)*(x1[1,1]**2) - 2*d1[0]*d1[1]*x1[0,0]*x1[1,0] - 2*d1[0]*d1[1]*x1[0,1]*x1[1,1] - coeffs[7] = (d1[0]**2) - 2*d1[0]*d1[1] + (d1[1]**2) - coeffs[8] = 2*x2[0,0]*x2[2,0] + 2*x2[0,1]*x2[2,1] - (x2[0,0]**2) - (x2[0,1]**2) - (x2[2,0]**2) - (x2[2,1]**2) - coeffs[9] = (x1[0,0]**2) - 2*x1[0,1]*x1[2,1] - 2*x1[0,0]*x1[2,0] + (x1[0,1]**2) + (x1[2,0]**2) + (x1[2,1]**2) - coeffs[10] = 2*d2[0]*x2[0,0]*x2[2,0] - 2*d2[0]*(x2[0,1]**2) - 2*d2[2]*(x2[2,0]**2) - 2*d2[2]*(x2[2,1]**2) - 2*d2[0]*(x2[0,0]**2) + 2*d2[0]*x2[0,1]*x2[2,1] + 2*d2[2]*x2[0,0]*x2[2,0] + 2*d2[2]*x2[0,1]*x2[2,1] - coeffs[11] = 2*d2[0]*d2[2]*x2[0,0]*x2[2,0] - (d2[0]**2)*(x2[0,1]**2) - (d2[2]**2)*(x2[2,0]**2) - (d2[2]**2)*(x2[2,1]**2) - (d2[0]**2)*(x2[0,0]**2) + 2*d2[0]*d2[2]*x2[0,1]*x2[2,1] - coeffs[12] = 2*d1[0]*(x1[0,0]**2) + 2*d1[0]*(x1[0,1]**2) + 2*d1[2]*(x1[2,0]**2) + 2*d1[2]*(x1[2,1]**2) - 2*d1[0]*x1[0,0]*x1[2,0] - 2*d1[0]*x1[0,1]*x1[2,1] - 2*d1[2]*x1[0,0]*x1[2,0] - 2*d1[2]*x1[0,1]*x1[2,1] - coeffs[13] = 2*d2[0]*d2[2] - (d2[0]**2) - (d2[2]**2) - coeffs[14] = (d1[0]**2)*(x1[0,0]**2) + (d1[0]**2)*(x1[0,1]**2) + (d1[2]**2)*(x1[2,0]**2) + (d1[2]**2)*(x1[2,1]**2) - 2*d1[0]*d1[2]*x1[0,0]*x1[2,0] - 2*d1[0]*d1[2]*x1[0,1]*x1[2,1] - coeffs[15] = (d1[0]**2) - 2*d1[0]*d1[2] + (d1[2]**2) - coeffs[16] = 2*x2[1,0]*x2[2,0] + 2*x2[1,1]*x2[2,1] - (x2[1,0]**2) - (x2[1,1]**2) - (x2[2,0]**2) - (x2[2,1]**2) - coeffs[17] = (x1[1,0]**2) - 2*x1[1,1]*x1[2,1] - 2*x1[1,0]*x1[2,0] + (x1[1,1]**2) + (x1[2,0]**2) + (x1[2,1]**2) - coeffs[18] = 2*d2[1]*x2[1,0]*x2[2,0] - 2*d2[1]*(x2[1,1]**2) - 2*d2[2]*(x2[2,0]**2) - 2*d2[2]*(x2[2,1]**2) - 2*d2[1]*(x2[1,0]**2) + 2*d2[2]*x2[1,0]*x2[2,0] + 2*d2[1]*x2[1,1]*x2[2,1] + 2*d2[2]*x2[1,1]*x2[2,1] - coeffs[19] = 2*d2[1]*d2[2]*x2[1,0]*x2[2,0] - (d2[1]**2)*(x2[1,1]**2) - (d2[2]**2)*(x2[2,0]**2) - (d2[2]**2)*(x2[2,1]**2) - (d2[1]**2)*(x2[1,0]**2) + 2*d2[1]*d2[2]*x2[1,1]*x2[2,1] - coeffs[20] = 2*d1[1]*(x1[1,0]**2) + 2*d1[1]*(x1[1,1]**2) + 2*d1[2]*(x1[2,0]**2) + 2*d1[2]*(x1[2,1]**2) - 2*d1[1]*x1[1,0]*x1[2,0] - 2*d1[2]*x1[1,0]*x1[2,0] - 2*d1[1]*x1[1,1]*x1[2,1] - 2*d1[2]*x1[1,1]*x1[2,1] - coeffs[21] = 2*d2[1]*d2[2] - (d2[1]**2) - (d2[2]**2) - coeffs[22] = (d1[1]**2)*(x1[1,0]**2) + (d1[1]**2)*(x1[1,1]**2) + (d1[2]**2)*(x1[2,0]**2) + (d1[2]**2)*(x1[2,1]**2) - 2*d1[1]*d1[2]*x1[1,0]*x1[2,0] - 2*d1[1]*d1[2]*x1[1,1]*x1[2,1] - coeffs[23] = (d1[1]**2) - 2*d1[1]*d1[2] + (d1[2]**2) - coeffs[24] = 2*x2[0,0]*x2[3,0] + 2*x2[0,1]*x2[3,1] - (x2[0,0]**2) - (x2[0,1]**2) - (x2[3,0]**2) - (x2[3,1]**2) - coeffs[25] = (x1[0,0]**2) - 2*x1[0,1]*x1[3,1] - 2*x1[0,0]*x1[3,0] + (x1[0,1]**2) + (x1[3,0]**2) + (x1[3,1]**2) - coeffs[26] = 2*d2[0]*x2[0,0]*x2[3,0] - 2*d2[0]*(x2[0,1]**2) - 2*d2[3]*(x2[3,0]**2) - 2*d2[3]*(x2[3,1]**2) - 2*d2[0]*(x2[0,0]**2) + 2*d2[0]*x2[0,1]*x2[3,1] + 2*d2[3]*x2[0,0]*x2[3,0] + 2*d2[3]*x2[0,1]*x2[3,1] - coeffs[27] = 2*d2[0]*d2[3]*x2[0,0]*x2[3,0] - (d2[0]**2)*(x2[0,1]**2) - (d2[3]**2)*(x2[3,0]**2) - (d2[3]**2)*(x2[3,1]**2) - (d2[0]**2)*(x2[0,0]**2) + 2*d2[0]*d2[3]*x2[0,1]*x2[3,1] - coeffs[28] = 2*d1[0]*(x1[0,0]**2) + 2*d1[0]*(x1[0,1]**2) + 2*d1[3]*(x1[3,0]**2) + 2*d1[3]*(x1[3,1]**2) - 2*d1[0]*x1[0,0]*x1[3,0] - 2*d1[0]*x1[0,1]*x1[3,1] - 2*d1[3]*x1[0,0]*x1[3,0] - 2*d1[3]*x1[0,1]*x1[3,1] - coeffs[29] = 2*d2[0]*d2[3] - (d2[0]**2) - (d2[3]**2) - coeffs[30] = (d1[0]**2)*(x1[0,0]**2) + (d1[0]**2)*(x1[0,1]**2) + (d1[3]**2)*(x1[3,0]**2) + (d1[3]**2)*(x1[3,1]**2) - 2*d1[0]*d1[3]*x1[0,0]*x1[3,0] - 2*d1[0]*d1[3]*x1[0,1]*x1[3,1] - coeffs[31] = (d1[0]**2) - 2*d1[0]*d1[3] + (d1[3]**2) + coeffs[0] = ( + 2 * x2[0, 0] * x2[1, 0] + + 2 * x2[0, 1] * x2[1, 1] + - (x2[0, 0] ** 2) + - (x2[0, 1] ** 2) + - (x2[1, 0] ** 2) + - (x2[1, 1] ** 2) + ) + coeffs[1] = ( + (x1[0, 0] ** 2) + - 2 * x1[0, 1] * x1[1, 1] + - 2 * x1[0, 0] * x1[1, 0] + + (x1[0, 1] ** 2) + + (x1[1, 0] ** 2) + + (x1[1, 1] ** 2) + ) + coeffs[2] = ( + 2 * d2[0] * x2[0, 0] * x2[1, 0] + - 2 * d2[0] * (x2[0, 1] ** 2) + - 2 * d2[1] * (x2[1, 0] ** 2) + - 2 * d2[1] * (x2[1, 1] ** 2) + - 2 * d2[0] * (x2[0, 0] ** 2) + + 2 * d2[1] * x2[0, 0] * x2[1, 0] + + 2 * d2[0] * x2[0, 1] * x2[1, 1] + + 2 * d2[1] * x2[0, 1] * x2[1, 1] + ) + coeffs[3] = ( + 2 * d2[0] * d2[1] * x2[0, 0] * x2[1, 0] + - (d2[0] ** 2) * (x2[0, 1] ** 2) + - (d2[1] ** 2) * (x2[1, 0] ** 2) + - (d2[1] ** 2) * (x2[1, 1] ** 2) + - (d2[0] ** 2) * (x2[0, 0] ** 2) + + 2 * d2[0] * d2[1] * x2[0, 1] * x2[1, 1] + ) + coeffs[4] = ( + 2 * d1[0] * (x1[0, 0] ** 2) + + 2 * d1[0] * (x1[0, 1] ** 2) + + 2 * d1[1] * (x1[1, 0] ** 2) + + 2 * d1[1] * (x1[1, 1] ** 2) + - 2 * d1[0] * x1[0, 0] * x1[1, 0] + - 2 * d1[1] * x1[0, 0] * x1[1, 0] + - 2 * d1[0] * x1[0, 1] * x1[1, 1] + - 2 * d1[1] * x1[0, 1] * x1[1, 1] + ) + coeffs[5] = 2 * d2[0] * d2[1] - (d2[0] ** 2) - (d2[1] ** 2) + coeffs[6] = ( + (d1[0] ** 2) * (x1[0, 0] ** 2) + + (d1[0] ** 2) * (x1[0, 1] ** 2) + + (d1[1] ** 2) * (x1[1, 0] ** 2) + + (d1[1] ** 2) * (x1[1, 1] ** 2) + - 2 * d1[0] * d1[1] * x1[0, 0] * x1[1, 0] + - 2 * d1[0] * d1[1] * x1[0, 1] * x1[1, 1] + ) + coeffs[7] = (d1[0] ** 2) - 2 * d1[0] * d1[1] + (d1[1] ** 2) + coeffs[8] = ( + 2 * x2[0, 0] * x2[2, 0] + + 2 * x2[0, 1] * x2[2, 1] + - (x2[0, 0] ** 2) + - (x2[0, 1] ** 2) + - (x2[2, 0] ** 2) + - (x2[2, 1] ** 2) + ) + coeffs[9] = ( + (x1[0, 0] ** 2) + - 2 * x1[0, 1] * x1[2, 1] + - 2 * x1[0, 0] * x1[2, 0] + + (x1[0, 1] ** 2) + + (x1[2, 0] ** 2) + + (x1[2, 1] ** 2) + ) + coeffs[10] = ( + 2 * d2[0] * x2[0, 0] * x2[2, 0] + - 2 * d2[0] * (x2[0, 1] ** 2) + - 2 * d2[2] * (x2[2, 0] ** 2) + - 2 * d2[2] * (x2[2, 1] ** 2) + - 2 * d2[0] * (x2[0, 0] ** 2) + + 2 * d2[0] * x2[0, 1] * x2[2, 1] + + 2 * d2[2] * x2[0, 0] * x2[2, 0] + + 2 * d2[2] * x2[0, 1] * x2[2, 1] + ) + coeffs[11] = ( + 2 * d2[0] * d2[2] * x2[0, 0] * x2[2, 0] + - (d2[0] ** 2) * (x2[0, 1] ** 2) + - (d2[2] ** 2) * (x2[2, 0] ** 2) + - (d2[2] ** 2) * (x2[2, 1] ** 2) + - (d2[0] ** 2) * (x2[0, 0] ** 2) + + 2 * d2[0] * d2[2] * x2[0, 1] * x2[2, 1] + ) + coeffs[12] = ( + 2 * d1[0] * (x1[0, 0] ** 2) + + 2 * d1[0] * (x1[0, 1] ** 2) + + 2 * d1[2] * (x1[2, 0] ** 2) + + 2 * d1[2] * (x1[2, 1] ** 2) + - 2 * d1[0] * x1[0, 0] * x1[2, 0] + - 2 * d1[0] * x1[0, 1] * x1[2, 1] + - 2 * d1[2] * x1[0, 0] * x1[2, 0] + - 2 * d1[2] * x1[0, 1] * x1[2, 1] + ) + coeffs[13] = 2 * d2[0] * d2[2] - (d2[0] ** 2) - (d2[2] ** 2) + coeffs[14] = ( + (d1[0] ** 2) * (x1[0, 0] ** 2) + + (d1[0] ** 2) * (x1[0, 1] ** 2) + + (d1[2] ** 2) * (x1[2, 0] ** 2) + + (d1[2] ** 2) * (x1[2, 1] ** 2) + - 2 * d1[0] * d1[2] * x1[0, 0] * x1[2, 0] + - 2 * d1[0] * d1[2] * x1[0, 1] * x1[2, 1] + ) + coeffs[15] = (d1[0] ** 2) - 2 * d1[0] * d1[2] + (d1[2] ** 2) + coeffs[16] = ( + 2 * x2[1, 0] * x2[2, 0] + + 2 * x2[1, 1] * x2[2, 1] + - (x2[1, 0] ** 2) + - (x2[1, 1] ** 2) + - (x2[2, 0] ** 2) + - (x2[2, 1] ** 2) + ) + coeffs[17] = ( + (x1[1, 0] ** 2) + - 2 * x1[1, 1] * x1[2, 1] + - 2 * x1[1, 0] * x1[2, 0] + + (x1[1, 1] ** 2) + + (x1[2, 0] ** 2) + + (x1[2, 1] ** 2) + ) + coeffs[18] = ( + 2 * d2[1] * x2[1, 0] * x2[2, 0] + - 2 * d2[1] * (x2[1, 1] ** 2) + - 2 * d2[2] * (x2[2, 0] ** 2) + - 2 * d2[2] * (x2[2, 1] ** 2) + - 2 * d2[1] * (x2[1, 0] ** 2) + + 2 * d2[2] * x2[1, 0] * x2[2, 0] + + 2 * d2[1] * x2[1, 1] * x2[2, 1] + + 2 * d2[2] * x2[1, 1] * x2[2, 1] + ) + coeffs[19] = ( + 2 * d2[1] * d2[2] * x2[1, 0] * x2[2, 0] + - (d2[1] ** 2) * (x2[1, 1] ** 2) + - (d2[2] ** 2) * (x2[2, 0] ** 2) + - (d2[2] ** 2) * (x2[2, 1] ** 2) + - (d2[1] ** 2) * (x2[1, 0] ** 2) + + 2 * d2[1] * d2[2] * x2[1, 1] * x2[2, 1] + ) + coeffs[20] = ( + 2 * d1[1] * (x1[1, 0] ** 2) + + 2 * d1[1] * (x1[1, 1] ** 2) + + 2 * d1[2] * (x1[2, 0] ** 2) + + 2 * d1[2] * (x1[2, 1] ** 2) + - 2 * d1[1] * x1[1, 0] * x1[2, 0] + - 2 * d1[2] * x1[1, 0] * x1[2, 0] + - 2 * d1[1] * x1[1, 1] * x1[2, 1] + - 2 * d1[2] * x1[1, 1] * x1[2, 1] + ) + coeffs[21] = 2 * d2[1] * d2[2] - (d2[1] ** 2) - (d2[2] ** 2) + coeffs[22] = ( + (d1[1] ** 2) * (x1[1, 0] ** 2) + + (d1[1] ** 2) * (x1[1, 1] ** 2) + + (d1[2] ** 2) * (x1[2, 0] ** 2) + + (d1[2] ** 2) * (x1[2, 1] ** 2) + - 2 * d1[1] * d1[2] * x1[1, 0] * x1[2, 0] + - 2 * d1[1] * d1[2] * x1[1, 1] * x1[2, 1] + ) + coeffs[23] = (d1[1] ** 2) - 2 * d1[1] * d1[2] + (d1[2] ** 2) + coeffs[24] = ( + 2 * x2[0, 0] * x2[3, 0] + + 2 * x2[0, 1] * x2[3, 1] + - (x2[0, 0] ** 2) + - (x2[0, 1] ** 2) + - (x2[3, 0] ** 2) + - (x2[3, 1] ** 2) + ) + coeffs[25] = ( + (x1[0, 0] ** 2) + - 2 * x1[0, 1] * x1[3, 1] + - 2 * x1[0, 0] * x1[3, 0] + + (x1[0, 1] ** 2) + + (x1[3, 0] ** 2) + + (x1[3, 1] ** 2) + ) + coeffs[26] = ( + 2 * d2[0] * x2[0, 0] * x2[3, 0] + - 2 * d2[0] * (x2[0, 1] ** 2) + - 2 * d2[3] * (x2[3, 0] ** 2) + - 2 * d2[3] * (x2[3, 1] ** 2) + - 2 * d2[0] * (x2[0, 0] ** 2) + + 2 * d2[0] * x2[0, 1] * x2[3, 1] + + 2 * d2[3] * x2[0, 0] * x2[3, 0] + + 2 * d2[3] * x2[0, 1] * x2[3, 1] + ) + coeffs[27] = ( + 2 * d2[0] * d2[3] * x2[0, 0] * x2[3, 0] + - (d2[0] ** 2) * (x2[0, 1] ** 2) + - (d2[3] ** 2) * (x2[3, 0] ** 2) + - (d2[3] ** 2) * (x2[3, 1] ** 2) + - (d2[0] ** 2) * (x2[0, 0] ** 2) + + 2 * d2[0] * d2[3] * x2[0, 1] * x2[3, 1] + ) + coeffs[28] = ( + 2 * d1[0] * (x1[0, 0] ** 2) + + 2 * d1[0] * (x1[0, 1] ** 2) + + 2 * d1[3] * (x1[3, 0] ** 2) + + 2 * d1[3] * (x1[3, 1] ** 2) + - 2 * d1[0] * x1[0, 0] * x1[3, 0] + - 2 * d1[0] * x1[0, 1] * x1[3, 1] + - 2 * d1[3] * x1[0, 0] * x1[3, 0] + - 2 * d1[3] * x1[0, 1] * x1[3, 1] + ) + coeffs[29] = 2 * d2[0] * d2[3] - (d2[0] ** 2) - (d2[3] ** 2) + coeffs[30] = ( + (d1[0] ** 2) * (x1[0, 0] ** 2) + + (d1[0] ** 2) * (x1[0, 1] ** 2) + + (d1[3] ** 2) * (x1[3, 0] ** 2) + + (d1[3] ** 2) * (x1[3, 1] ** 2) + - 2 * d1[0] * d1[3] * x1[0, 0] * x1[3, 0] + - 2 * d1[0] * d1[3] * x1[0, 1] * x1[3, 1] + ) + coeffs[31] = (d1[0] ** 2) - 2 * d1[0] * d1[3] + (d1[3] ** 2) # Setup expanded equation system - coeff_ind0 = [0, 8, 16, 24, 1, 9, 17, 25, 2, 10, 0, 8, 16, 18, 24, 26, 0, 8, 16, 24, 0, 8, 16, 24, 0, 8, 16, 24, 1, 9, 17, 25, 1, 9, 17, 25, 3, 11, 2, 10, 18, 19, 26, 27, 4, 12, 2, 1, 10, 9, 20, 18, 17, 28, 26, 25, 1, 9, 17, 25, 2, 10, 8, 0, 16, 18, 24, 26, 2, 10, 0, 16, 18, 8, 24, 26, 8, 0, 16, 24, 5, 13, 21, 29, 3, 11, 19, 27, 4, 3, 12, 11, 20, 9, 28, 1, 19, 17, 27, 25, 4, 12, 1, 17, 20, 9, 25, 28, 3, 11, 10, 2, 18, 19, 26, 27, 6, 14, 4, 3, 12, 11, - 2, 22, 18, 19, 20, 10, 26, 30, 28, 27, 4, 12, 9, 20, 1, 17, 25, 28, 10, 2, 18, 0, 16, 24, 26, 8, 5, 13, 21, 29, 11, 3, 19, 27, 6, 14, 22, 12, 3, 30, 4, 19, 20, 11, 27, 28, 6, 14, 4, 20, 22, 12, 1, 17, 25, 28, 30, 9, 6, 14, 11, 3, 19, 22, 2, 18, 26, 27, 30, 10, 6, 14, 12, 22, 4, 20, 28, 30, 14, 6, 22, 3, 19, 27, 30, 11, 14, 6, 22, 30, 5, 13, 21, 29, 6, 22, 14, 4, 20, 28, 30, 12, 7, 15, 23, 31, 5, 13, 21, 29, 7, 15, 23, 31, 7, 15, 5, 13, 23, 21, 31, 29] - coeff_ind1 = [7, 23, 31, 15, 6, 22, 30, 14, 7, 23, 15, 31, 7, 15, 23, 5, 31, 21, 13, 29, - 15, 7, 23, 31, 7, 15, 13, 5, 21, 23, 29, 31, 15, 7, 23, 5, 21, 29, 31, 13, 13, 5, 21, 29] - ind0 = [0, 1, 14, 30, 36, 37, 50, 66, 72, 73, 74, 78, 80, 86, 87, 102, 111, 115, 126, 139, 148, 153, 167, 177, 185, 190, 199, 215, 218, 222, 224, 231, 255, 259, 270, 283, 288, 289, 290, 294, 296, 302, 303, 318, 324, 325, 327, 328, 331, 333, 338, 342, 347, 354, 355, 357, 365, 370, 379, 395, 400, 405, 407, 412, 418, 419, 428, 429, 437, 442, 444, 449, 451, 456, 461, 467, 481, 488, 489, 496, 504, 505, 518, 534, 542, 546, 548, 555, 578, 579, 582, 583, 584, 587, 591, 592, 594, 598, 607, 608, 615, 619, 624, 629, 630, 636, 641, 643, 652, 657, 659, 664, 670, 671, 680, 681, 684, 685, 688, 689, 693, 694, 696, 698, 701, 703, 707, 708, - 713, 714, 717, 719, 725, 730, 733, 739, 740, 741, 748, 755, 769, 776, 777, 781, 782, 783, 784, 790, 796, 801, 815, 825, 839, 844, 850, 860, 866, 870, 872, 875, 876, 879, 880, 881, 886, 888, 893, 896, 903, 907, 912, 917, 918, 924, 925, 926, 927, 929, 931, 934, 940, 945, 949, 956, 957, 959, 961, 962, 963, 964, 969, 970, 977, 982, 985, 991, 992, 993, 1000, 1007, 1019, 1024, 1030, 1033, 1034, 1035, 1040, 1042, 1057, 1064, 1065, 1072, 1082, 1086, 1088, 1095, 1128, 1133, 1140, 1141, 1142, 1143, 1145, 1150, 1155, 1159, 1170, 1183, 1191, 1195, 1206, 1219, 1229, 1234, 1243, 1259, 1260, 1261, 1265, 1270, 1274, 1279, 1290, 1295] - ind1 = [25, 26, 27, 34, 61, 62, 63, 70, 84, 89, 96, 101, 110, 114, 116, 120, 123, 125, 132, 137, 157, 164, - 165, 172, 184, 189, 193, 200, 201, 203, 208, 213, 227, 232, 238, 241, 242, 243, 248, 250, 263, 268, 274, 284] + coeff_ind0 = [ + 0, + 8, + 16, + 24, + 1, + 9, + 17, + 25, + 2, + 10, + 0, + 8, + 16, + 18, + 24, + 26, + 0, + 8, + 16, + 24, + 0, + 8, + 16, + 24, + 0, + 8, + 16, + 24, + 1, + 9, + 17, + 25, + 1, + 9, + 17, + 25, + 3, + 11, + 2, + 10, + 18, + 19, + 26, + 27, + 4, + 12, + 2, + 1, + 10, + 9, + 20, + 18, + 17, + 28, + 26, + 25, + 1, + 9, + 17, + 25, + 2, + 10, + 8, + 0, + 16, + 18, + 24, + 26, + 2, + 10, + 0, + 16, + 18, + 8, + 24, + 26, + 8, + 0, + 16, + 24, + 5, + 13, + 21, + 29, + 3, + 11, + 19, + 27, + 4, + 3, + 12, + 11, + 20, + 9, + 28, + 1, + 19, + 17, + 27, + 25, + 4, + 12, + 1, + 17, + 20, + 9, + 25, + 28, + 3, + 11, + 10, + 2, + 18, + 19, + 26, + 27, + 6, + 14, + 4, + 3, + 12, + 11, + 2, + 22, + 18, + 19, + 20, + 10, + 26, + 30, + 28, + 27, + 4, + 12, + 9, + 20, + 1, + 17, + 25, + 28, + 10, + 2, + 18, + 0, + 16, + 24, + 26, + 8, + 5, + 13, + 21, + 29, + 11, + 3, + 19, + 27, + 6, + 14, + 22, + 12, + 3, + 30, + 4, + 19, + 20, + 11, + 27, + 28, + 6, + 14, + 4, + 20, + 22, + 12, + 1, + 17, + 25, + 28, + 30, + 9, + 6, + 14, + 11, + 3, + 19, + 22, + 2, + 18, + 26, + 27, + 30, + 10, + 6, + 14, + 12, + 22, + 4, + 20, + 28, + 30, + 14, + 6, + 22, + 3, + 19, + 27, + 30, + 11, + 14, + 6, + 22, + 30, + 5, + 13, + 21, + 29, + 6, + 22, + 14, + 4, + 20, + 28, + 30, + 12, + 7, + 15, + 23, + 31, + 5, + 13, + 21, + 29, + 7, + 15, + 23, + 31, + 7, + 15, + 5, + 13, + 23, + 21, + 31, + 29, + ] + coeff_ind1 = [ + 7, + 23, + 31, + 15, + 6, + 22, + 30, + 14, + 7, + 23, + 15, + 31, + 7, + 15, + 23, + 5, + 31, + 21, + 13, + 29, + 15, + 7, + 23, + 31, + 7, + 15, + 13, + 5, + 21, + 23, + 29, + 31, + 15, + 7, + 23, + 5, + 21, + 29, + 31, + 13, + 13, + 5, + 21, + 29, + ] + ind0 = [ + 0, + 1, + 14, + 30, + 36, + 37, + 50, + 66, + 72, + 73, + 74, + 78, + 80, + 86, + 87, + 102, + 111, + 115, + 126, + 139, + 148, + 153, + 167, + 177, + 185, + 190, + 199, + 215, + 218, + 222, + 224, + 231, + 255, + 259, + 270, + 283, + 288, + 289, + 290, + 294, + 296, + 302, + 303, + 318, + 324, + 325, + 327, + 328, + 331, + 333, + 338, + 342, + 347, + 354, + 355, + 357, + 365, + 370, + 379, + 395, + 400, + 405, + 407, + 412, + 418, + 419, + 428, + 429, + 437, + 442, + 444, + 449, + 451, + 456, + 461, + 467, + 481, + 488, + 489, + 496, + 504, + 505, + 518, + 534, + 542, + 546, + 548, + 555, + 578, + 579, + 582, + 583, + 584, + 587, + 591, + 592, + 594, + 598, + 607, + 608, + 615, + 619, + 624, + 629, + 630, + 636, + 641, + 643, + 652, + 657, + 659, + 664, + 670, + 671, + 680, + 681, + 684, + 685, + 688, + 689, + 693, + 694, + 696, + 698, + 701, + 703, + 707, + 708, + 713, + 714, + 717, + 719, + 725, + 730, + 733, + 739, + 740, + 741, + 748, + 755, + 769, + 776, + 777, + 781, + 782, + 783, + 784, + 790, + 796, + 801, + 815, + 825, + 839, + 844, + 850, + 860, + 866, + 870, + 872, + 875, + 876, + 879, + 880, + 881, + 886, + 888, + 893, + 896, + 903, + 907, + 912, + 917, + 918, + 924, + 925, + 926, + 927, + 929, + 931, + 934, + 940, + 945, + 949, + 956, + 957, + 959, + 961, + 962, + 963, + 964, + 969, + 970, + 977, + 982, + 985, + 991, + 992, + 993, + 1000, + 1007, + 1019, + 1024, + 1030, + 1033, + 1034, + 1035, + 1040, + 1042, + 1057, + 1064, + 1065, + 1072, + 1082, + 1086, + 1088, + 1095, + 1128, + 1133, + 1140, + 1141, + 1142, + 1143, + 1145, + 1150, + 1155, + 1159, + 1170, + 1183, + 1191, + 1195, + 1206, + 1219, + 1229, + 1234, + 1243, + 1259, + 1260, + 1261, + 1265, + 1270, + 1274, + 1279, + 1290, + 1295, + ] + ind1 = [ + 25, + 26, + 27, + 34, + 61, + 62, + 63, + 70, + 84, + 89, + 96, + 101, + 110, + 114, + 116, + 120, + 123, + 125, + 132, + 137, + 157, + 164, + 165, + 172, + 184, + 189, + 193, + 200, + 201, + 203, + 208, + 213, + 227, + 232, + 238, + 241, + 242, + 243, + 248, + 250, + 263, + 268, + 274, + 284, + ] C0 = np.zeros((36, 36)) C1 = np.zeros((36, 8)) - C0[np.unravel_index(ind0, (36, 36), 'F')] = coeffs[coeff_ind0] - C1[np.unravel_index(ind1, (36, 8), 'F')] = coeffs[coeff_ind1] + C0[np.unravel_index(ind0, (36, 36), "F")] = coeffs[coeff_ind0] + C1[np.unravel_index(ind1, (36, 8), "F")] = coeffs[coeff_ind1] # Linear elimination C2 = np.linalg.solve(C0, C1) # Setup action matrix - AM = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], -C2[31, :], -C2[32, :], - - C2[33, :], -C2[34, :], -C2[35, :], [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0], -C2[30, :]]) + AM = np.array( + [ + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + -C2[31, :], + -C2[32, :], + -C2[33, :], + -C2[34, :], + -C2[35, :], + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0], + -C2[30, :], + ] + ) # Solve eigenvalue problem and get real solutions D, V = np.linalg.eig(AM) - sols = np.array([V[6, :] / V[0, :], D, V[4, :] / - V[0, :], V[1, :] / V[0, :]]).T + sols = np.array( + [V[6, :] / V[0, :], D, V[4, :] / V[0, :], V[1, :] / V[0, :]] + ).T sols = sols[np.isreal(D), :] # Extract solutions @@ -92,8 +864,9 @@ def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): s = np.real(s) if s[3] < 0: continue - solutions.append((1.0, s[1], np.sqrt(s[0]), s[2] - * np.sqrt(s[0]), f0 / np.sqrt(s[3]))) + solutions.append( + (1.0, s[1], np.sqrt(s[0]), s[2] * np.sqrt(s[0]), f0 / np.sqrt(s[3])) + ) return solutions @@ -104,13 +877,12 @@ def find_transform(X1, X2): X1m = X1 - m1 X2m = X2 - m2 u, s, vt = np.linalg.svd(X2m.T @ X1m) - R = u @ np.diag([1.0, 1.0, np.linalg.det(u@vt)]) @ vt + R = u @ np.diag([1.0, 1.0, np.linalg.det(u @ vt)]) @ vt t = m2 - R @ m1 return R, t def test_solver(): - # Setup instance (with positive depths) while True: x1 = np.c_[np.random.randn(4, 2), np.ones((4,))] @@ -144,7 +916,9 @@ def test_solver(): sols = solve_shift_and_scale_shared_focal(x1, x2, d1, d2) sols_mono = madpose.solve_scale_and_shift_shared_focal(x1.T, x2.T, d1, d2) - posescaleoffsetsfs = madpose.estimate_scale_shift_pose_shared_focal(x1.T, x2.T, d1, d2) + posescaleoffsetsfs = madpose.estimate_scale_shift_pose_shared_focal( + x1.T, x2.T, d1, d2 + ) for p in posescaleoffsetsfs: R_est, t_est = p.R(), p.t() @@ -165,15 +939,24 @@ def test_solver(): X2 = x2u * d2_corr[:, None] print(R_est @ X1.T + t_est[:, None] - X2.T) - err_R = np.linalg.norm(R-R_est) - err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) - print(f'posescaleoffsetsfs, residual={err_a}, rotation={err_R}, translation={err_t}') + err_R = np.linalg.norm(R - R_est) + err_t = np.linalg.norm( + t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) + ) + print( + f"posescaleoffsetsfs, residual={err_a}, rotation={err_R}, translation={err_t}" + ) print(len(sols), len(sols_mono)) for k, (a1, b1, a2, b2, f) in enumerate(sols + sols_mono): - err = np.abs(a2 - a2_gt / a1_gt) + np.abs(b1 - b1_gt / a1_gt) + np.abs(b2 - b2_gt / a1_gt) + np.abs(f - f_gt) + err = ( + np.abs(a2 - a2_gt / a1_gt) + + np.abs(b1 - b1_gt / a1_gt) + + np.abs(b2 - b2_gt / a1_gt) + + np.abs(f - f_gt) + ) focal_err = np.abs(f - f_gt) - print(f'focal_err={focal_err}') + print(f"focal_err={focal_err}") d1_corr = a1 * d1 + b1 d2_corr = a2 * d2 + b2 @@ -187,10 +970,14 @@ def test_solver(): X2 = x2u * d2_corr[:, None] R_est, t_est = find_transform(X1[0:4, :], X2[0:4, :]) - err_R = np.linalg.norm(R-R_est) - err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) - print(f'solution={k}, residual={err}, rotation={err_R}, translation={err_t}') + err_R = np.linalg.norm(R - R_est) + err_t = np.linalg.norm( + t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) + ) + print( + f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}" + ) -if __name__ == '__main__': +if __name__ == "__main__": test_solver() diff --git a/solver_py/shift_and_scale_two_focal.py b/solver_py/shift_and_scale_two_focal.py index 37e6481..a16bcdc 100644 --- a/solver_py/shift_and_scale_two_focal.py +++ b/solver_py/shift_and_scale_two_focal.py @@ -1,5 +1,6 @@ -import numpy as np import madpose +import numpy as np + def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): # Estimates scale, shift, and two focal lengths @@ -11,7 +12,7 @@ def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): # d2_corrected = a2 * d2 + b2 # and f1 and f2 is the two focal lengths. # Note that only the first 3 points satisfy the rigidity constraints. - + # Normalize focal length x1 = x1_.copy() x2 = x2_.copy() @@ -23,61 +24,931 @@ def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): # Compute coefficients coeffs = np.zeros((40,)) - coeffs[0] = 2*x2[0,0]*x2[1,0] + 2*x2[0,1]*x2[1,1] - (x2[0,0]**2) - (x2[0,1]**2) - (x2[1,0]**2) - (x2[1,1]**2) - coeffs[1] = (x1[0,0]**2) - 2*x1[0,1]*x1[1,1] - 2*x1[0,0]*x1[1,0] + (x1[0,1]**2) + (x1[1,0]**2) + (x1[1,1]**2) - coeffs[2] = 2*d2[0]*x2[0,0]*x2[1,0] - 2*d2[0]*(x2[0,1]**2) - 2*d2[1]*(x2[1,0]**2) - 2*d2[1]*(x2[1,1]**2) - 2*d2[0]*(x2[0,0]**2) + 2*d2[1]*x2[0,0]*x2[1,0] + 2*d2[0]*x2[0,1]*x2[1,1] + 2*d2[1]*x2[0,1]*x2[1,1] - coeffs[3] = 2*d1[0]*(x1[0,0]**2) + 2*d1[0]*(x1[0,1]**2) + 2*d1[1]*(x1[1,0]**2) + 2*d1[1]*(x1[1,1]**2) - 2*d1[0]*x1[0,0]*x1[1,0] - 2*d1[1]*x1[0,0]*x1[1,0] - 2*d1[0]*x1[0,1]*x1[1,1] - 2*d1[1]*x1[0,1]*x1[1,1] - coeffs[4] = 2*d2[0]*d2[1]*x2[0,0]*x2[1,0] - (d2[0]**2)*(x2[0,1]**2) - (d2[1]**2)*(x2[1,0]**2) - (d2[1]**2)*(x2[1,1]**2) - (d2[0]**2)*(x2[0,0]**2) + 2*d2[0]*d2[1]*x2[0,1]*x2[1,1] - coeffs[5] = 2*d2[0]*d2[1] - (d2[0]**2) - (d2[1]**2) - coeffs[6] = (d1[0]**2)*(x1[0,0]**2) + (d1[0]**2)*(x1[0,1]**2) + (d1[1]**2)*(x1[1,0]**2) + (d1[1]**2)*(x1[1,1]**2) - 2*d1[0]*d1[1]*x1[0,0]*x1[1,0] - 2*d1[0]*d1[1]*x1[0,1]*x1[1,1] - coeffs[7] = (d1[0]**2) - 2*d1[0]*d1[1] + (d1[1]**2) - coeffs[8] = 2*x2[0,0]*x2[2,0] + 2*x2[0,1]*x2[2,1] - (x2[0,0]**2) - (x2[0,1]**2) - (x2[2,0]**2) - (x2[2,1]**2) - coeffs[9] = (x1[0,0]**2) - 2*x1[0,1]*x1[2,1] - 2*x1[0,0]*x1[2,0] + (x1[0,1]**2) + (x1[2,0]**2) + (x1[2,1]**2) - coeffs[10] = 2*d2[0]*x2[0,0]*x2[2,0] - 2*d2[0]*(x2[0,1]**2) - 2*d2[2]*(x2[2,0]**2) - 2*d2[2]*(x2[2,1]**2) - 2*d2[0]*(x2[0,0]**2) + 2*d2[0]*x2[0,1]*x2[2,1] + 2*d2[2]*x2[0,0]*x2[2,0] + 2*d2[2]*x2[0,1]*x2[2,1] - coeffs[11] = 2*d1[0]*(x1[0,0]**2) + 2*d1[0]*(x1[0,1]**2) + 2*d1[2]*(x1[2,0]**2) + 2*d1[2]*(x1[2,1]**2) - 2*d1[0]*x1[0,0]*x1[2,0] - 2*d1[0]*x1[0,1]*x1[2,1] - 2*d1[2]*x1[0,0]*x1[2,0] - 2*d1[2]*x1[0,1]*x1[2,1] - coeffs[12] = 2*d2[0]*d2[2]*x2[0,0]*x2[2,0] - (d2[0]**2)*(x2[0,1]**2) - (d2[2]**2)*(x2[2,0]**2) - (d2[2]**2)*(x2[2,1]**2) - (d2[0]**2)*(x2[0,0]**2) + 2*d2[0]*d2[2]*x2[0,1]*x2[2,1] - coeffs[13] = 2*d2[0]*d2[2] - (d2[0]**2) - (d2[2]**2) - coeffs[14] = (d1[0]**2)*(x1[0,0]**2) + (d1[0]**2)*(x1[0,1]**2) + (d1[2]**2)*(x1[2,0]**2) + (d1[2]**2)*(x1[2,1]**2) - 2*d1[0]*d1[2]*x1[0,0]*x1[2,0] - 2*d1[0]*d1[2]*x1[0,1]*x1[2,1] - coeffs[15] = (d1[0]**2) - 2*d1[0]*d1[2] + (d1[2]**2) - coeffs[16] = 2*x2[1,0]*x2[2,0] + 2*x2[1,1]*x2[2,1] - (x2[1,0]**2) - (x2[1,1]**2) - (x2[2,0]**2) - (x2[2,1]**2) - coeffs[17] = (x1[1,0]**2) - 2*x1[1,1]*x1[2,1] - 2*x1[1,0]*x1[2,0] + (x1[1,1]**2) + (x1[2,0]**2) + (x1[2,1]**2) - coeffs[18] = 2*d2[1]*x2[1,0]*x2[2,0] - 2*d2[1]*(x2[1,1]**2) - 2*d2[2]*(x2[2,0]**2) - 2*d2[2]*(x2[2,1]**2) - 2*d2[1]*(x2[1,0]**2) + 2*d2[2]*x2[1,0]*x2[2,0] + 2*d2[1]*x2[1,1]*x2[2,1] + 2*d2[2]*x2[1,1]*x2[2,1] - coeffs[19] = 2*d1[1]*(x1[1,0]**2) + 2*d1[1]*(x1[1,1]**2) + 2*d1[2]*(x1[2,0]**2) + 2*d1[2]*(x1[2,1]**2) - 2*d1[1]*x1[1,0]*x1[2,0] - 2*d1[2]*x1[1,0]*x1[2,0] - 2*d1[1]*x1[1,1]*x1[2,1] - 2*d1[2]*x1[1,1]*x1[2,1] - coeffs[20] = 2*d2[1]*d2[2]*x2[1,0]*x2[2,0] - (d2[1]**2)*(x2[1,1]**2) - (d2[2]**2)*(x2[2,0]**2) - (d2[2]**2)*(x2[2,1]**2) - (d2[1]**2)*(x2[1,0]**2) + 2*d2[1]*d2[2]*x2[1,1]*x2[2,1] - coeffs[21] = 2*d2[1]*d2[2] - (d2[1]**2) - (d2[2]**2) - coeffs[22] = (d1[1]**2)*(x1[1,0]**2) + (d1[1]**2)*(x1[1,1]**2) + (d1[2]**2)*(x1[2,0]**2) + (d1[2]**2)*(x1[2,1]**2) - 2*d1[1]*d1[2]*x1[1,0]*x1[2,0] - 2*d1[1]*d1[2]*x1[1,1]*x1[2,1] - coeffs[23] = (d1[1]**2) - 2*d1[1]*d1[2] + (d1[2]**2) - coeffs[24] = 2*x2[0,0]*x2[3,0] + 2*x2[0,1]*x2[3,1] - (x2[0,0]**2) - (x2[0,1]**2) - (x2[3,0]**2) - (x2[3,1]**2) - coeffs[25] = (x1[0,0]**2) - 2*x1[0,1]*x1[3,1] - 2*x1[0,0]*x1[3,0] + (x1[0,1]**2) + (x1[3,0]**2) + (x1[3,1]**2) - coeffs[26] = 2*d2[0]*x2[0,0]*x2[3,0] - 2*d2[0]*(x2[0,1]**2) - 2*d2[3]*(x2[3,0]**2) - 2*d2[3]*(x2[3,1]**2) - 2*d2[0]*(x2[0,0]**2) + 2*d2[0]*x2[0,1]*x2[3,1] + 2*d2[3]*x2[0,0]*x2[3,0] + 2*d2[3]*x2[0,1]*x2[3,1] - coeffs[27] = 2*d1[0]*(x1[0,0]**2) + 2*d1[0]*(x1[0,1]**2) + 2*d1[3]*(x1[3,0]**2) + 2*d1[3]*(x1[3,1]**2) - 2*d1[0]*x1[0,0]*x1[3,0] - 2*d1[0]*x1[0,1]*x1[3,1] - 2*d1[3]*x1[0,0]*x1[3,0] - 2*d1[3]*x1[0,1]*x1[3,1] - coeffs[28] = 2*d2[0]*d2[3]*x2[0,0]*x2[3,0] - (d2[0]**2)*(x2[0,1]**2) - (d2[3]**2)*(x2[3,0]**2) - (d2[3]**2)*(x2[3,1]**2) - (d2[0]**2)*(x2[0,0]**2) + 2*d2[0]*d2[3]*x2[0,1]*x2[3,1] - coeffs[29] = 2*d2[0]*d2[3] - (d2[0]**2) - (d2[3]**2) - coeffs[30] = (d1[0]**2)*(x1[0,0]**2) + (d1[0]**2)*(x1[0,1]**2) + (d1[3]**2)*(x1[3,0]**2) + (d1[3]**2)*(x1[3,1]**2) - 2*d1[0]*d1[3]*x1[0,0]*x1[3,0] - 2*d1[0]*d1[3]*x1[0,1]*x1[3,1] - coeffs[31] = (d1[0]**2) - 2*d1[0]*d1[3] + (d1[3]**2) - coeffs[32] = 2*x2[1,0]*x2[3,0] + 2*x2[1,1]*x2[3,1] - (x2[1,0]**2) - (x2[1,1]**2) - (x2[3,0]**2) - (x2[3,1]**2) - coeffs[33] = (x1[1,0]**2) - 2*x1[1,1]*x1[3,1] - 2*x1[1,0]*x1[3,0] + (x1[1,1]**2) + (x1[3,0]**2) + (x1[3,1]**2) - coeffs[34] = 2*d2[1]*x2[1,0]*x2[3,0] - 2*d2[1]*(x2[1,1]**2) - 2*d2[3]*(x2[3,0]**2) - 2*d2[3]*(x2[3,1]**2) - 2*d2[1]*(x2[1,0]**2) + 2*d2[1]*x2[1,1]*x2[3,1] + 2*d2[3]*x2[1,0]*x2[3,0] + 2*d2[3]*x2[1,1]*x2[3,1] - coeffs[35] = 2*d1[1]*(x1[1,0]**2) + 2*d1[1]*(x1[1,1]**2) + 2*d1[3]*(x1[3,0]**2) + 2*d1[3]*(x1[3,1]**2) - 2*d1[1]*x1[1,0]*x1[3,0] - 2*d1[1]*x1[1,1]*x1[3,1] - 2*d1[3]*x1[1,0]*x1[3,0] - 2*d1[3]*x1[1,1]*x1[3,1] - coeffs[36] = 2*d2[1]*d2[3]*x2[1,0]*x2[3,0] - (d2[1]**2)*(x2[1,1]**2) - (d2[3]**2)*(x2[3,0]**2) - (d2[3]**2)*(x2[3,1]**2) - (d2[1]**2)*(x2[1,0]**2) + 2*d2[1]*d2[3]*x2[1,1]*x2[3,1] - coeffs[37] = 2*d2[1]*d2[3] - (d2[1]**2) - (d2[3]**2) - coeffs[38] = (d1[1]**2)*(x1[1,0]**2) + (d1[1]**2)*(x1[1,1]**2) + (d1[3]**2)*(x1[3,0]**2) + (d1[3]**2)*(x1[3,1]**2) - 2*d1[1]*d1[3]*x1[1,0]*x1[3,0] - 2*d1[1]*d1[3]*x1[1,1]*x1[3,1] - coeffs[39] = (d1[1]**2) - 2*d1[1]*d1[3] + (d1[3]**2) + coeffs[0] = ( + 2 * x2[0, 0] * x2[1, 0] + + 2 * x2[0, 1] * x2[1, 1] + - (x2[0, 0] ** 2) + - (x2[0, 1] ** 2) + - (x2[1, 0] ** 2) + - (x2[1, 1] ** 2) + ) + coeffs[1] = ( + (x1[0, 0] ** 2) + - 2 * x1[0, 1] * x1[1, 1] + - 2 * x1[0, 0] * x1[1, 0] + + (x1[0, 1] ** 2) + + (x1[1, 0] ** 2) + + (x1[1, 1] ** 2) + ) + coeffs[2] = ( + 2 * d2[0] * x2[0, 0] * x2[1, 0] + - 2 * d2[0] * (x2[0, 1] ** 2) + - 2 * d2[1] * (x2[1, 0] ** 2) + - 2 * d2[1] * (x2[1, 1] ** 2) + - 2 * d2[0] * (x2[0, 0] ** 2) + + 2 * d2[1] * x2[0, 0] * x2[1, 0] + + 2 * d2[0] * x2[0, 1] * x2[1, 1] + + 2 * d2[1] * x2[0, 1] * x2[1, 1] + ) + coeffs[3] = ( + 2 * d1[0] * (x1[0, 0] ** 2) + + 2 * d1[0] * (x1[0, 1] ** 2) + + 2 * d1[1] * (x1[1, 0] ** 2) + + 2 * d1[1] * (x1[1, 1] ** 2) + - 2 * d1[0] * x1[0, 0] * x1[1, 0] + - 2 * d1[1] * x1[0, 0] * x1[1, 0] + - 2 * d1[0] * x1[0, 1] * x1[1, 1] + - 2 * d1[1] * x1[0, 1] * x1[1, 1] + ) + coeffs[4] = ( + 2 * d2[0] * d2[1] * x2[0, 0] * x2[1, 0] + - (d2[0] ** 2) * (x2[0, 1] ** 2) + - (d2[1] ** 2) * (x2[1, 0] ** 2) + - (d2[1] ** 2) * (x2[1, 1] ** 2) + - (d2[0] ** 2) * (x2[0, 0] ** 2) + + 2 * d2[0] * d2[1] * x2[0, 1] * x2[1, 1] + ) + coeffs[5] = 2 * d2[0] * d2[1] - (d2[0] ** 2) - (d2[1] ** 2) + coeffs[6] = ( + (d1[0] ** 2) * (x1[0, 0] ** 2) + + (d1[0] ** 2) * (x1[0, 1] ** 2) + + (d1[1] ** 2) * (x1[1, 0] ** 2) + + (d1[1] ** 2) * (x1[1, 1] ** 2) + - 2 * d1[0] * d1[1] * x1[0, 0] * x1[1, 0] + - 2 * d1[0] * d1[1] * x1[0, 1] * x1[1, 1] + ) + coeffs[7] = (d1[0] ** 2) - 2 * d1[0] * d1[1] + (d1[1] ** 2) + coeffs[8] = ( + 2 * x2[0, 0] * x2[2, 0] + + 2 * x2[0, 1] * x2[2, 1] + - (x2[0, 0] ** 2) + - (x2[0, 1] ** 2) + - (x2[2, 0] ** 2) + - (x2[2, 1] ** 2) + ) + coeffs[9] = ( + (x1[0, 0] ** 2) + - 2 * x1[0, 1] * x1[2, 1] + - 2 * x1[0, 0] * x1[2, 0] + + (x1[0, 1] ** 2) + + (x1[2, 0] ** 2) + + (x1[2, 1] ** 2) + ) + coeffs[10] = ( + 2 * d2[0] * x2[0, 0] * x2[2, 0] + - 2 * d2[0] * (x2[0, 1] ** 2) + - 2 * d2[2] * (x2[2, 0] ** 2) + - 2 * d2[2] * (x2[2, 1] ** 2) + - 2 * d2[0] * (x2[0, 0] ** 2) + + 2 * d2[0] * x2[0, 1] * x2[2, 1] + + 2 * d2[2] * x2[0, 0] * x2[2, 0] + + 2 * d2[2] * x2[0, 1] * x2[2, 1] + ) + coeffs[11] = ( + 2 * d1[0] * (x1[0, 0] ** 2) + + 2 * d1[0] * (x1[0, 1] ** 2) + + 2 * d1[2] * (x1[2, 0] ** 2) + + 2 * d1[2] * (x1[2, 1] ** 2) + - 2 * d1[0] * x1[0, 0] * x1[2, 0] + - 2 * d1[0] * x1[0, 1] * x1[2, 1] + - 2 * d1[2] * x1[0, 0] * x1[2, 0] + - 2 * d1[2] * x1[0, 1] * x1[2, 1] + ) + coeffs[12] = ( + 2 * d2[0] * d2[2] * x2[0, 0] * x2[2, 0] + - (d2[0] ** 2) * (x2[0, 1] ** 2) + - (d2[2] ** 2) * (x2[2, 0] ** 2) + - (d2[2] ** 2) * (x2[2, 1] ** 2) + - (d2[0] ** 2) * (x2[0, 0] ** 2) + + 2 * d2[0] * d2[2] * x2[0, 1] * x2[2, 1] + ) + coeffs[13] = 2 * d2[0] * d2[2] - (d2[0] ** 2) - (d2[2] ** 2) + coeffs[14] = ( + (d1[0] ** 2) * (x1[0, 0] ** 2) + + (d1[0] ** 2) * (x1[0, 1] ** 2) + + (d1[2] ** 2) * (x1[2, 0] ** 2) + + (d1[2] ** 2) * (x1[2, 1] ** 2) + - 2 * d1[0] * d1[2] * x1[0, 0] * x1[2, 0] + - 2 * d1[0] * d1[2] * x1[0, 1] * x1[2, 1] + ) + coeffs[15] = (d1[0] ** 2) - 2 * d1[0] * d1[2] + (d1[2] ** 2) + coeffs[16] = ( + 2 * x2[1, 0] * x2[2, 0] + + 2 * x2[1, 1] * x2[2, 1] + - (x2[1, 0] ** 2) + - (x2[1, 1] ** 2) + - (x2[2, 0] ** 2) + - (x2[2, 1] ** 2) + ) + coeffs[17] = ( + (x1[1, 0] ** 2) + - 2 * x1[1, 1] * x1[2, 1] + - 2 * x1[1, 0] * x1[2, 0] + + (x1[1, 1] ** 2) + + (x1[2, 0] ** 2) + + (x1[2, 1] ** 2) + ) + coeffs[18] = ( + 2 * d2[1] * x2[1, 0] * x2[2, 0] + - 2 * d2[1] * (x2[1, 1] ** 2) + - 2 * d2[2] * (x2[2, 0] ** 2) + - 2 * d2[2] * (x2[2, 1] ** 2) + - 2 * d2[1] * (x2[1, 0] ** 2) + + 2 * d2[2] * x2[1, 0] * x2[2, 0] + + 2 * d2[1] * x2[1, 1] * x2[2, 1] + + 2 * d2[2] * x2[1, 1] * x2[2, 1] + ) + coeffs[19] = ( + 2 * d1[1] * (x1[1, 0] ** 2) + + 2 * d1[1] * (x1[1, 1] ** 2) + + 2 * d1[2] * (x1[2, 0] ** 2) + + 2 * d1[2] * (x1[2, 1] ** 2) + - 2 * d1[1] * x1[1, 0] * x1[2, 0] + - 2 * d1[2] * x1[1, 0] * x1[2, 0] + - 2 * d1[1] * x1[1, 1] * x1[2, 1] + - 2 * d1[2] * x1[1, 1] * x1[2, 1] + ) + coeffs[20] = ( + 2 * d2[1] * d2[2] * x2[1, 0] * x2[2, 0] + - (d2[1] ** 2) * (x2[1, 1] ** 2) + - (d2[2] ** 2) * (x2[2, 0] ** 2) + - (d2[2] ** 2) * (x2[2, 1] ** 2) + - (d2[1] ** 2) * (x2[1, 0] ** 2) + + 2 * d2[1] * d2[2] * x2[1, 1] * x2[2, 1] + ) + coeffs[21] = 2 * d2[1] * d2[2] - (d2[1] ** 2) - (d2[2] ** 2) + coeffs[22] = ( + (d1[1] ** 2) * (x1[1, 0] ** 2) + + (d1[1] ** 2) * (x1[1, 1] ** 2) + + (d1[2] ** 2) * (x1[2, 0] ** 2) + + (d1[2] ** 2) * (x1[2, 1] ** 2) + - 2 * d1[1] * d1[2] * x1[1, 0] * x1[2, 0] + - 2 * d1[1] * d1[2] * x1[1, 1] * x1[2, 1] + ) + coeffs[23] = (d1[1] ** 2) - 2 * d1[1] * d1[2] + (d1[2] ** 2) + coeffs[24] = ( + 2 * x2[0, 0] * x2[3, 0] + + 2 * x2[0, 1] * x2[3, 1] + - (x2[0, 0] ** 2) + - (x2[0, 1] ** 2) + - (x2[3, 0] ** 2) + - (x2[3, 1] ** 2) + ) + coeffs[25] = ( + (x1[0, 0] ** 2) + - 2 * x1[0, 1] * x1[3, 1] + - 2 * x1[0, 0] * x1[3, 0] + + (x1[0, 1] ** 2) + + (x1[3, 0] ** 2) + + (x1[3, 1] ** 2) + ) + coeffs[26] = ( + 2 * d2[0] * x2[0, 0] * x2[3, 0] + - 2 * d2[0] * (x2[0, 1] ** 2) + - 2 * d2[3] * (x2[3, 0] ** 2) + - 2 * d2[3] * (x2[3, 1] ** 2) + - 2 * d2[0] * (x2[0, 0] ** 2) + + 2 * d2[0] * x2[0, 1] * x2[3, 1] + + 2 * d2[3] * x2[0, 0] * x2[3, 0] + + 2 * d2[3] * x2[0, 1] * x2[3, 1] + ) + coeffs[27] = ( + 2 * d1[0] * (x1[0, 0] ** 2) + + 2 * d1[0] * (x1[0, 1] ** 2) + + 2 * d1[3] * (x1[3, 0] ** 2) + + 2 * d1[3] * (x1[3, 1] ** 2) + - 2 * d1[0] * x1[0, 0] * x1[3, 0] + - 2 * d1[0] * x1[0, 1] * x1[3, 1] + - 2 * d1[3] * x1[0, 0] * x1[3, 0] + - 2 * d1[3] * x1[0, 1] * x1[3, 1] + ) + coeffs[28] = ( + 2 * d2[0] * d2[3] * x2[0, 0] * x2[3, 0] + - (d2[0] ** 2) * (x2[0, 1] ** 2) + - (d2[3] ** 2) * (x2[3, 0] ** 2) + - (d2[3] ** 2) * (x2[3, 1] ** 2) + - (d2[0] ** 2) * (x2[0, 0] ** 2) + + 2 * d2[0] * d2[3] * x2[0, 1] * x2[3, 1] + ) + coeffs[29] = 2 * d2[0] * d2[3] - (d2[0] ** 2) - (d2[3] ** 2) + coeffs[30] = ( + (d1[0] ** 2) * (x1[0, 0] ** 2) + + (d1[0] ** 2) * (x1[0, 1] ** 2) + + (d1[3] ** 2) * (x1[3, 0] ** 2) + + (d1[3] ** 2) * (x1[3, 1] ** 2) + - 2 * d1[0] * d1[3] * x1[0, 0] * x1[3, 0] + - 2 * d1[0] * d1[3] * x1[0, 1] * x1[3, 1] + ) + coeffs[31] = (d1[0] ** 2) - 2 * d1[0] * d1[3] + (d1[3] ** 2) + coeffs[32] = ( + 2 * x2[1, 0] * x2[3, 0] + + 2 * x2[1, 1] * x2[3, 1] + - (x2[1, 0] ** 2) + - (x2[1, 1] ** 2) + - (x2[3, 0] ** 2) + - (x2[3, 1] ** 2) + ) + coeffs[33] = ( + (x1[1, 0] ** 2) + - 2 * x1[1, 1] * x1[3, 1] + - 2 * x1[1, 0] * x1[3, 0] + + (x1[1, 1] ** 2) + + (x1[3, 0] ** 2) + + (x1[3, 1] ** 2) + ) + coeffs[34] = ( + 2 * d2[1] * x2[1, 0] * x2[3, 0] + - 2 * d2[1] * (x2[1, 1] ** 2) + - 2 * d2[3] * (x2[3, 0] ** 2) + - 2 * d2[3] * (x2[3, 1] ** 2) + - 2 * d2[1] * (x2[1, 0] ** 2) + + 2 * d2[1] * x2[1, 1] * x2[3, 1] + + 2 * d2[3] * x2[1, 0] * x2[3, 0] + + 2 * d2[3] * x2[1, 1] * x2[3, 1] + ) + coeffs[35] = ( + 2 * d1[1] * (x1[1, 0] ** 2) + + 2 * d1[1] * (x1[1, 1] ** 2) + + 2 * d1[3] * (x1[3, 0] ** 2) + + 2 * d1[3] * (x1[3, 1] ** 2) + - 2 * d1[1] * x1[1, 0] * x1[3, 0] + - 2 * d1[1] * x1[1, 1] * x1[3, 1] + - 2 * d1[3] * x1[1, 0] * x1[3, 0] + - 2 * d1[3] * x1[1, 1] * x1[3, 1] + ) + coeffs[36] = ( + 2 * d2[1] * d2[3] * x2[1, 0] * x2[3, 0] + - (d2[1] ** 2) * (x2[1, 1] ** 2) + - (d2[3] ** 2) * (x2[3, 0] ** 2) + - (d2[3] ** 2) * (x2[3, 1] ** 2) + - (d2[1] ** 2) * (x2[1, 0] ** 2) + + 2 * d2[1] * d2[3] * x2[1, 1] * x2[3, 1] + ) + coeffs[37] = 2 * d2[1] * d2[3] - (d2[1] ** 2) - (d2[3] ** 2) + coeffs[38] = ( + (d1[1] ** 2) * (x1[1, 0] ** 2) + + (d1[1] ** 2) * (x1[1, 1] ** 2) + + (d1[3] ** 2) * (x1[3, 0] ** 2) + + (d1[3] ** 2) * (x1[3, 1] ** 2) + - 2 * d1[1] * d1[3] * x1[1, 0] * x1[3, 0] + - 2 * d1[1] * d1[3] * x1[1, 1] * x1[3, 1] + ) + coeffs[39] = (d1[1] ** 2) - 2 * d1[1] * d1[3] + (d1[3] ** 2) # Setup expanded equation system - coeff_ind0 = [0, 8, 16, 24, 32, 0, 8, 16, 24, 32, 1, 9, 17, 25, 33, 2, 10, 0, 8, 16, 18, 24, 26, 32, 34, 0, 8, 16, 24, 32, 1, 9, 17, 25, 33, 2, 10, 8, 0, 16, 18, 24, 26, 32, 34, 8, 0, 16, 24, 32, 1, 9, 17, 25, 33, 3, 11, 1, 9, 17, 19, 25, 27, 33, 35, 4, 12, 2, 10, 18, 20, 26, 28, 34, 36, 2, 10, 8, 16, 18, 0, 26, 24, 32, 34, 9, 1, 17, 25, 33, 3, 11, 9, 1, 19, 17, 27, 25, 33, 35, 5, 4, 13, 12, 10, 2, 18, 21, 20, 26, 29, 28, 34, 36, 37, 10, 2, 8, 0, 18, 24, 26, 32, 34, 16, 3, 11, 19, 9, 17, 1, 27, 25, 33, 35, 6, 14, 3, 11, 19, 22, 27, 30, 35, 38, 4, 12, 20, 28, 36, - 4, 12, 10, 18, 20, 2, 28, 26, 34, 36, 5, 13, 21, 29, 37, 11, 3, 19, 9, 1, 27, 25, 33, 17, 35, 6, 14, 11, 3, 22, 19, 30, 27, 35, 38, 5, 12, 13, 21, 4, 20, 28, 29, 36, 37, 5, 12, 13, 4, 10, 21, 2, 20, 26, 29, 28, 34, 36, 37, 18, 7, 15, 23, 31, 39, 6, 14, 22, 11, 19, 3, 30, 27, 35, 38, 6, 14, 22, 30, 38, 12, 20, 4, 28, 36, 13, 5, 21, 29, 37, 13, 5, 21, 29, 37, 14, 6, 22, 30, 38, 13, 12, 21, 5, 4, 28, 29, 37, 36, 20, 7, 15, 23, 31, 39, 14, 22, 6, 30, 38, 13, 5, 29, 37, 21, 15, 7, 23, 31, 39, 7, 15, 23, 31, 39, 14, 6, 22, 11, 3, 30, 27, 35, 19, 38, 7, 15, 23, 31, 39,] - coeff_ind1 = [15, 7, 31, 39, 23, 15, 7, 23, 31, - 39, 14, 6, 30, 38, 22, 15, 23, 7, 31, 39,] - ind0 = [0, 2, 18, 28, 39, 41, 45, 60, 69, 78, 80, 82, 98, 108, 119, 120, 122, 123, 128, 130, 138, 145, 148, 157, 159, 164, 169, 177, 186, 194, 201, 205, 220, 229, 238, 241, 245, 246, 252, 254, 260, 263, 269, 276, 278, 287, 293, 302, 310, 313, 323, 328, 330, 345, 357, 360, 362, 364, 369, 377, 378, 386, 388, 394, 399, 400, 402, 403, 408, 410, 418, 425, 428, 437, 439, 444, 449, 451, 456, 457, 459, 466, 467, 471, 474, 486, 492, 494, 503, 516, 521, 525, 527, 533, 540, 542, 549, 550, 553, 558, 560, 561, 562, 565, 566, 572, 574, 578, 580, 583, 588, 589, 596, 598, 599, 607, 613, 615, 621, 622, 624, 630, 632, 633, 635, 643, 648, 650, 651, 656, 659, 665, 667, 671, 677, 680, 682, 684, 689, 697, 698, 706, 708, 714, 719, 723, 728, 730, 745, 757, 764, 769, 771, 776, 777, 779, 786, 787, 791, 794, - 801, 805, 820, 829, 838, 846, 852, 854, 855, 861, 863, 864, 872, 875, 876, 881, 885, 887, 893, 900, 902, 909, 910, 913, 918, 923, 926, 928, 930, 932, 934, 943, 945, 956, 957, 964, 967, 969, 973, 975, 977, 981, 982, 984, 986, 990, 992, 993, 994, 995, 1000, 1002, 1018, 1028, 1039, 1043, 1048, 1050, 1051, 1056, 1059, 1065, 1067, 1071, 1077, 1084, 1089, 1097, 1106, 1114, 1131, 1136, 1139, 1147, 1151, 1166, 1172, 1174, 1183, 1196, 1207, 1213, 1222, 1230, 1233, 1247, 1253, 1262, 1270, 1273, 1291, 1295, 1296, 1299, 1301, 1304, 1307, 1311, 1312, 1315, 1324, 1329, 1337, 1346, 1354, 1371, 1376, 1379, 1387, 1391, 1415, 1421, 1424, 1432, 1435, 1446, 1452, 1454, 1463, 1476, 1481, 1485, 1500, 1509, 1518, 1526, 1532, 1534, 1535, 1541, 1543, 1544, 1552, 1555, 1556, 1563, 1568, 1570, 1585, 1597,] - ind1 = [15, 21, 24, 32, 35, 47, 53, 62, 70, 73, 95, - 101, 104, 112, 115, 131, 136, 139, 147, 151,] + coeff_ind0 = [ + 0, + 8, + 16, + 24, + 32, + 0, + 8, + 16, + 24, + 32, + 1, + 9, + 17, + 25, + 33, + 2, + 10, + 0, + 8, + 16, + 18, + 24, + 26, + 32, + 34, + 0, + 8, + 16, + 24, + 32, + 1, + 9, + 17, + 25, + 33, + 2, + 10, + 8, + 0, + 16, + 18, + 24, + 26, + 32, + 34, + 8, + 0, + 16, + 24, + 32, + 1, + 9, + 17, + 25, + 33, + 3, + 11, + 1, + 9, + 17, + 19, + 25, + 27, + 33, + 35, + 4, + 12, + 2, + 10, + 18, + 20, + 26, + 28, + 34, + 36, + 2, + 10, + 8, + 16, + 18, + 0, + 26, + 24, + 32, + 34, + 9, + 1, + 17, + 25, + 33, + 3, + 11, + 9, + 1, + 19, + 17, + 27, + 25, + 33, + 35, + 5, + 4, + 13, + 12, + 10, + 2, + 18, + 21, + 20, + 26, + 29, + 28, + 34, + 36, + 37, + 10, + 2, + 8, + 0, + 18, + 24, + 26, + 32, + 34, + 16, + 3, + 11, + 19, + 9, + 17, + 1, + 27, + 25, + 33, + 35, + 6, + 14, + 3, + 11, + 19, + 22, + 27, + 30, + 35, + 38, + 4, + 12, + 20, + 28, + 36, + 4, + 12, + 10, + 18, + 20, + 2, + 28, + 26, + 34, + 36, + 5, + 13, + 21, + 29, + 37, + 11, + 3, + 19, + 9, + 1, + 27, + 25, + 33, + 17, + 35, + 6, + 14, + 11, + 3, + 22, + 19, + 30, + 27, + 35, + 38, + 5, + 12, + 13, + 21, + 4, + 20, + 28, + 29, + 36, + 37, + 5, + 12, + 13, + 4, + 10, + 21, + 2, + 20, + 26, + 29, + 28, + 34, + 36, + 37, + 18, + 7, + 15, + 23, + 31, + 39, + 6, + 14, + 22, + 11, + 19, + 3, + 30, + 27, + 35, + 38, + 6, + 14, + 22, + 30, + 38, + 12, + 20, + 4, + 28, + 36, + 13, + 5, + 21, + 29, + 37, + 13, + 5, + 21, + 29, + 37, + 14, + 6, + 22, + 30, + 38, + 13, + 12, + 21, + 5, + 4, + 28, + 29, + 37, + 36, + 20, + 7, + 15, + 23, + 31, + 39, + 14, + 22, + 6, + 30, + 38, + 13, + 5, + 29, + 37, + 21, + 15, + 7, + 23, + 31, + 39, + 7, + 15, + 23, + 31, + 39, + 14, + 6, + 22, + 11, + 3, + 30, + 27, + 35, + 19, + 38, + 7, + 15, + 23, + 31, + 39, + ] + coeff_ind1 = [ + 15, + 7, + 31, + 39, + 23, + 15, + 7, + 23, + 31, + 39, + 14, + 6, + 30, + 38, + 22, + 15, + 23, + 7, + 31, + 39, + ] + ind0 = [ + 0, + 2, + 18, + 28, + 39, + 41, + 45, + 60, + 69, + 78, + 80, + 82, + 98, + 108, + 119, + 120, + 122, + 123, + 128, + 130, + 138, + 145, + 148, + 157, + 159, + 164, + 169, + 177, + 186, + 194, + 201, + 205, + 220, + 229, + 238, + 241, + 245, + 246, + 252, + 254, + 260, + 263, + 269, + 276, + 278, + 287, + 293, + 302, + 310, + 313, + 323, + 328, + 330, + 345, + 357, + 360, + 362, + 364, + 369, + 377, + 378, + 386, + 388, + 394, + 399, + 400, + 402, + 403, + 408, + 410, + 418, + 425, + 428, + 437, + 439, + 444, + 449, + 451, + 456, + 457, + 459, + 466, + 467, + 471, + 474, + 486, + 492, + 494, + 503, + 516, + 521, + 525, + 527, + 533, + 540, + 542, + 549, + 550, + 553, + 558, + 560, + 561, + 562, + 565, + 566, + 572, + 574, + 578, + 580, + 583, + 588, + 589, + 596, + 598, + 599, + 607, + 613, + 615, + 621, + 622, + 624, + 630, + 632, + 633, + 635, + 643, + 648, + 650, + 651, + 656, + 659, + 665, + 667, + 671, + 677, + 680, + 682, + 684, + 689, + 697, + 698, + 706, + 708, + 714, + 719, + 723, + 728, + 730, + 745, + 757, + 764, + 769, + 771, + 776, + 777, + 779, + 786, + 787, + 791, + 794, + 801, + 805, + 820, + 829, + 838, + 846, + 852, + 854, + 855, + 861, + 863, + 864, + 872, + 875, + 876, + 881, + 885, + 887, + 893, + 900, + 902, + 909, + 910, + 913, + 918, + 923, + 926, + 928, + 930, + 932, + 934, + 943, + 945, + 956, + 957, + 964, + 967, + 969, + 973, + 975, + 977, + 981, + 982, + 984, + 986, + 990, + 992, + 993, + 994, + 995, + 1000, + 1002, + 1018, + 1028, + 1039, + 1043, + 1048, + 1050, + 1051, + 1056, + 1059, + 1065, + 1067, + 1071, + 1077, + 1084, + 1089, + 1097, + 1106, + 1114, + 1131, + 1136, + 1139, + 1147, + 1151, + 1166, + 1172, + 1174, + 1183, + 1196, + 1207, + 1213, + 1222, + 1230, + 1233, + 1247, + 1253, + 1262, + 1270, + 1273, + 1291, + 1295, + 1296, + 1299, + 1301, + 1304, + 1307, + 1311, + 1312, + 1315, + 1324, + 1329, + 1337, + 1346, + 1354, + 1371, + 1376, + 1379, + 1387, + 1391, + 1415, + 1421, + 1424, + 1432, + 1435, + 1446, + 1452, + 1454, + 1463, + 1476, + 1481, + 1485, + 1500, + 1509, + 1518, + 1526, + 1532, + 1534, + 1535, + 1541, + 1543, + 1544, + 1552, + 1555, + 1556, + 1563, + 1568, + 1570, + 1585, + 1597, + ] + ind1 = [ + 15, + 21, + 24, + 32, + 35, + 47, + 53, + 62, + 70, + 73, + 95, + 101, + 104, + 112, + 115, + 131, + 136, + 139, + 147, + 151, + ] C0 = np.zeros((40, 40)) C1 = np.zeros((40, 4)) - C0[np.unravel_index(ind0, (40, 40), 'F')] = coeffs[coeff_ind0] - C1[np.unravel_index(ind1, (40, 4), 'F')] = coeffs[coeff_ind1] + C0[np.unravel_index(ind0, (40, 40), "F")] = coeffs[coeff_ind0] + C1[np.unravel_index(ind1, (40, 4), "F")] = coeffs[coeff_ind1] # Linear elimination C2 = np.linalg.solve(C0, C1) @@ -87,7 +958,15 @@ def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): # Solve eigenvalue problem and get real solutions D, V = np.linalg.eig(AM) - sols = np.array([(-C2[35, :] @ V) / V[0, :], D, V[1, :] / V[0, :], V[2, :] / V[0, :], V[3, :] / V[0, :]]).T + sols = np.array( + [ + (-C2[35, :] @ V) / V[0, :], + D, + V[1, :] / V[0, :], + V[2, :] / V[0, :], + V[3, :] / V[0, :], + ] + ).T sols = sols[np.isreal(D), :] # Extract solutions @@ -97,7 +976,16 @@ def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): s = np.real(s) if s[3] < 0 or s[4] < 0: continue - solutions.append((1.0, s[1], np.sqrt(s[0]), s[2] * np.sqrt(s[0]), f1_0 / np.sqrt(s[3]), f2_0 / np.sqrt(s[4]))) + solutions.append( + ( + 1.0, + s[1], + np.sqrt(s[0]), + s[2] * np.sqrt(s[0]), + f1_0 / np.sqrt(s[3]), + f2_0 / np.sqrt(s[4]), + ) + ) return solutions @@ -108,13 +996,12 @@ def find_transform(X1, X2): X1m = X1 - m1 X2m = X2 - m2 u, s, vt = np.linalg.svd(X2m.T @ X1m) - R = u @ np.diag([1.0, 1.0, np.linalg.det(u@vt)]) @ vt + R = u @ np.diag([1.0, 1.0, np.linalg.det(u @ vt)]) @ vt t = m2 - R @ m1 return R, t def test_solver(): - # Setup instance (with positive depths) while True: x1 = np.c_[np.random.randn(4, 2), np.ones((4,))] @@ -149,15 +1036,25 @@ def test_solver(): sols = solve_shift_and_scale_two_focal(x1, x2, d1, d2) sols_mono = madpose.solve_scale_and_shift_two_focal(x1.T, x2.T, d1, d2) - posescaleoffsettfs = madpose.estimate_scale_shift_pose_two_focal(x1.T, x2.T, d1, d2) + posescaleoffsettfs = madpose.estimate_scale_shift_pose_two_focal( + x1.T, x2.T, d1, d2 + ) print(len(sols), len(sols_mono)) for k, (a1, b1, a2, b2, f1, f2) in enumerate(sols + sols_mono): - err = np.abs(a2 - a2_gt / a1_gt) + np.abs(b1 - b1_gt / a1_gt) + np.abs(b2 - b2_gt / a1_gt) + np.abs(f1 - f1_gt) + np.abs(f2 - f2_gt) + err = ( + np.abs(a2 - a2_gt / a1_gt) + + np.abs(b1 - b1_gt / a1_gt) + + np.abs(b2 - b2_gt / a1_gt) + + np.abs(f1 - f1_gt) + + np.abs(f2 - f2_gt) + ) focal_err = np.abs(f1 - f1_gt) + np.abs(f2 - f2_gt) - print(f'focal_error={focal_err}') + print(f"focal_error={focal_err}") - t_skew = np.array([[0, -t[2], t[1]], [t[2], 0, -t[0]], [-t[1], t[0], 0]]) + t_skew = np.array( + [[0, -t[2], t[1]], [t[2], 0, -t[0]], [-t[1], t[0], 0]] + ) E = t_skew @ R K2_ = np.array([[f2, 0, 0], [0, f2, 0], [0, 0, 1]]) K1_ = np.array([[f1, 0, 0], [0, f1, 0], [0, 0, 1]]) @@ -176,7 +1073,12 @@ def test_solver(): R_est, t_est = find_transform(X1[0:3, :], X2[0:3, :]) t_est_skew = np.array( - [[0, -t_est[2], t_est[1]], [t_est[2], 0, -t_est[0]], [-t_est[1], t_est[0], 0]]) + [ + [0, -t_est[2], t_est[1]], + [t_est[2], 0, -t_est[0]], + [-t_est[1], t_est[0], 0], + ] + ) E_est = t_est_skew @ R_est K2_est = np.array([[f2, 0, 0], [0, f2, 0], [0, 0, 1]]) K1_est = np.array([[f1, 0, 0], [0, f1, 0], [0, 0, 1]]) @@ -186,12 +1088,16 @@ def test_solver(): F = F / np.linalg.norm(F) F_est = F_est / np.linalg.norm(F_est) F_error = np.linalg.norm(F - F_est) - print(f'F_error={F_error}') + print(f"F_error={F_error}") err_R = np.linalg.norm(R - R_est) - err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) - print(f'solution={k}, residual={err}, rotation={err_R}, translation={err_t}') + err_t = np.linalg.norm( + t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) + ) + print( + f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}" + ) -if __name__ == '__main__': +if __name__ == "__main__": test_solver() From a2c09d143856b93d2c7f390a151f3094d9b475ee Mon Sep 17 00:00:00 2001 From: B1ueber2y Date: Tue, 7 Jan 2025 11:03:02 +0100 Subject: [PATCH 3/9] move to format folder --- scripts/{ => format}/clang_format.sh | 0 scripts/{ => format}/python.sh | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename scripts/{ => format}/clang_format.sh (100%) rename scripts/{ => format}/python.sh (100%) diff --git a/scripts/clang_format.sh b/scripts/format/clang_format.sh similarity index 100% rename from scripts/clang_format.sh rename to scripts/format/clang_format.sh diff --git a/scripts/python.sh b/scripts/format/python.sh similarity index 100% rename from scripts/python.sh rename to scripts/format/python.sh From c0611fe21cf389c70550e88aa0cc8877ed9edf95 Mon Sep 17 00:00:00 2001 From: B1ueber2y Date: Tue, 7 Jan 2025 11:04:57 +0100 Subject: [PATCH 4/9] format --- solver_py/shift_and_scale_two_focal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/solver_py/shift_and_scale_two_focal.py b/solver_py/shift_and_scale_two_focal.py index a16bcdc..83fd387 100644 --- a/solver_py/shift_and_scale_two_focal.py +++ b/solver_py/shift_and_scale_two_focal.py @@ -1036,7 +1036,7 @@ def test_solver(): sols = solve_shift_and_scale_two_focal(x1, x2, d1, d2) sols_mono = madpose.solve_scale_and_shift_two_focal(x1.T, x2.T, d1, d2) - posescaleoffsettfs = madpose.estimate_scale_shift_pose_two_focal( + madpose.estimate_scale_shift_pose_two_focal( x1.T, x2.T, d1, d2 ) From c8d236bd529e935b4e61966c45736a15adf82f07 Mon Sep 17 00:00:00 2001 From: B1ueber2y Date: Tue, 7 Jan 2025 11:08:37 +0100 Subject: [PATCH 5/9] fix sim102. add E501 linting exception --- ruff.toml | 1 + setup.py | 13 +++++++------ solver_py/shift_and_scale_two_focal.py | 4 +--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ruff.toml b/ruff.toml index da11434..e58f259 100644 --- a/ruff.toml +++ b/ruff.toml @@ -15,3 +15,4 @@ select = [ # isort "I", ] +ignore = ["E501"] diff --git a/setup.py b/setup.py index 6c395cf..4639e19 100644 --- a/setup.py +++ b/setup.py @@ -105,12 +105,13 @@ def build_extension(self, ext: CMakeExtension) -> None: # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level # across all generators. - if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ: - # self.parallel is a Python 3 only way to set parallel jobs by hand - # using -j in the build_ext call, not supported by pip or PyPA-build. - if hasattr(self, "parallel") and self.parallel: - # CMake 3.12+ only. - build_args += [f"-j{self.parallel}"] + # self.parallel is a Python 3 only way to set parallel jobs by hand + # using -j in the build_ext call, not supported by pip or PyPA-build. + if ("CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ) and ( + hasattr(self, "parallel") and self.parallel + ): + # CMake 3.12+ only. + build_args += [f"-j{self.parallel}"] build_temp = Path(self.build_temp) / ext.name if not build_temp.exists(): diff --git a/solver_py/shift_and_scale_two_focal.py b/solver_py/shift_and_scale_two_focal.py index 83fd387..80b4370 100644 --- a/solver_py/shift_and_scale_two_focal.py +++ b/solver_py/shift_and_scale_two_focal.py @@ -1036,9 +1036,7 @@ def test_solver(): sols = solve_shift_and_scale_two_focal(x1, x2, d1, d2) sols_mono = madpose.solve_scale_and_shift_two_focal(x1.T, x2.T, d1, d2) - madpose.estimate_scale_shift_pose_two_focal( - x1.T, x2.T, d1, d2 - ) + madpose.estimate_scale_shift_pose_two_focal(x1.T, x2.T, d1, d2) print(len(sols), len(sols_mono)) for k, (a1, b1, a2, b2, f1, f2) in enumerate(sols + sols_mono): From 474180ee0881568d1e963953fea3d40424c47e95 Mon Sep 17 00:00:00 2001 From: B1ueber2y Date: Tue, 7 Jan 2025 11:09:26 +0100 Subject: [PATCH 6/9] add formatting ci. --- .github/workflows/format.yml | 44 ++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 .github/workflows/format.yml diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 0000000..767948e --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,44 @@ +name: Check code formatting (Ubuntu) + +on: + push: + branches: + - main + pull_request: + types: [ assigned, opened, synchronize, reopened ] + release: + types: [ published, edited ] + +jobs: + build: + name: ${{ matrix.config.os }} formatting + runs-on: ${{ matrix.config.os }} + strategy: + matrix: + config: [ + { + os: ubuntu-22.04, + checkCodeFormat: true, + }, + ] + + env: + COMPILER_CACHE_VERSION: 1 + COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache + CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache + CCACHE_BASEDIR: ${{ github.workspace }} + CTCACHE_DIR: ${{ github.workspace }}/compiler-cache/ctcache + + steps: + - uses: actions/checkout@v4 + - name: Check code format + run: | + if [ "${{ matrix.config.checkCodeFormat }}" != "true" ]; then + exit 0 + fi + set +x -euo pipefail + python -m pip install ruff==0.6.7 clang-format==19.1.0 + ./scripts/format/clang_format.sh + ./scripts/format/python.sh + git diff --name-only + git diff --exit-code || (echo "Code formatting failed" && exit 1) From 1c269e005d06677293d8244c205279fcf34082e2 Mon Sep 17 00:00:00 2001 From: B1ueber2y Date: Tue, 7 Jan 2025 11:12:03 +0100 Subject: [PATCH 7/9] change permission --- scripts/format/clang_format.sh | 0 scripts/format/python.sh | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 scripts/format/clang_format.sh mode change 100644 => 100755 scripts/format/python.sh diff --git a/scripts/format/clang_format.sh b/scripts/format/clang_format.sh old mode 100644 new mode 100755 diff --git a/scripts/format/python.sh b/scripts/format/python.sh old mode 100644 new mode 100755 From e2c90c8eb3c2aaac307345e3c9137bb8bef7e5dd Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Thu, 9 Jan 2025 20:42:05 +0100 Subject: [PATCH 8/9] optimize format --- examples/calibrated.py | 78 ++- examples/shared_focal.py | 56 +- examples/two_focal.py | 70 ++- madpose/__init__.py | 2 +- madpose/utils.py | 25 +- ruff.toml | 2 +- setup.py | 30 +- solver_py/scale_and_shift.py | 144 +---- solver_py/scale_and_shift_shared_focal.py | 630 +------------------- solver_py/scale_and_shift_two_focal.py | 685 +--------------------- 10 files changed, 204 insertions(+), 1518 deletions(-) diff --git a/examples/calibrated.py b/examples/calibrated.py index 19d6136..7f0a58e 100644 --- a/examples/calibrated.py +++ b/examples/calibrated.py @@ -1,12 +1,13 @@ import json import os -import numpy as np + import cv2 +import numpy as np import madpose -from madpose.utils import get_depths, compute_pose_error +from madpose.utils import compute_pose_error, get_depths -sample_path = 'examples/image_pairs/0_scannet' +sample_path = "examples/image_pairs/0_scannet" # Thresholds for reprojection and epipolar errors reproj_pix_thres = 8.0 @@ -21,35 +22,35 @@ options.final_least_squares = True options.threshold_multiplier = 5.0 options.num_lo_steps = 4 -options.squared_inlier_thresholds = [reproj_pix_thres ** 2, epipolar_pix_thres ** 2] +options.squared_inlier_thresholds = [reproj_pix_thres**2, epipolar_pix_thres**2] options.data_type_weights = [1.0, epipolar_weight] -options.random_seed = 0 +options.random_seed = 0 est_config = madpose.EstimatorConfig() est_config.min_depth_constraint = True est_config.use_shift = True # Read the image pair -image0 = cv2.imread(os.path.join(sample_path, 'image0.png')) -image1 = cv2.imread(os.path.join(sample_path, 'image1.png')) +image0 = cv2.imread(os.path.join(sample_path, "image0.png")) +image1 = cv2.imread(os.path.join(sample_path, "image1.png")) # Read info -with open(os.path.join(sample_path, 'info.json'), 'r') as f: +with open(os.path.join(sample_path, "info.json")) as f: info = json.load(f) # Load camera intrinsics -K0 = np.array(info['K0']) -K1 = np.array(info['K1']) +K0 = np.array(info["K0"]) +K1 = np.array(info["K1"]) # Load pre-computed keypoints (you can also run keypoint detectors of your choice) -matches_0_file = os.path.join(sample_path, info['matches_0_file']) -matches_1_file = os.path.join(sample_path, info['matches_1_file']) +matches_0_file = os.path.join(sample_path, info["matches_0_file"]) +matches_1_file = os.path.join(sample_path, info["matches_1_file"]) mkpts0 = np.load(matches_0_file) mkpts1 = np.load(matches_1_file) # Load pre-computed depth maps (you can also run Monocular Depth models of your choice) -depth_0_file = os.path.join(sample_path, info['depth_0_file']) -depth_1_file = os.path.join(sample_path, info['depth_1_file']) +depth_0_file = os.path.join(sample_path, info["depth_0_file"]) +depth_1_file = os.path.join(sample_path, info["depth_1_file"]) depth_map0 = np.load(depth_0_file) depth_map1 = np.load(depth_1_file) @@ -59,18 +60,23 @@ # Run hybrid estimation pose, stats = madpose.HybridEstimatePoseScaleOffset( - mkpts0, mkpts1, - depth0, depth1, - [depth_map0.min(), depth_map1.min()], - K0, K1, options, est_config - ) + mkpts0, + mkpts1, + depth0, + depth1, + [depth_map0.min(), depth_map1.min()], + K0, + K1, + options, + est_config, +) # rotation and translation of the estimated pose R_est, t_est = pose.R(), pose.t() # scale and offsets of the affine corrected depth maps s_est, o0_est, o1_est = pose.scale, pose.offset0, pose.offset1 # Load the GT Pose -T_0to1 = np.array(info['T_0to1']) +T_0to1 = np.array(info["T_0to1"]) # Compute the pose error err_t, err_R = compute_pose_error(T_0to1, R_est, t_est) @@ -81,13 +87,29 @@ print(f"Estimated scale, offset0, offset1: {s_est:.4f}, {o0_est:.4f}, {o1_est:.4f}") # Run point-based estimation using PoseLib -import poselib - -ransac_opt_dict = {'max_epipolar_error': epipolar_pix_thres, 'min_iterations': 1000, 'max_iterations': 10000} -cam0_dict = {'model': 'PINHOLE', 'width': image0.shape[1], 'height': image0.shape[0], 'params': [K0[0, 0], K0[1, 1], K0[0, 2], K0[1, 2]]} -cam1_dict = {'model': 'PINHOLE', 'width': image1.shape[1], 'height': image1.shape[0], 'params': [K1[0, 0], K1[1, 1], K1[0, 2], K1[1, 2]]} - -pose_ponly, stats = poselib.estimate_relative_pose(mkpts0, mkpts1, cam0_dict, cam1_dict, ransac_opt_dict) +import poselib # noqa: E402 + +ransac_opt_dict = { + "max_epipolar_error": epipolar_pix_thres, + "min_iterations": 1000, + "max_iterations": 10000, +} +cam0_dict = { + "model": "PINHOLE", + "width": image0.shape[1], + "height": image0.shape[0], + "params": [K0[0, 0], K0[1, 1], K0[0, 2], K0[1, 2]], +} +cam1_dict = { + "model": "PINHOLE", + "width": image1.shape[1], + "height": image1.shape[0], + "params": [K1[0, 0], K1[1, 1], K1[0, 2], K1[1, 2]], +} + +pose_ponly, stats = poselib.estimate_relative_pose( + mkpts0, mkpts1, cam0_dict, cam1_dict, ransac_opt_dict +) R_ponly, t_ponly = pose_ponly.R, pose_ponly.t # Compute the pose error @@ -95,4 +117,4 @@ print("--- Point-based Estimation Results ---") print(f"Rotation Error: {err_R:.4f} degrees") -print(f"Translation Error: {err_t:.4f} degrees") \ No newline at end of file +print(f"Translation Error: {err_t:.4f} degrees") diff --git a/examples/shared_focal.py b/examples/shared_focal.py index 836bda9..10e5e7e 100644 --- a/examples/shared_focal.py +++ b/examples/shared_focal.py @@ -1,12 +1,13 @@ import json import os -import numpy as np + import cv2 +import numpy as np import madpose -from madpose.utils import get_depths, compute_pose_error +from madpose.utils import compute_pose_error, get_depths -sample_path = 'examples/image_pairs/1_eth3d' +sample_path = "examples/image_pairs/1_eth3d" # Thresholds for reprojection and epipolar errors reproj_pix_thres = 8.0 @@ -21,35 +22,35 @@ options.final_least_squares = True options.threshold_multiplier = 5.0 options.num_lo_steps = 4 -options.squared_inlier_thresholds = [reproj_pix_thres ** 2, epipolar_pix_thres ** 2] +options.squared_inlier_thresholds = [reproj_pix_thres**2, epipolar_pix_thres**2] options.data_type_weights = [1.0, epipolar_weight] -options.random_seed = 0 +options.random_seed = 0 est_config = madpose.EstimatorConfig() est_config.min_depth_constraint = True est_config.use_shift = True # Read the image pair -image0 = cv2.imread(os.path.join(sample_path, 'image0.png')) -image1 = cv2.imread(os.path.join(sample_path, 'image1.png')) +image0 = cv2.imread(os.path.join(sample_path, "image0.png")) +image1 = cv2.imread(os.path.join(sample_path, "image1.png")) # Read info -with open(os.path.join(sample_path, 'info.json'), 'r') as f: +with open(os.path.join(sample_path, "info.json")) as f: info = json.load(f) # Load camera intrinsics -K0 = np.array(info['K0']) -K1 = np.array(info['K1']) +K0 = np.array(info["K0"]) +K1 = np.array(info["K1"]) # Load pre-computed keypoints (you can also run keypoint detectors of your choice) -matches_0_file = os.path.join(sample_path, info['matches_0_file']) -matches_1_file = os.path.join(sample_path, info['matches_1_file']) +matches_0_file = os.path.join(sample_path, info["matches_0_file"]) +matches_1_file = os.path.join(sample_path, info["matches_1_file"]) mkpts0 = np.load(matches_0_file) mkpts1 = np.load(matches_1_file) # Load pre-computed depth maps (you can also run Monocular Depth models of your choice) -depth_0_file = os.path.join(sample_path, info['depth_0_file']) -depth_1_file = os.path.join(sample_path, info['depth_1_file']) +depth_0_file = os.path.join(sample_path, info["depth_0_file"]) +depth_1_file = os.path.join(sample_path, info["depth_1_file"]) depth_map0 = np.load(depth_0_file) depth_map1 = np.load(depth_1_file) @@ -64,11 +65,16 @@ # Run hybrid estimation pose, stats = madpose.HybridEstimatePoseScaleOffsetSharedFocal( - mkpts0, mkpts1, - depth0, depth1, - [depth_map0.min(), depth_map1.min()], - pp0, pp1, options, est_config - ) + mkpts0, + mkpts1, + depth0, + depth1, + [depth_map0.min(), depth_map1.min()], + pp0, + pp1, + options, + est_config, +) # rotation and translation of the estimated pose R_est, t_est = pose.R(), pose.t() # scale and offsets of the affine corrected depth maps @@ -77,7 +83,7 @@ f_est = pose.focal # Load the GT Pose -T_0to1 = np.array(info['T_0to1']) +T_0to1 = np.array(info["T_0to1"]) # Compute the pose error err_t, err_R = compute_pose_error(T_0to1, R_est, t_est) @@ -93,9 +99,13 @@ print(f"Estimated scale, offset0, offset1: {s_est:.4f}, {o0_est:.4f}, {o1_est:.4f}") # Run point-based estimation using PoseLib -import poselib +import poselib # noqa: E402 -ransac_opt_dict = {'max_epipolar_error': epipolar_pix_thres, 'min_iterations': 1000, 'max_iterations': 10000} +ransac_opt_dict = { + "max_epipolar_error": epipolar_pix_thres, + "min_iterations": 1000, + "max_iterations": 10000, +} img_pair, stats = poselib.estimate_shared_focal_relative_pose(mkpts0, mkpts1, pp, ransac_opt_dict) shared_focal_pose = img_pair.pose R_ponly, t_ponly = shared_focal_pose.R, shared_focal_pose.t @@ -110,4 +120,4 @@ print("--- Point-based Estimation Results ---") print(f"Rotation Error: {err_R:.4f} degrees") print(f"Translation Error: {err_t:.4f} degrees") -print(f"Focal Error: {(err_f * 100):.2f}%") \ No newline at end of file +print(f"Focal Error: {(err_f * 100):.2f}%") diff --git a/examples/two_focal.py b/examples/two_focal.py index 2de59e4..090bd9a 100644 --- a/examples/two_focal.py +++ b/examples/two_focal.py @@ -1,12 +1,13 @@ import json import os -import numpy as np + import cv2 +import numpy as np import madpose -from madpose.utils import get_depths, compute_pose_error, bougnoux_numpy +from madpose.utils import bougnoux_numpy, compute_pose_error, get_depths -sample_path = 'examples/image_pairs/2_2d3ds' +sample_path = "examples/image_pairs/2_2d3ds" # Thresholds for reprojection and epipolar errors reproj_pix_thres = 16.0 @@ -21,35 +22,35 @@ options.final_least_squares = True options.threshold_multiplier = 5.0 options.num_lo_steps = 4 -options.squared_inlier_thresholds = [reproj_pix_thres ** 2, epipolar_pix_thres ** 2] +options.squared_inlier_thresholds = [reproj_pix_thres**2, epipolar_pix_thres**2] options.data_type_weights = [1.0, epipolar_weight] -options.random_seed = 0 +options.random_seed = 0 est_config = madpose.EstimatorConfig() est_config.min_depth_constraint = True est_config.use_shift = True # Read the image pair -image0 = cv2.imread(os.path.join(sample_path, 'image0.png')) -image1 = cv2.imread(os.path.join(sample_path, 'image1.png')) +image0 = cv2.imread(os.path.join(sample_path, "image0.png")) +image1 = cv2.imread(os.path.join(sample_path, "image1.png")) # Read info -with open(os.path.join(sample_path, 'info.json'), 'r') as f: +with open(os.path.join(sample_path, "info.json")) as f: info = json.load(f) # Load camera intrinsics -K0 = np.array(info['K0']) -K1 = np.array(info['K1']) +K0 = np.array(info["K0"]) +K1 = np.array(info["K1"]) # Load pre-computed keypoints (you can also run keypoint detectors of your choice) -matches_0_file = os.path.join(sample_path, info['matches_0_file']) -matches_1_file = os.path.join(sample_path, info['matches_1_file']) +matches_0_file = os.path.join(sample_path, info["matches_0_file"]) +matches_1_file = os.path.join(sample_path, info["matches_1_file"]) mkpts0 = np.load(matches_0_file) mkpts1 = np.load(matches_1_file) # Load pre-computed depth maps (you can also run Monocular Depth models of your choice) -depth_0_file = os.path.join(sample_path, info['depth_0_file']) -depth_1_file = os.path.join(sample_path, info['depth_1_file']) +depth_0_file = os.path.join(sample_path, info["depth_0_file"]) +depth_1_file = os.path.join(sample_path, info["depth_1_file"]) depth_map0 = np.load(depth_0_file) depth_map1 = np.load(depth_1_file) @@ -64,11 +65,16 @@ # Run hybrid estimation pose, stats = madpose.HybridEstimatePoseScaleOffsetTwoFocal( - mkpts0, mkpts1, - depth0, depth1, - [depth_map0.min(), depth_map1.min()], - pp0, pp1, options, est_config - ) + mkpts0, + mkpts1, + depth0, + depth1, + [depth_map0.min(), depth_map1.min()], + pp0, + pp1, + options, + est_config, +) # rotation and translation of the estimated pose R_est, t_est = pose.R(), pose.t() # scale and offsets of the affine corrected depth maps @@ -77,7 +83,7 @@ f0_est, f1_est = pose.focal0, pose.focal1 # Load the GT Pose -T_0to1 = np.array(info['T_0to1']) +T_0to1 = np.array(info["T_0to1"]) # Compute the pose error err_t, err_R = compute_pose_error(T_0to1, R_est, t_est) @@ -92,12 +98,14 @@ print(f"Estimated scale, offset0, offset1: {s_est:.4f}, {o0_est:.4f}, {o1_est:.4f}") # Run point-based estimation using PoseLib -import poselib +import poselib # noqa: E402 -ransac_opt_dict = {'max_epipolar_error': epipolar_pix_thres, - 'progressive_sampling': True, - 'min_iterations': 1000, - 'max_iterations': 10000} +ransac_opt_dict = { + "max_epipolar_error": epipolar_pix_thres, + "progressive_sampling": True, + "min_iterations": 1000, + "max_iterations": 10000, +} F, stats = poselib.estimate_fundamental(mkpts0, mkpts1, ransac_opt_dict) f0, f1 = bougnoux_numpy(F, pp0, pp1) @@ -112,7 +120,7 @@ n_f, R_f, t_f, _ = cv2.recoverPose(E_f, kpts0_f, kpts1_f, np.eye(3), 1e9) t_f = t_f.flatten() R_ponly, t_ponly = R_f, t_f - + # Compute the pose error err_t, err_R = compute_pose_error(T_0to1, R_ponly, t_ponly) @@ -126,13 +134,13 @@ # Since we used mast3r's matchings and depth maps, # we also include here pose estimation results from mast3r on this image pair (2_2d3ds) -with open(os.path.join(sample_path, 'mast3r_pose.json'), 'r') as f: +with open(os.path.join(sample_path, "mast3r_pose.json")) as f: mast3r_results = json.load(f) -RT_mast3r = np.array(mast3r_results['RT']) +RT_mast3r = np.array(mast3r_results["RT"]) R_mast3r = RT_mast3r[:3, :3] t_mast3r = RT_mast3r[:3, 3] -f0_mast3r = mast3r_results['f0'] -f1_mast3r = mast3r_results['f1'] +f0_mast3r = mast3r_results["f0"] +f1_mast3r = mast3r_results["f1"] # Compute the pose error err_t, err_R = compute_pose_error(T_0to1, R_mast3r, t_mast3r) @@ -143,4 +151,4 @@ print("--- MASt3R Estimation Results ---") print(f"Rotation Error: {err_R:.4f} degrees") print(f"Translation Error: {err_t:.4f} degrees") -print(f"Focal Error: {(err_f * 100):.2f}%") \ No newline at end of file +print(f"Focal Error: {(err_f * 100):.2f}%") diff --git a/madpose/__init__.py b/madpose/__init__.py index a8c3728..106eafe 100644 --- a/madpose/__init__.py +++ b/madpose/__init__.py @@ -1 +1 @@ -from .madpose import * \ No newline at end of file +from .madpose import * # noqa diff --git a/madpose/utils.py b/madpose/utils.py index 33e9e8e..27c456b 100644 --- a/madpose/utils.py +++ b/madpose/utils.py @@ -3,7 +3,12 @@ def get_depths(image, depth_map, mkpts): # Calculate the scaling factor between the image and the depth map - resize = np.array([depth_map.shape[1] / image.shape[1], depth_map.shape[0] / image.shape[0]]) + resize = np.array( + [ + depth_map.shape[1] / image.shape[1], + depth_map.shape[0] / image.shape[0], + ] + ) # Scale the keypoints to match the depth map's resolution and round to the nearest integer scaled_coords = np.round(mkpts * resize).astype(int) @@ -32,17 +37,9 @@ def bougnoux_numpy(F, p1, p2): e2 = e2 / e2[2] # Skew-symmetric matrices for e1 and e2 - s_e2 = np.array([ - [0, -e2[2], e2[1]], - [e2[2], 0, -e2[0]], - [-e2[1], e2[0], 0] - ]) - - s_e1 = np.array([ - [0, -e1[2], e1[1]], - [e1[2], 0, -e1[0]], - [-e1[1], e1[0], 0] - ]) + s_e2 = np.array([[0, -e2[2], e2[1]], [e2[2], 0, -e2[0]], [-e2[1], e2[0], 0]]) + + s_e1 = np.array([[0, -e1[2], e1[1]], [e1[2], 0, -e1[0]], [-e1[1], e1[0], 0]]) # Diagonal matrix II II = np.diag([1.0, 1.0, 0.0]) @@ -61,7 +58,7 @@ def bougnoux_numpy(F, p1, p2): def angle_error_mat(R1, R2): cos = (np.trace(np.dot(R1.T, R2)) - 1) / 2 - cos = np.clip(cos, -1., 1.) # numercial errors can make it out of bounds + cos = np.clip(cos, -1.0, 1.0) # numercial errors can make it out of bounds return np.rad2deg(np.abs(np.arccos(cos))) @@ -78,4 +75,4 @@ def compute_pose_error(T_0to1, R, t, t_thres=None): error_R = angle_error_mat(R, R_gt) if t_thres is not None and np.linalg.norm(t_gt) < t_thres: error_t = 0 - return error_t, error_R \ No newline at end of file + return error_t, error_R diff --git a/ruff.toml b/ruff.toml index e58f259..7267c7d 100644 --- a/ruff.toml +++ b/ruff.toml @@ -1,4 +1,4 @@ -line-length = 80 +line-length = 100 [lint] select = [ diff --git a/setup.py b/setup.py index 4639e19..da0a005 100644 --- a/setup.py +++ b/setup.py @@ -34,11 +34,7 @@ def build_extension(self, ext: CMakeExtension) -> None: # Using this requires trailing slash for auto-detection & inclusion of # auxiliary "native" libs - debug = ( - int(os.environ.get("DEBUG", 0)) - if self.debug is None - else self.debug - ) + debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug cfg = "Debug" if debug else "Release" # CMake lets you override the generator - we need to check this. @@ -57,9 +53,7 @@ def build_extension(self, ext: CMakeExtension) -> None: # Adding CMake arguments set as environment variable # (needed e.g. to build for ARM OSx on conda-forge) if "CMAKE_ARGS" in os.environ: - cmake_args += [ - item for item in os.environ["CMAKE_ARGS"].split(" ") if item - ] + cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item] # In this example, we pass in the version to C++. You might not need to. # cmake_args += [f"-DEXAMPLE_VERSION_INFO={self.distribution.get_version()}"] @@ -75,9 +69,7 @@ def build_extension(self, ext: CMakeExtension) -> None: else: # Single config generators are handled "normally" - single_config = any( - x in cmake_generator for x in {"NMake", "Ninja"} - ) + single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) # CMake allows an arch-in-generator style for backward compatibility contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) @@ -90,18 +82,14 @@ def build_extension(self, ext: CMakeExtension) -> None: # Multi-config generators have a different way to specify configs if not single_config: - cmake_args += [ - f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}" - ] + cmake_args += [f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}"] build_args += ["--config", cfg] if sys.platform.startswith("darwin"): # Cross-compile support for macOS - respect ARCHFLAGS if set archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", "")) if archs: - cmake_args += [ - "-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs)) - ] + cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))] # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level # across all generators. @@ -117,12 +105,8 @@ def build_extension(self, ext: CMakeExtension) -> None: if not build_temp.exists(): build_temp.mkdir(parents=True) - subprocess.run( - ["cmake", ext.sourcedir, *cmake_args], cwd=build_temp, check=True - ) - subprocess.run( - ["cmake", "--build", ".", *build_args], cwd=build_temp, check=True - ) + subprocess.run(["cmake", ext.sourcedir, *cmake_args], cwd=build_temp, check=True) + subprocess.run(["cmake", "--build", ".", *build_args], cwd=build_temp, check=True) # The information here can also be placed in setup.cfg - better separation of diff --git a/solver_py/scale_and_shift.py b/solver_py/scale_and_shift.py index 537fefd..69d0b41 100644 --- a/solver_py/scale_and_shift.py +++ b/solver_py/scale_and_shift.py @@ -1,6 +1,7 @@ -import madpose import numpy as np +import madpose + def solve_shift_and_scale(x1, x2, d1, d2): # Estimates scale and shift @@ -80,127 +81,16 @@ def solve_shift_and_scale(x1, x2, d1, d2): - 2 * d1[1] * d1[2] * x1[1].dot(x1[2]) ) + # fmt: off # Setup expanded equation system - coeff_ind0 = [ - 0, - 6, - 12, - 1, - 7, - 13, - 2, - 8, - 0, - 6, - 12, - 14, - 6, - 0, - 12, - 1, - 7, - 13, - 3, - 9, - 2, - 8, - 14, - 15, - 4, - 10, - 7, - 1, - 16, - 13, - 8, - 2, - 6, - 12, - 0, - 14, - 9, - 3, - 8, - 14, - 2, - 15, - 3, - 9, - 15, - 4, - 10, - 16, - 7, - 13, - 1, - 5, - 11, - 10, - 4, - 17, - 16, - ] + coeff_ind0 = [0, 6, 12, 1, 7, 13, 2, 8, 0, 6, 12, 14, 6, 0, 12, 1, 7, 13, 3, 9, 2, 8, 14, 15, 4, 10, 7, + 1, 16, 13, 8, 2, 6, 12, 0, 14, 9, 3, 8, 14, 2, 15, 3, 9, 15, 4, 10, 16, 7, 13, 1, 5, 11, 10, 4, 17, 16] coeff_ind1 = [11, 17, 5, 9, 15, 3, 5, 11, 17, 10, 16, 4, 11, 5, 17] - ind0 = [ - 0, - 1, - 9, - 12, - 13, - 21, - 24, - 25, - 26, - 28, - 29, - 33, - 39, - 42, - 47, - 50, - 52, - 53, - 60, - 61, - 62, - 64, - 65, - 69, - 72, - 73, - 75, - 78, - 81, - 83, - 87, - 90, - 91, - 92, - 94, - 95, - 99, - 102, - 103, - 104, - 106, - 107, - 110, - 112, - 113, - 122, - 124, - 125, - 127, - 128, - 130, - 132, - 133, - 135, - 138, - 141, - 143, - ] + ind0 = [0, 1, 9, 12, 13, 21, 24, 25, 26, 28, 29, 33, 39, 42, 47, 50, 52, 53, 60, 61, 62, 64, 65, 69, 72, 73, 75, 78, 81, 83, 87, + 90, 91, 92, 94, 95, 99, 102, 103, 104, 106, 107, 110, 112, 113, 122, 124, 125, 127, 128, 130, 132, 133, 135, 138, 141, 143] ind1 = [7, 8, 10, 19, 20, 22, 26, 28, 29, 31, 32, 34, 39, 42, 47] + + # fmt: on C0 = np.zeros((12, 12)) C1 = np.zeros((12, 4)) C0[np.unravel_index(ind0, (12, 12), "F")] = coeffs[coeff_ind0] @@ -279,12 +169,8 @@ def test_solver(): X2 = x2 * d2_corr[:, None] err_R = np.linalg.norm(R - R_est) - err_t = np.linalg.norm( - t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) - ) - print( - f"posescaleoffsets, residual={err_a}, rotation={err_R}, translation={err_t}" - ) + err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) + print(f"posescaleoffsets, residual={err_a}, rotation={err_R}, translation={err_t}") for k, (a1, b1, a2, b2) in enumerate(sols + sols_madpose): err = np.abs(a2 - a2_gt / a1_gt) + np.abs(b1 - b1_gt / a1_gt) + np.abs(b2 - b2_gt / a1_gt) @@ -298,12 +184,8 @@ def test_solver(): R_est, t_est = find_transform(X1, X2) err_R = np.linalg.norm(R - R_est) - err_t = np.linalg.norm( - t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) - ) - print( - f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}" - ) + err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) + print(f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}") if __name__ == "__main__": diff --git a/solver_py/scale_and_shift_shared_focal.py b/solver_py/scale_and_shift_shared_focal.py index 3a5491f..efbeddf 100644 --- a/solver_py/scale_and_shift_shared_focal.py +++ b/solver_py/scale_and_shift_shared_focal.py @@ -1,6 +1,7 @@ -import madpose import numpy as np +import madpose + def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): # Estimates scale, shift and a shared focal length @@ -242,592 +243,18 @@ def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): ) coeffs[31] = (d1[0] ** 2) - 2 * d1[0] * d1[3] + (d1[3] ** 2) + # fmt: off # Setup expanded equation system - coeff_ind0 = [ - 0, - 8, - 16, - 24, - 1, - 9, - 17, - 25, - 2, - 10, - 0, - 8, - 16, - 18, - 24, - 26, - 0, - 8, - 16, - 24, - 0, - 8, - 16, - 24, - 0, - 8, - 16, - 24, - 1, - 9, - 17, - 25, - 1, - 9, - 17, - 25, - 3, - 11, - 2, - 10, - 18, - 19, - 26, - 27, - 4, - 12, - 2, - 1, - 10, - 9, - 20, - 18, - 17, - 28, - 26, - 25, - 1, - 9, - 17, - 25, - 2, - 10, - 8, - 0, - 16, - 18, - 24, - 26, - 2, - 10, - 0, - 16, - 18, - 8, - 24, - 26, - 8, - 0, - 16, - 24, - 5, - 13, - 21, - 29, - 3, - 11, - 19, - 27, - 4, - 3, - 12, - 11, - 20, - 9, - 28, - 1, - 19, - 17, - 27, - 25, - 4, - 12, - 1, - 17, - 20, - 9, - 25, - 28, - 3, - 11, - 10, - 2, - 18, - 19, - 26, - 27, - 6, - 14, - 4, - 3, - 12, - 11, - 2, - 22, - 18, - 19, - 20, - 10, - 26, - 30, - 28, - 27, - 4, - 12, - 9, - 20, - 1, - 17, - 25, - 28, - 10, - 2, - 18, - 0, - 16, - 24, - 26, - 8, - 5, - 13, - 21, - 29, - 11, - 3, - 19, - 27, - 6, - 14, - 22, - 12, - 3, - 30, - 4, - 19, - 20, - 11, - 27, - 28, - 6, - 14, - 4, - 20, - 22, - 12, - 1, - 17, - 25, - 28, - 30, - 9, - 6, - 14, - 11, - 3, - 19, - 22, - 2, - 18, - 26, - 27, - 30, - 10, - 6, - 14, - 12, - 22, - 4, - 20, - 28, - 30, - 14, - 6, - 22, - 3, - 19, - 27, - 30, - 11, - 14, - 6, - 22, - 30, - 5, - 13, - 21, - 29, - 6, - 22, - 14, - 4, - 20, - 28, - 30, - 12, - 7, - 15, - 23, - 31, - 5, - 13, - 21, - 29, - 7, - 15, - 23, - 31, - 7, - 15, - 5, - 13, - 23, - 21, - 31, - 29, - ] - coeff_ind1 = [ - 7, - 23, - 31, - 15, - 6, - 22, - 30, - 14, - 7, - 23, - 15, - 31, - 7, - 15, - 23, - 5, - 31, - 21, - 13, - 29, - 15, - 7, - 23, - 31, - 7, - 15, - 13, - 5, - 21, - 23, - 29, - 31, - 15, - 7, - 23, - 5, - 21, - 29, - 31, - 13, - 13, - 5, - 21, - 29, - ] - ind0 = [ - 0, - 1, - 14, - 30, - 36, - 37, - 50, - 66, - 72, - 73, - 74, - 78, - 80, - 86, - 87, - 102, - 111, - 115, - 126, - 139, - 148, - 153, - 167, - 177, - 185, - 190, - 199, - 215, - 218, - 222, - 224, - 231, - 255, - 259, - 270, - 283, - 288, - 289, - 290, - 294, - 296, - 302, - 303, - 318, - 324, - 325, - 327, - 328, - 331, - 333, - 338, - 342, - 347, - 354, - 355, - 357, - 365, - 370, - 379, - 395, - 400, - 405, - 407, - 412, - 418, - 419, - 428, - 429, - 437, - 442, - 444, - 449, - 451, - 456, - 461, - 467, - 481, - 488, - 489, - 496, - 504, - 505, - 518, - 534, - 542, - 546, - 548, - 555, - 578, - 579, - 582, - 583, - 584, - 587, - 591, - 592, - 594, - 598, - 607, - 608, - 615, - 619, - 624, - 629, - 630, - 636, - 641, - 643, - 652, - 657, - 659, - 664, - 670, - 671, - 680, - 681, - 684, - 685, - 688, - 689, - 693, - 694, - 696, - 698, - 701, - 703, - 707, - 708, - 713, - 714, - 717, - 719, - 725, - 730, - 733, - 739, - 740, - 741, - 748, - 755, - 769, - 776, - 777, - 781, - 782, - 783, - 784, - 790, - 796, - 801, - 815, - 825, - 839, - 844, - 850, - 860, - 866, - 870, - 872, - 875, - 876, - 879, - 880, - 881, - 886, - 888, - 893, - 896, - 903, - 907, - 912, - 917, - 918, - 924, - 925, - 926, - 927, - 929, - 931, - 934, - 940, - 945, - 949, - 956, - 957, - 959, - 961, - 962, - 963, - 964, - 969, - 970, - 977, - 982, - 985, - 991, - 992, - 993, - 1000, - 1007, - 1019, - 1024, - 1030, - 1033, - 1034, - 1035, - 1040, - 1042, - 1057, - 1064, - 1065, - 1072, - 1082, - 1086, - 1088, - 1095, - 1128, - 1133, - 1140, - 1141, - 1142, - 1143, - 1145, - 1150, - 1155, - 1159, - 1170, - 1183, - 1191, - 1195, - 1206, - 1219, - 1229, - 1234, - 1243, - 1259, - 1260, - 1261, - 1265, - 1270, - 1274, - 1279, - 1290, - 1295, - ] - ind1 = [ - 25, - 26, - 27, - 34, - 61, - 62, - 63, - 70, - 84, - 89, - 96, - 101, - 110, - 114, - 116, - 120, - 123, - 125, - 132, - 137, - 157, - 164, - 165, - 172, - 184, - 189, - 193, - 200, - 201, - 203, - 208, - 213, - 227, - 232, - 238, - 241, - 242, - 243, - 248, - 250, - 263, - 268, - 274, - 284, - ] - + coeff_ind0 = [0, 8, 16, 24, 1, 9, 17, 25, 2, 10, 0, 8, 16, 18, 24, 26, 0, 8, 16, 24, 0, 8, 16, 24, 0, 8, 16, 24, 1, 9, 17, 25, 1, 9, 17, 25, 3, 11, 2, 10, 18, 19, 26, 27, 4, 12, 2, 1, 10, 9, 20, 18, 17, 28, 26, 25, 1, 9, 17, 25, 2, 10, 8, 0, 16, 18, 24, 26, 2, 10, 0, 16, 18, 8, 24, 26, 8, 0, 16, 24, 5, 13, 21, 29, 3, 11, 19, 27, 4, 3, 12, 11, 20, 9, 28, 1, 19, 17, 27, 25, 4, 12, 1, 17, 20, 9, 25, 28, 3, 11, 10, 2, 18, 19, 26, 27, 6, 14, 4, 3, 12, 11, + 2, 22, 18, 19, 20, 10, 26, 30, 28, 27, 4, 12, 9, 20, 1, 17, 25, 28, 10, 2, 18, 0, 16, 24, 26, 8, 5, 13, 21, 29, 11, 3, 19, 27, 6, 14, 22, 12, 3, 30, 4, 19, 20, 11, 27, 28, 6, 14, 4, 20, 22, 12, 1, 17, 25, 28, 30, 9, 6, 14, 11, 3, 19, 22, 2, 18, 26, 27, 30, 10, 6, 14, 12, 22, 4, 20, 28, 30, 14, 6, 22, 3, 19, 27, 30, 11, 14, 6, 22, 30, 5, 13, 21, 29, 6, 22, 14, 4, 20, 28, 30, 12, 7, 15, 23, 31, 5, 13, 21, 29, 7, 15, 23, 31, 7, 15, 5, 13, 23, 21, 31, 29] + coeff_ind1 = [7, 23, 31, 15, 6, 22, 30, 14, 7, 23, 15, 31, 7, 15, 23, 5, 31, 21, 13, 29, + 15, 7, 23, 31, 7, 15, 13, 5, 21, 23, 29, 31, 15, 7, 23, 5, 21, 29, 31, 13, 13, 5, 21, 29] + ind0 = [0, 1, 14, 30, 36, 37, 50, 66, 72, 73, 74, 78, 80, 86, 87, 102, 111, 115, 126, 139, 148, 153, 167, 177, 185, 190, 199, 215, 218, 222, 224, 231, 255, 259, 270, 283, 288, 289, 290, 294, 296, 302, 303, 318, 324, 325, 327, 328, 331, 333, 338, 342, 347, 354, 355, 357, 365, 370, 379, 395, 400, 405, 407, 412, 418, 419, 428, 429, 437, 442, 444, 449, 451, 456, 461, 467, 481, 488, 489, 496, 504, 505, 518, 534, 542, 546, 548, 555, 578, 579, 582, 583, 584, 587, 591, 592, 594, 598, 607, 608, 615, 619, 624, 629, 630, 636, 641, 643, 652, 657, 659, 664, 670, 671, 680, 681, 684, 685, 688, 689, 693, 694, 696, 698, 701, 703, 707, 708, + 713, 714, 717, 719, 725, 730, 733, 739, 740, 741, 748, 755, 769, 776, 777, 781, 782, 783, 784, 790, 796, 801, 815, 825, 839, 844, 850, 860, 866, 870, 872, 875, 876, 879, 880, 881, 886, 888, 893, 896, 903, 907, 912, 917, 918, 924, 925, 926, 927, 929, 931, 934, 940, 945, 949, 956, 957, 959, 961, 962, 963, 964, 969, 970, 977, 982, 985, 991, 992, 993, 1000, 1007, 1019, 1024, 1030, 1033, 1034, 1035, 1040, 1042, 1057, 1064, 1065, 1072, 1082, 1086, 1088, 1095, 1128, 1133, 1140, 1141, 1142, 1143, 1145, 1150, 1155, 1159, 1170, 1183, 1191, 1195, 1206, 1219, 1229, 1234, 1243, 1259, 1260, 1261, 1265, 1270, 1274, 1279, 1290, 1295] + ind1 = [25, 26, 27, 34, 61, 62, 63, 70, 84, 89, 96, 101, 110, 114, 116, 120, 123, 125, 132, 137, 157, 164, + 165, 172, 184, 189, 193, 200, 201, 203, 208, 213, 227, 232, 238, 241, 242, 243, 248, 250, 263, 268, 274, 284] + + # fmt: on C0 = np.zeros((36, 36)) C1 = np.zeros((36, 8)) C0[np.unravel_index(ind0, (36, 36), "F")] = coeffs[coeff_ind0] @@ -852,9 +279,7 @@ def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): # Solve eigenvalue problem and get real solutions D, V = np.linalg.eig(AM) - sols = np.array( - [V[6, :] / V[0, :], D, V[4, :] / V[0, :], V[1, :] / V[0, :]] - ).T + sols = np.array([V[6, :] / V[0, :], D, V[4, :] / V[0, :], V[1, :] / V[0, :]]).T sols = sols[np.isreal(D), :] # Extract solutions @@ -864,9 +289,7 @@ def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): s = np.real(s) if s[3] < 0: continue - solutions.append( - (1.0, s[1], np.sqrt(s[0]), s[2] * np.sqrt(s[0]), f0 / np.sqrt(s[3])) - ) + solutions.append((1.0, s[1], np.sqrt(s[0]), s[2] * np.sqrt(s[0]), f0 / np.sqrt(s[3]))) return solutions @@ -937,16 +360,17 @@ def test_solver(): X2 = x2u * d2_corr[:, None] err_R = np.linalg.norm(R - R_est) - err_t = np.linalg.norm( - t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) - ) - print( - f"posescaleoffsetsfs, residual={err_a}, rotation={err_R}, translation={err_t}" - ) + err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) + print(f"posescaleoffsetsfs, residual={err_a}, rotation={err_R}, translation={err_t}") print(len(sols), len(sols_madpose)) for k, (a1, b1, a2, b2, f) in enumerate(sols + sols_madpose): - err = np.abs(a2 - a2_gt / a1_gt) + np.abs(b1 - b1_gt / a1_gt) + np.abs(b2 - b2_gt / a1_gt) + np.abs(f - f_gt) + err = ( + np.abs(a2 - a2_gt / a1_gt) + + np.abs(b1 - b1_gt / a1_gt) + + np.abs(b2 - b2_gt / a1_gt) + + np.abs(f - f_gt) + ) focal_err = np.abs(f - f_gt) print(f"focal_err={focal_err}") @@ -963,12 +387,8 @@ def test_solver(): R_est, t_est = find_transform(X1[0:4, :], X2[0:4, :]) err_R = np.linalg.norm(R - R_est) - err_t = np.linalg.norm( - t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) - ) - print( - f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}" - ) + err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) + print(f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}") if __name__ == "__main__": diff --git a/solver_py/scale_and_shift_two_focal.py b/solver_py/scale_and_shift_two_focal.py index fd74018..d46485f 100644 --- a/solver_py/scale_and_shift_two_focal.py +++ b/solver_py/scale_and_shift_two_focal.py @@ -1,6 +1,7 @@ -import madpose import numpy as np +import madpose + def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): # Estimates scale, shift, and two focal lengths @@ -295,656 +296,18 @@ def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): ) coeffs[39] = (d1[1] ** 2) - 2 * d1[1] * d1[3] + (d1[3] ** 2) + # fmt: off # Setup expanded equation system - coeff_ind0 = [ - 0, - 8, - 16, - 24, - 32, - 0, - 8, - 16, - 24, - 32, - 1, - 9, - 17, - 25, - 33, - 2, - 10, - 0, - 8, - 16, - 18, - 24, - 26, - 32, - 34, - 0, - 8, - 16, - 24, - 32, - 1, - 9, - 17, - 25, - 33, - 2, - 10, - 8, - 0, - 16, - 18, - 24, - 26, - 32, - 34, - 8, - 0, - 16, - 24, - 32, - 1, - 9, - 17, - 25, - 33, - 3, - 11, - 1, - 9, - 17, - 19, - 25, - 27, - 33, - 35, - 4, - 12, - 2, - 10, - 18, - 20, - 26, - 28, - 34, - 36, - 2, - 10, - 8, - 16, - 18, - 0, - 26, - 24, - 32, - 34, - 9, - 1, - 17, - 25, - 33, - 3, - 11, - 9, - 1, - 19, - 17, - 27, - 25, - 33, - 35, - 5, - 4, - 13, - 12, - 10, - 2, - 18, - 21, - 20, - 26, - 29, - 28, - 34, - 36, - 37, - 10, - 2, - 8, - 0, - 18, - 24, - 26, - 32, - 34, - 16, - 3, - 11, - 19, - 9, - 17, - 1, - 27, - 25, - 33, - 35, - 6, - 14, - 3, - 11, - 19, - 22, - 27, - 30, - 35, - 38, - 4, - 12, - 20, - 28, - 36, - 4, - 12, - 10, - 18, - 20, - 2, - 28, - 26, - 34, - 36, - 5, - 13, - 21, - 29, - 37, - 11, - 3, - 19, - 9, - 1, - 27, - 25, - 33, - 17, - 35, - 6, - 14, - 11, - 3, - 22, - 19, - 30, - 27, - 35, - 38, - 5, - 12, - 13, - 21, - 4, - 20, - 28, - 29, - 36, - 37, - 5, - 12, - 13, - 4, - 10, - 21, - 2, - 20, - 26, - 29, - 28, - 34, - 36, - 37, - 18, - 7, - 15, - 23, - 31, - 39, - 6, - 14, - 22, - 11, - 19, - 3, - 30, - 27, - 35, - 38, - 6, - 14, - 22, - 30, - 38, - 12, - 20, - 4, - 28, - 36, - 13, - 5, - 21, - 29, - 37, - 13, - 5, - 21, - 29, - 37, - 14, - 6, - 22, - 30, - 38, - 13, - 12, - 21, - 5, - 4, - 28, - 29, - 37, - 36, - 20, - 7, - 15, - 23, - 31, - 39, - 14, - 22, - 6, - 30, - 38, - 13, - 5, - 29, - 37, - 21, - 15, - 7, - 23, - 31, - 39, - 7, - 15, - 23, - 31, - 39, - 14, - 6, - 22, - 11, - 3, - 30, - 27, - 35, - 19, - 38, - 7, - 15, - 23, - 31, - 39, - ] - coeff_ind1 = [ - 15, - 7, - 31, - 39, - 23, - 15, - 7, - 23, - 31, - 39, - 14, - 6, - 30, - 38, - 22, - 15, - 23, - 7, - 31, - 39, - ] - ind0 = [ - 0, - 2, - 18, - 28, - 39, - 41, - 45, - 60, - 69, - 78, - 80, - 82, - 98, - 108, - 119, - 120, - 122, - 123, - 128, - 130, - 138, - 145, - 148, - 157, - 159, - 164, - 169, - 177, - 186, - 194, - 201, - 205, - 220, - 229, - 238, - 241, - 245, - 246, - 252, - 254, - 260, - 263, - 269, - 276, - 278, - 287, - 293, - 302, - 310, - 313, - 323, - 328, - 330, - 345, - 357, - 360, - 362, - 364, - 369, - 377, - 378, - 386, - 388, - 394, - 399, - 400, - 402, - 403, - 408, - 410, - 418, - 425, - 428, - 437, - 439, - 444, - 449, - 451, - 456, - 457, - 459, - 466, - 467, - 471, - 474, - 486, - 492, - 494, - 503, - 516, - 521, - 525, - 527, - 533, - 540, - 542, - 549, - 550, - 553, - 558, - 560, - 561, - 562, - 565, - 566, - 572, - 574, - 578, - 580, - 583, - 588, - 589, - 596, - 598, - 599, - 607, - 613, - 615, - 621, - 622, - 624, - 630, - 632, - 633, - 635, - 643, - 648, - 650, - 651, - 656, - 659, - 665, - 667, - 671, - 677, - 680, - 682, - 684, - 689, - 697, - 698, - 706, - 708, - 714, - 719, - 723, - 728, - 730, - 745, - 757, - 764, - 769, - 771, - 776, - 777, - 779, - 786, - 787, - 791, - 794, - 801, - 805, - 820, - 829, - 838, - 846, - 852, - 854, - 855, - 861, - 863, - 864, - 872, - 875, - 876, - 881, - 885, - 887, - 893, - 900, - 902, - 909, - 910, - 913, - 918, - 923, - 926, - 928, - 930, - 932, - 934, - 943, - 945, - 956, - 957, - 964, - 967, - 969, - 973, - 975, - 977, - 981, - 982, - 984, - 986, - 990, - 992, - 993, - 994, - 995, - 1000, - 1002, - 1018, - 1028, - 1039, - 1043, - 1048, - 1050, - 1051, - 1056, - 1059, - 1065, - 1067, - 1071, - 1077, - 1084, - 1089, - 1097, - 1106, - 1114, - 1131, - 1136, - 1139, - 1147, - 1151, - 1166, - 1172, - 1174, - 1183, - 1196, - 1207, - 1213, - 1222, - 1230, - 1233, - 1247, - 1253, - 1262, - 1270, - 1273, - 1291, - 1295, - 1296, - 1299, - 1301, - 1304, - 1307, - 1311, - 1312, - 1315, - 1324, - 1329, - 1337, - 1346, - 1354, - 1371, - 1376, - 1379, - 1387, - 1391, - 1415, - 1421, - 1424, - 1432, - 1435, - 1446, - 1452, - 1454, - 1463, - 1476, - 1481, - 1485, - 1500, - 1509, - 1518, - 1526, - 1532, - 1534, - 1535, - 1541, - 1543, - 1544, - 1552, - 1555, - 1556, - 1563, - 1568, - 1570, - 1585, - 1597, - ] - ind1 = [ - 15, - 21, - 24, - 32, - 35, - 47, - 53, - 62, - 70, - 73, - 95, - 101, - 104, - 112, - 115, - 131, - 136, - 139, - 147, - 151, - ] - + coeff_ind0 = [0, 8, 16, 24, 32, 0, 8, 16, 24, 32, 1, 9, 17, 25, 33, 2, 10, 0, 8, 16, 18, 24, 26, 32, 34, 0, 8, 16, 24, 32, 1, 9, 17, 25, 33, 2, 10, 8, 0, 16, 18, 24, 26, 32, 34, 8, 0, 16, 24, 32, 1, 9, 17, 25, 33, 3, 11, 1, 9, 17, 19, 25, 27, 33, 35, 4, 12, 2, 10, 18, 20, 26, 28, 34, 36, 2, 10, 8, 16, 18, 0, 26, 24, 32, 34, 9, 1, 17, 25, 33, 3, 11, 9, 1, 19, 17, 27, 25, 33, 35, 5, 4, 13, 12, 10, 2, 18, 21, 20, 26, 29, 28, 34, 36, 37, 10, 2, 8, 0, 18, 24, 26, 32, 34, 16, 3, 11, 19, 9, 17, 1, 27, 25, 33, 35, 6, 14, 3, 11, 19, 22, 27, 30, 35, 38, 4, 12, 20, 28, 36, + 4, 12, 10, 18, 20, 2, 28, 26, 34, 36, 5, 13, 21, 29, 37, 11, 3, 19, 9, 1, 27, 25, 33, 17, 35, 6, 14, 11, 3, 22, 19, 30, 27, 35, 38, 5, 12, 13, 21, 4, 20, 28, 29, 36, 37, 5, 12, 13, 4, 10, 21, 2, 20, 26, 29, 28, 34, 36, 37, 18, 7, 15, 23, 31, 39, 6, 14, 22, 11, 19, 3, 30, 27, 35, 38, 6, 14, 22, 30, 38, 12, 20, 4, 28, 36, 13, 5, 21, 29, 37, 13, 5, 21, 29, 37, 14, 6, 22, 30, 38, 13, 12, 21, 5, 4, 28, 29, 37, 36, 20, 7, 15, 23, 31, 39, 14, 22, 6, 30, 38, 13, 5, 29, 37, 21, 15, 7, 23, 31, 39, 7, 15, 23, 31, 39, 14, 6, 22, 11, 3, 30, 27, 35, 19, 38, 7, 15, 23, 31, 39,] + coeff_ind1 = [15, 7, 31, 39, 23, 15, 7, 23, 31, + 39, 14, 6, 30, 38, 22, 15, 23, 7, 31, 39,] + ind0 = [0, 2, 18, 28, 39, 41, 45, 60, 69, 78, 80, 82, 98, 108, 119, 120, 122, 123, 128, 130, 138, 145, 148, 157, 159, 164, 169, 177, 186, 194, 201, 205, 220, 229, 238, 241, 245, 246, 252, 254, 260, 263, 269, 276, 278, 287, 293, 302, 310, 313, 323, 328, 330, 345, 357, 360, 362, 364, 369, 377, 378, 386, 388, 394, 399, 400, 402, 403, 408, 410, 418, 425, 428, 437, 439, 444, 449, 451, 456, 457, 459, 466, 467, 471, 474, 486, 492, 494, 503, 516, 521, 525, 527, 533, 540, 542, 549, 550, 553, 558, 560, 561, 562, 565, 566, 572, 574, 578, 580, 583, 588, 589, 596, 598, 599, 607, 613, 615, 621, 622, 624, 630, 632, 633, 635, 643, 648, 650, 651, 656, 659, 665, 667, 671, 677, 680, 682, 684, 689, 697, 698, 706, 708, 714, 719, 723, 728, 730, 745, 757, 764, 769, 771, 776, 777, 779, 786, 787, 791, 794, + 801, 805, 820, 829, 838, 846, 852, 854, 855, 861, 863, 864, 872, 875, 876, 881, 885, 887, 893, 900, 902, 909, 910, 913, 918, 923, 926, 928, 930, 932, 934, 943, 945, 956, 957, 964, 967, 969, 973, 975, 977, 981, 982, 984, 986, 990, 992, 993, 994, 995, 1000, 1002, 1018, 1028, 1039, 1043, 1048, 1050, 1051, 1056, 1059, 1065, 1067, 1071, 1077, 1084, 1089, 1097, 1106, 1114, 1131, 1136, 1139, 1147, 1151, 1166, 1172, 1174, 1183, 1196, 1207, 1213, 1222, 1230, 1233, 1247, 1253, 1262, 1270, 1273, 1291, 1295, 1296, 1299, 1301, 1304, 1307, 1311, 1312, 1315, 1324, 1329, 1337, 1346, 1354, 1371, 1376, 1379, 1387, 1391, 1415, 1421, 1424, 1432, 1435, 1446, 1452, 1454, 1463, 1476, 1481, 1485, 1500, 1509, 1518, 1526, 1532, 1534, 1535, 1541, 1543, 1544, 1552, 1555, 1556, 1563, 1568, 1570, 1585, 1597,] + ind1 = [15, 21, 24, 32, 35, 47, 53, 62, 70, 73, 95, + 101, 104, 112, 115, 131, 136, 139, 147, 151,] + + # fmt: on C0 = np.zeros((40, 40)) C1 = np.zeros((40, 4)) C0[np.unravel_index(ind0, (40, 40), "F")] = coeffs[coeff_ind0] @@ -1036,17 +399,21 @@ def test_solver(): sols = solve_shift_and_scale_two_focal(x1, x2, d1, d2) sols_madpose = madpose.solve_scale_and_shift_two_focal(x1.T, x2.T, d1, d2) - posescaleoffsettfs = madpose.solve_scale_shift_pose_two_focal(x1.T, x2.T, d1, d2) + # poses = madpose.solve_scale_shift_pose_two_focal(x1.T, x2.T, d1, d2) print(len(sols), len(sols_madpose)) for k, (a1, b1, a2, b2, f1, f2) in enumerate(sols + sols_madpose): - err = np.abs(a2 - a2_gt / a1_gt) + np.abs(b1 - b1_gt / a1_gt) + np.abs(b2 - b2_gt / a1_gt) + np.abs(f1 - f1_gt) + np.abs(f2 - f2_gt) + err = ( + np.abs(a2 - a2_gt / a1_gt) + + np.abs(b1 - b1_gt / a1_gt) + + np.abs(b2 - b2_gt / a1_gt) + + np.abs(f1 - f1_gt) + + np.abs(f2 - f2_gt) + ) focal_err = np.abs(f1 - f1_gt) + np.abs(f2 - f2_gt) print(f"focal_error={focal_err}") - t_skew = np.array( - [[0, -t[2], t[1]], [t[2], 0, -t[0]], [-t[1], t[0], 0]] - ) + t_skew = np.array([[0, -t[2], t[1]], [t[2], 0, -t[0]], [-t[1], t[0], 0]]) E = t_skew @ R K2_ = np.array([[f2, 0, 0], [0, f2, 0], [0, 0, 1]]) K1_ = np.array([[f1, 0, 0], [0, f1, 0], [0, 0, 1]]) @@ -1083,12 +450,8 @@ def test_solver(): print(f"F_error={F_error}") err_R = np.linalg.norm(R - R_est) - err_t = np.linalg.norm( - t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est) - ) - print( - f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}" - ) + err_t = np.linalg.norm(t / np.linalg.norm(t) - t_est / np.linalg.norm(t_est)) + print(f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}") if __name__ == "__main__": From 9aa8a62f63fc7b8383809f14f5b0b340693deb14 Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Thu, 9 Jan 2025 20:44:04 +0100 Subject: [PATCH 9/9] format C++ changes --- src/bindings.cpp | 7 +++---- src/solver.cpp | 31 +++++++++++++++---------------- src/solver.h | 32 ++++++++++++++++---------------- 3 files changed, 34 insertions(+), 36 deletions(-) diff --git a/src/bindings.cpp b/src/bindings.cpp index 25a4ad7..adc6637 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -160,10 +160,9 @@ void bind_estimator(py::module &m) { "depth_x"_a, "depth_y"_a); m.def("solve_scale_and_shift_two_focal", &solve_scale_and_shift_two_focal, "x_homo"_a, "y_homo"_a, "depth_x"_a, "depth_y"_a); - m.def("solve_scale_shift_pose", &solve_scale_shift_pose_wrapper, "x_homo"_a, "y_homo"_a, "depth_x"_a, - "depth_y"_a); - m.def("solve_scale_shift_pose_shared_focal", &solve_scale_shift_pose_shared_focal_wrapper, "x_homo"_a, - "y_homo"_a, "depth_x"_a, "depth_y"_a); + m.def("solve_scale_shift_pose", &solve_scale_shift_pose_wrapper, "x_homo"_a, "y_homo"_a, "depth_x"_a, "depth_y"_a); + m.def("solve_scale_shift_pose_shared_focal", &solve_scale_shift_pose_shared_focal_wrapper, "x_homo"_a, "y_homo"_a, + "depth_x"_a, "depth_y"_a); m.def("solve_scale_shift_pose_two_focal", &solve_scale_shift_pose_two_focal_wrapper, "x_homo"_a, "y_homo"_a, "depth_x"_a, "depth_y"_a); m.def("HybridEstimatePoseAndScale", &HybridEstimatePoseAndScale, "x0"_a, "x1"_a, "depth0"_a, "depth1"_a, "K0"_a, diff --git a/src/solver.cpp b/src/solver.cpp index 91f1048..8eb4405 100644 --- a/src/solver.cpp +++ b/src/solver.cpp @@ -479,9 +479,8 @@ std::vector> solve_scale_and_shift_two_focal(const Eige return solutions; } -int solve_scale_shift_pose(const Eigen::Matrix3d &x_homo, const Eigen::Matrix3d &y_homo, - const Eigen::Vector3d &depth_x, const Eigen::Vector3d &depth_y, - std::vector *output, bool scale_on_x) { +int solve_scale_shift_pose(const Eigen::Matrix3d &x_homo, const Eigen::Matrix3d &y_homo, const Eigen::Vector3d &depth_x, + const Eigen::Vector3d &depth_y, std::vector *output, bool scale_on_x) { // X: 3 x 3, column vectors are homogeneous 2D points // Y: 3 x 3, column vectors are homogeneous 2D points std::vector solutions; @@ -535,8 +534,8 @@ int solve_scale_shift_pose(const Eigen::Matrix3d &x_homo, const Eigen::Matrix3d } int solve_scale_shift_pose_shared_focal(const Eigen::Matrix3x4d &x_homo, const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, - std::vector *output, bool scale_on_x) { + const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, + std::vector *output, bool scale_on_x) { std::vector> solutions; if (scale_on_x) solutions = solve_scale_and_shift_shared_focal(y_homo, x_homo, depth_y, depth_x); @@ -594,8 +593,8 @@ int solve_scale_shift_pose_shared_focal(const Eigen::Matrix3x4d &x_homo, const E } int solve_scale_shift_pose_two_focal(const Eigen::Matrix3x4d &x_homo, const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, - std::vector *output, bool scale_on_x) { + const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, + std::vector *output, bool scale_on_x) { std::vector> solutions; if (scale_on_x) solutions = solve_scale_and_shift_two_focal(y_homo, x_homo, depth_y, depth_x); @@ -653,27 +652,27 @@ int solve_scale_shift_pose_two_focal(const Eigen::Matrix3x4d &x_homo, const Eige } std::vector solve_scale_shift_pose_wrapper(const Eigen::Matrix3d &x_homo, - const Eigen::Matrix3d &y_homo, - const Eigen::Vector3d &depth_x, - const Eigen::Vector3d &depth_y) { + const Eigen::Matrix3d &y_homo, + const Eigen::Vector3d &depth_x, + const Eigen::Vector3d &depth_y) { std::vector output; int sol_num = solve_scale_shift_pose(x_homo, y_homo, depth_x, depth_y, &output); return output; } std::vector solve_scale_shift_pose_shared_focal_wrapper(const Eigen::Matrix3x4d &x_homo, - const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, - const Eigen::Vector4d &depth_y) { + const Eigen::Matrix3x4d &y_homo, + const Eigen::Vector4d &depth_x, + const Eigen::Vector4d &depth_y) { std::vector output; int sol_num = solve_scale_shift_pose_shared_focal(x_homo, y_homo, depth_x, depth_y, &output); return output; } std::vector solve_scale_shift_pose_two_focal_wrapper(const Eigen::Matrix3x4d &x_homo, - const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, - const Eigen::Vector4d &depth_y) { + const Eigen::Matrix3x4d &y_homo, + const Eigen::Vector4d &depth_x, + const Eigen::Vector4d &depth_y) { std::vector output; int sol_num = solve_scale_shift_pose_two_focal(x_homo, y_homo, depth_x, depth_y, &output); return output; diff --git a/src/solver.h b/src/solver.h index 098fb77..0ab23b2 100644 --- a/src/solver.h +++ b/src/solver.h @@ -27,31 +27,31 @@ std::vector> solve_scale_and_shift_two_focal(const Eige const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y); -int solve_scale_shift_pose(const Eigen::Matrix3d &x_homo, const Eigen::Matrix3d &y_homo, - const Eigen::Vector3d &depth_x, const Eigen::Vector3d &depth_y, - std::vector *output, bool scale_on_x = false); +int solve_scale_shift_pose(const Eigen::Matrix3d &x_homo, const Eigen::Matrix3d &y_homo, const Eigen::Vector3d &depth_x, + const Eigen::Vector3d &depth_y, std::vector *output, + bool scale_on_x = false); int solve_scale_shift_pose_shared_focal(const Eigen::Matrix3x4d &x_homo, const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, - std::vector *output, bool scale_on_x = false); + const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, + std::vector *output, bool scale_on_x = false); int solve_scale_shift_pose_two_focal(const Eigen::Matrix3x4d &x_homo, const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, - std::vector *output, bool scale_on_x = false); + const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, + std::vector *output, bool scale_on_x = false); std::vector solve_scale_shift_pose_wrapper(const Eigen::Matrix3d &x_homo, - const Eigen::Matrix3d &y_homo, - const Eigen::Vector3d &depth_x, - const Eigen::Vector3d &depth_y); + const Eigen::Matrix3d &y_homo, + const Eigen::Vector3d &depth_x, + const Eigen::Vector3d &depth_y); std::vector solve_scale_shift_pose_shared_focal_wrapper(const Eigen::Matrix3x4d &x_homo, - const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, - const Eigen::Vector4d &depth_y); + const Eigen::Matrix3x4d &y_homo, + const Eigen::Vector4d &depth_x, + const Eigen::Vector4d &depth_y); std::vector solve_scale_shift_pose_two_focal_wrapper(const Eigen::Matrix3x4d &x_homo, - const Eigen::Matrix3x4d &y_homo, - const Eigen::Vector4d &depth_x, - const Eigen::Vector4d &depth_y); + const Eigen::Matrix3x4d &y_homo, + const Eigen::Vector4d &depth_x, + const Eigen::Vector4d &depth_y); } // namespace madpose