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) 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 new file mode 100644 index 0000000..7267c7d --- /dev/null +++ b/ruff.toml @@ -0,0 +1,18 @@ +line-length = 100 + +[lint] +select = [ + # pycodestyle + "E", + # Pyflakes + "F", + # pyupgrade + "UP", + # flake8-bugbear + "B", + # flake8-simplify + "SIM", + # isort + "I", +] +ignore = ["E501"] diff --git a/scripts/clang_format.sh b/scripts/clang_format.sh deleted file mode 100644 index 7fd6696..0000000 --- a/scripts/clang_format.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env bash - -# Get all C++ files checked into the repo under src/ -root_folder=$(git rev-parse --show-toplevel) -all_files=$( \ - git ls-tree --full-tree -r --name-only HEAD . \ - | grep "src/.*\(\.cc\|\.h\|\.hpp\|\.cpp\|\.cu\)$" \ - | sed "s~^~$root_folder/~") -num_files=$(echo $all_files | wc -w) -echo "Formatting ${num_files} files" - -cd $root_folder -clang-format -i --style=file $all_files -cd - \ No newline at end of file diff --git a/scripts/format/clang_format.sh b/scripts/format/clang_format.sh new file mode 100755 index 0000000..65dc27f --- /dev/null +++ b/scripts/format/clang_format.sh @@ -0,0 +1,44 @@ +#!/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=$( \ + git ls-tree --full-tree -r --name-only HEAD . \ + | grep "src/.*\(\.cc\|\.h\|\.hpp\|\.cpp\|\.cu\)$" \ + | sed "s~^~$root_folder/~") +num_files=$(echo $all_files | wc -w) +echo "Formatting ${num_files} files" + +cd $root_folder +clang-format -i --style=file $all_files +cd - diff --git a/scripts/format/python.sh b/scripts/format/python.sh new file mode 100755 index 0000000..7dd575c --- /dev/null +++ b/scripts/format/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..da0a005 100644 --- a/setup.py +++ b/setup.py @@ -82,9 +82,7 @@ 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"): @@ -95,23 +93,20 @@ 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(): 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 @@ -121,11 +116,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 5ece820..69d0b41 100644 --- a/solver_py/scale_and_shift.py +++ b/solver_py/scale_and_shift.py @@ -1,6 +1,8 @@ import numpy as np + import madpose + def solve_shift_and_scale(x1, x2, d1, d2): # Estimates scale and shift # x1 should be three image points, i.e. x1[0] = [x,y,1] @@ -12,25 +14,74 @@ 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]) + ) + + # 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] @@ -38,10 +89,12 @@ def solve_shift_and_scale(x1, x2, d1, d2): 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] - 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 +123,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,7 +170,7 @@ def test_solver(): 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}') + 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) @@ -131,10 +183,10 @@ def test_solver(): R_est, t_est = find_transform(X1, X2) - err_R = np.linalg.norm(R-R_est) + 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}') - + 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/scale_and_shift_shared_focal.py b/solver_py/scale_and_shift_shared_focal.py index 33b69e0..efbeddf 100644 --- a/solver_py/scale_and_shift_shared_focal.py +++ b/solver_py/scale_and_shift_shared_focal.py @@ -1,6 +1,8 @@ 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 # x1 should be four image points, i.e. x1[0] = [x,y,1] @@ -24,39 +26,224 @@ 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) + + # 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] @@ -67,22 +254,32 @@ def solve_shift_and_scale_shared_focal(x1_, x2_, d1, d2): 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] - 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 +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 @@ -104,13 +300,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,))] @@ -164,15 +359,20 @@ def test_solver(): X1 = x1u * d1_corr[:, None] X2 = x2u * d2_corr[:, None] - err_R = np.linalg.norm(R-R_est) + 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(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}') + print(f"focal_err={focal_err}") d1_corr = a1 * d1 + b1 d2_corr = a2 * d2 + b2 @@ -186,10 +386,10 @@ 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_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}') + 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/scale_and_shift_two_focal.py b/solver_py/scale_and_shift_two_focal.py index 7d28565..d46485f 100644 --- a/solver_py/scale_and_shift_two_focal.py +++ b/solver_py/scale_and_shift_two_focal.py @@ -1,6 +1,8 @@ import numpy as np + import madpose + def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): # Estimates scale, shift, and two focal lengths # x1 should be four image points, i.e. x1[0] = [x,y,1] @@ -11,7 +13,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,47 +25,278 @@ 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) + # 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,] @@ -74,10 +307,11 @@ def solve_shift_and_scale_two_focal(x1_, x2_, d1, d2): 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] - 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 +321,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 +339,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 +359,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,13 +399,19 @@ 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}') + 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]]) E = t_skew @ R @@ -176,7 +432,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 +447,12 @@ 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}') + print(f"solution={k}, residual={err}, rotation={err_R}, translation={err_t}") -if __name__ == '__main__': +if __name__ == "__main__": test_solver() 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