diff --git a/README.md b/README.md
index 57292788f49a6f4dd5b3cf99ea545d0f003acaba..33ad49c6219bb9f51e70994535638fd4d4b48c83 100644
--- a/README.md
+++ b/README.md
@@ -1,17 +1,6 @@
# Aruco Frame
-
-
-## Getting started
-
-To make it easy for you to get started with GitLab, here's a list of recommended next steps.
-
-Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)!
-
-## Add your files
-
-- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files
-- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command:
+## Usage
```
cd existing_repo
@@ -20,74 +9,10 @@ git branch -M main
git push -uf origin main
```
-## Integrate with your tools
-
-- [ ] [Set up project integrations](https://gitlab.cba.mit.edu/quentinbolsee/aruco-frame/-/settings/integrations)
-
-## Collaborate with your team
-
-- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/)
-- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html)
-- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically)
-- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/)
-- [ ] [Set auto-merge](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html)
-
-## Test and Deploy
-
-Use the built-in continuous integration in GitLab.
-
-- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html)
-- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing (SAST)](https://docs.gitlab.com/ee/user/application_security/sast/)
-- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html)
-- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/)
-- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html)
-
-***
-
-# Editing this README
-
-When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thanks to [makeareadme.com](https://www.makeareadme.com/) for this template.
-
-## Suggestions for a good README
-
-Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information.
-
-## Name
-Choose a self-explaining name for your project.
-
-## Description
-Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors.
-
-## Badges
-On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge.
-
-## Visuals
-Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method.
-
-## Installation
-Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection.
-
-## Usage
-Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README.
-
-## Support
-Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc.
-
-## Roadmap
-If you have ideas for releases in the future, it is a good idea to list them in the README.
-
-## Contributing
-State if you are open to contributions and what your requirements are for accepting them.
-
-For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self.
-
-You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser.
-
## Authors and acknowledgment
-Show your appreciation to those who have contributed to the project.
+
+Quentin Bolsee 2024
## License
-For open source projects, say how it is licensed.
-## Project status
-If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers.
+This project is provided under the MIT License.
diff --git a/aruco-frame.py b/aruco-frame.py
new file mode 100644
index 0000000000000000000000000000000000000000..47daae3eef2509093e323ff28bd26ae64a56bcd0
--- /dev/null
+++ b/aruco-frame.py
@@ -0,0 +1,325 @@
+import datetime
+import os
+import json
+import sys
+import argparse
+
+import cv2
+import numpy as np
+
+import utils
+import solve_lens
+
+
+def parse_arguments():
+ usage_text = (
+ "Usage: python aruco-frame.py [options]"
+ )
+ parser = argparse.ArgumentParser(description=usage_text)
+ parser.add_argument("-i", "--input", type=str,
+ help="Input filename.")
+ parser.add_argument("-o", "--output", type=str, default="",
+ help="Output filename (default: <filename_in>_extracted.png).")
+ parser.add_argument("-d", "--dpi", type=int, default=-1,
+ help="Manual output DPI (default: auto).")
+ parser.add_argument("-s", "--show", action="store_true",
+ help="Show debug image.")
+ parser.add_argument("-c", "--config", type=str, default="./config/config.json",
+ help="Frame configuration file (default: ./config/config.json).")
+ parser.add_argument("-v", "--verbose", action="store_true",
+ help="Verbose mode (default: false).")
+ return parser.parse_args()
+
+
+def imshow(img, h_view=700, win_name="debug"):
+ h, w = img.shape[:2]
+ w_view = int(h_view * w / h)
+ cv2.imshow(win_name, cv2.resize(img, (w_view, h_view), interpolation=cv2.INTER_AREA))
+ cv2.waitKey(0)
+
+
+def extract_image(img, proj, config, dots_per_mm, dist_params=None):
+ h, w, c = img.shape
+
+ m = config["margins"]["inner_content"]
+ xmin = m
+ xmax = config["width"] - m
+ ymin = m
+ ymax = config["height"] - m
+
+ h_out = int(dots_per_mm * (ymax - ymin))
+ w_out = int(dots_per_mm * (xmax - xmin))
+
+ x = np.linspace(xmin, xmax, w_out)
+ y = np.linspace(ymax, ymin, h_out)
+ xx, yy = np.meshgrid(x, y)
+
+ xy_list = np.ones((h_out * w_out, 2))
+ xy_list[:, 0] = xx.flatten()
+ xy_list[:, 1] = yy.flatten()
+ uv_src = apply_affine(proj, xy_list)
+
+ if dist_params is not None:
+ k1, k2, uc, vc = dist_params[:]
+ mat = np.array([[w, 0, uc], [0, w, vc], [0, 0, 1]], dtype=np.float32)
+ dist_coeffs = np.array([[0, 0, 0, 0, 0, k1, k2, 0]], dtype=np.float32)
+ out = cv2.undistortPoints(uv_src, mat, dist_coeffs, P=mat)
+ uv_src = out[:, 0, :]
+
+ map1 = uv_src[:, 0].reshape((h_out, w_out)).astype(np.float32)
+ map2 = uv_src[:, 1].reshape((h_out, w_out)).astype(np.float32)
+
+ # plt.figure()
+ # plt.imshow(map1)
+ # plt.figure()
+ # plt.imshow(map2)
+ # plt.show()
+
+ img_out = cv2.remap(img, map1, map2, interpolation=cv2.INTER_CUBIC)
+
+ return img_out
+
+
+def find_aruco(img):
+ aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+ params = cv2.aruco.DetectorParameters()
+ params.adaptiveThreshWinSizeMax = 40
+ params.useAruco3Detection = True
+ corners, ids, rejected = cv2.aruco.detectMarkers(img, dictionary=aruco_dict, parameters=params)
+ if ids is None:
+ return {}
+ else:
+ corners_dict = {ids[k][0]: corners[k][0, :, :] for k in range(len(ids))}
+ return corners_dict
+
+
+def identify_frame(img, config_frames, debug=False):
+ corners_dict = find_aruco(img)
+
+ if debug:
+ img_view = np.copy(img)
+ for c in corners_dict.values():
+ for uv in c[:, :]:
+ cv2.circle(img_view, uv.astype(np.int32), radius=30, color=(0, 0, 255), thickness=cv2.FILLED)
+
+ imshow(img_view)
+
+ name_found = None
+ for name in config_frames:
+ match = True
+ for aruco_id in config_frames[name]["aruco_id"]:
+ if aruco_id not in corners_dict:
+ match = False
+ break
+ if match:
+ name_found = name
+ break
+ return name_found
+
+
+def get_aruco_features(img, config):
+ corners_dict_all = find_aruco(img)
+ corners_dict = {k: corners_dict_all[k] for k in config["aruco_id"]}
+ centers_dict = {k: np.mean(corners_dict[k], axis=0) for k in corners_dict}
+
+ xy_array = np.zeros((4, 2))
+ uv_array = np.zeros((4, 2))
+
+ for i in range(4):
+ xy_array[i, :] = config["aruco_pos"][i]
+ uv_array[i, :] = centers_dict[config["aruco_id"][i]]
+
+ return xy_array, uv_array
+
+
+def apply_affine(a, xy):
+ n = len(xy)
+ xyz = np.ones((n, 3))
+ xyz[:, :2] = xy
+ uvw = xyz @ a.T
+ uv = uvw[:, :2] / uvw[:, 2:]
+ return uv
+
+
+def get_corner_features(img_gray, proj, config):
+ n_points = sum(len(edge) for edge in config["corner_pos"])
+
+ xy_feats = np.zeros((n_points, 2))
+ uv_feats_approx = np.zeros((n_points, 2))
+
+ k = 0
+ for edge in config["corner_pos"]:
+ n_edge = len(edge)
+ xy_feats[k:k + n_edge, :] = np.array(edge)
+ uv_feats_approx[k:k + n_edge] = apply_affine(proj, xy_feats[k:k + n_edge, :])
+ k += n_edge
+
+ # adjust search region to resolution
+ search_mm = 0.7 * config["corner_size"] / 2
+
+ cross_xy = np.zeros((4 * n_points, 2), dtype=np.float32)
+ cross_xy[0::4, :] = xy_feats - np.array([search_mm, 0])
+ cross_xy[1::4, :] = xy_feats + np.array([search_mm, 0])
+ cross_xy[2::4, :] = xy_feats - np.array([0, search_mm])
+ cross_xy[3::4, :] = xy_feats + np.array([0, search_mm])
+
+ cross_uv = apply_affine(proj, cross_xy)
+ cross_uv_r = cross_uv.reshape(n_points, 4, 2)
+
+ span_uv = (np.max(cross_uv_r, axis=1) - np.min(cross_uv_r, axis=1)) / 2
+ search_uv = np.mean(span_uv, axis=0).astype(np.int32)
+ # print(search_uv)
+
+ criteria = (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 40, 0.001)
+ ret = cv2.cornerSubPix(img_gray,
+ uv_feats_approx[:, np.newaxis, :].astype(np.float32),
+ (search_uv[0], search_uv[1]),
+ (-1, -1),
+ criteria)
+ uv_feats = ret[:, 0, :]
+ # return xy_feats, uv_feats_approx
+
+ return xy_feats, uv_feats
+
+
+def get_dots_per_mm(xy, uv, use_max=True):
+ xy_dist = np.zeros((4,))
+ uv_dist = np.zeros((4,))
+ for i in range(-1, 3):
+ xy_dist[i] = np.linalg.norm(xy[i + 1] - xy[i])
+ uv_dist[i] = np.linalg.norm(uv[i + 1] - uv[i])
+ if use_max:
+ return np.max(uv_dist / xy_dist)
+ else:
+ return np.mean(uv_dist / xy_dist)
+
+
+def process_image(img, config_frames, solve_dist=False, view=False, view_radius=16, verbose=False, dpi=None):
+ h, w = img.shape[:2]
+
+ if len(img.shape) == 2:
+ img_gray = img
+ img_rgb = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
+ else:
+ img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ img_rgb = img
+
+ frame_name = identify_frame(img_rgb, config_frames)
+
+ if verbose:
+ print(f"frame found: '{frame_name}'")
+
+ if frame_name is None:
+ raise RuntimeError("No frame found!")
+
+ config = config_frames[frame_name]
+
+ xy_a, uv_a = get_aruco_features(img_rgb, config)
+ proj = utils.solve_affine(xy_a, uv_a)
+
+ if dpi is None:
+ dpi = int(get_dots_per_mm(xy_a, uv_a) * 25.4)
+ dots_per_mm = dpi / 25.4
+
+ if verbose:
+ print(f"DPI: {dpi}")
+
+ xy_c, uv_c = get_corner_features(img_gray, proj, config)
+
+ proj_fine = utils.solve_affine(xy_c, uv_c)
+ err1 = solve_lens.xy_error(xy_c, uv_c, proj)
+ err2 = solve_lens.xy_error(xy_c, uv_c, proj_fine)
+ if verbose:
+ print(f"Error init. : {np.mean(np.linalg.norm(err1, axis=1)):.3f} mm")
+ print(f"Error refined : {np.mean(np.linalg.norm(err2, axis=1)):.3f} mm")
+
+ if view:
+ img_view = np.copy(img_rgb)
+ for uv in uv_a:
+ cv2.circle(img_view, uv.astype(np.int32), radius=view_radius, color=(255, 200, 0), thickness=cv2.FILLED)
+ for uv in uv_c:
+ cv2.circle(img_view, uv.astype(np.int32), radius=view_radius, color=(0, 0, 255), thickness=cv2.FILLED)
+ imshow(img_view, win_name="features")
+ # cv2.imwrite("view.jpg", img_view)
+
+ if solve_dist:
+ params = solve_lens.solve_distortion(xy_c, uv_c, proj_fine, w, w, h)
+
+ for i in range(4):
+ uv_u = solve_lens.undistort(params, uv_c, w)
+ proj_fine = utils.solve_affine(xy_c, uv_u)
+ params = solve_lens.solve_distortion(xy_c, uv_c, proj_fine, w, w, h)
+
+ uv_u = solve_lens.undistort(params, uv_c, w)
+ err3 = solve_lens.xy_error(xy_c, uv_u, proj_fine)
+ if verbose:
+ print(f"Error lens dist.: {np.mean(np.linalg.norm(err3, axis=1)):.3f} mm")
+
+ img_out = extract_image(img_rgb, proj_fine, config, dots_per_mm, dist_params=params)
+ else:
+ img_out = extract_image(img_rgb, proj_fine, config, dots_per_mm)
+
+ if view:
+ imshow(img_out, win_name="out")
+
+ # handle upside down case
+ if uv_a[0][1] < uv_a[2][1]:
+ img_out = cv2.rotate(img_out, cv2.ROTATE_180)
+
+ h_out, w_out, _ = img_out.shape
+
+ if verbose:
+ print(f"Dots per mm: {dots_per_mm:.2f}")
+ print(f"Dots per in: {dpi}")
+ print(f"Resolution: {w_out} x {h_out}")
+
+ return img_out, dpi
+
+
+def load_config_frames(filename):
+ head, tail = os.path.split(filename)
+
+ with open(filename, "r") as f:
+ config_all = json.load(f)
+
+ config = {}
+ for frame_name, frame_filename in config_all.items():
+ with open(os.path.join(head, frame_filename), "r") as f:
+ config[frame_name] = json.load(f)
+
+ return config
+
+
+def main():
+ args = parse_arguments()
+
+ filename_in = args.input
+
+ if args.output == "":
+ filename_out = os.path.splitext(filename_in)[0] + "_extracted.png"
+ else:
+ filename_out = args.output
+
+ head_out, _ = os.path.split(filename_out)
+ os.makedirs(head_out, exist_ok=True)
+
+ print(f"Processing: '{filename_in}' -> '{filename_out}'")
+
+ img = cv2.imread(filename_in, cv2.IMREAD_UNCHANGED)
+
+ config_frames = load_config_frames(args.config)
+
+ if args.dpi == -1:
+ img_out, dpi = process_image(img, config_frames,
+ solve_dist=True, view=args.show, verbose=args.verbose)
+ else:
+ img_out, dpi = process_image(img, config_frames,
+ solve_dist=True, view=args.show, verbose=args.verbose, dpi=args.dpi)
+
+ utils.writePNGwithdpi(filename_out, img_out, dpi=(dpi, dpi))
+
+ print("Done.")
+
+
+if __name__ == "__main__":
+ main()
diff --git a/config/config.json b/config/config.json
new file mode 100644
index 0000000000000000000000000000000000000000..d0a1b1a1664cfa302580233aa31312e110bc684c
--- /dev/null
+++ b/config/config.json
@@ -0,0 +1,5 @@
+{
+ "small": "./small.json",
+ "medium": "./medium.json",
+ "large": "./large.json"
+}
diff --git a/config/large.json b/config/large.json
new file mode 100644
index 0000000000000000000000000000000000000000..f865e920e08800e8db81a0f4e69754e18c0580c2
--- /dev/null
+++ b/config/large.json
@@ -0,0 +1,65 @@
+{
+ "width": 270,
+ "height": 350,
+ "aruco_id": [
+ 8,
+ 9,
+ 10,
+ 11
+ ],
+ "aruco_pos": [
+ [0, 0],
+ [270, 0],
+ [270, 350],
+ [0, 350]
+ ],
+ "aruco_size": 22,
+ "corner_size": 20,
+ "corner_pos": [
+ [
+ [45, 0],
+ [75, 0],
+ [105, 0],
+ [135, 0],
+ [165, 0],
+ [195, 0],
+ [225, 0]
+ ],
+ [
+ [270, 55],
+ [270, 85],
+ [270, 115],
+ [270, 145],
+ [270, 175],
+ [270, 205],
+ [270, 235],
+ [270, 265],
+ [270, 295]
+ ],
+ [
+ [225, 350],
+ [195, 350],
+ [165, 350],
+ [135, 350],
+ [105, 350],
+ [75, 350],
+ [45, 350]
+ ],
+ [
+ [0, 295],
+ [0, 265],
+ [0, 235],
+ [0, 205],
+ [0, 175],
+ [0, 145],
+ [0, 115],
+ [0, 85],
+ [0, 55]
+ ]
+ ],
+ "margins": {
+ "inner": 15,
+ "outer": 15,
+ "inner_content": 16
+ }
+}
diff --git a/config/medium.json b/config/medium.json
new file mode 100644
index 0000000000000000000000000000000000000000..1877e901fcbf2e0fe38bdd5fa27f31847f8bf2b3
--- /dev/null
+++ b/config/medium.json
@@ -0,0 +1,57 @@
+{
+ "width": 210,
+ "height": 290,
+ "aruco_id": [
+ 4,
+ 5,
+ 6,
+ 7
+ ],
+ "aruco_pos": [
+ [0, 0],
+ [210, 0],
+ [210, 290],
+ [0, 290]
+ ],
+ "aruco_size": 22,
+ "corner_size": 20,
+ "corner_pos": [
+ [
+ [45, 0],
+ [75, 0],
+ [105, 0],
+ [135, 0],
+ [165, 0]
+ ],
+ [
+ [210, 55],
+ [210, 85],
+ [210, 115],
+ [210, 145],
+ [210, 175],
+ [210, 205],
+ [210, 235]
+ ],
+ [
+ [165, 290],
+ [135, 290],
+ [105, 290],
+ [75, 290],
+ [45, 290]
+ ],
+ [
+ [0, 235],
+ [0, 205],
+ [0, 175],
+ [0, 145],
+ [0, 115],
+ [0, 85],
+ [0, 55]
+ ]
+ ],
+ "margins": {
+ "inner": 15,
+ "outer": 15,
+ "inner_content": 17
+ }
+}
diff --git a/config/small.json b/config/small.json
new file mode 100644
index 0000000000000000000000000000000000000000..04cc6440a5f4dcaa7dc3362f0e84edd55b84a229
--- /dev/null
+++ b/config/small.json
@@ -0,0 +1,61 @@
+{
+ "width": 150,
+ "height": 230,
+ "aruco_id": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "aruco_pos": [
+ [0, 0],
+ [150, 0],
+ [150, 230],
+ [0, 230]
+ ],
+ "aruco_size": 16,
+ "corner_size": 15,
+ "corner_pos": [
+ [
+ [35, 0],
+ [55, 0],
+ [75, 0],
+ [95, 0],
+ [115, 0]
+ ],
+ [
+ [150, 35],
+ [150, 55],
+ [150, 75],
+ [150, 95],
+ [150, 115],
+ [150, 135],
+ [150, 155],
+ [150, 175],
+ [150, 195]
+ ],
+ [
+ [115, 230],
+ [95, 230],
+ [75, 230],
+ [55, 230],
+ [35, 230]
+ ],
+ [
+ [0, 195],
+ [0, 175],
+ [0, 155],
+ [0, 135],
+ [0, 115],
+ [0, 95],
+ [0, 75],
+ [0, 55],
+ [0, 35]
+ ]
+ ],
+ "margins": {
+ "inner": 10,
+ "outer": 10,
+ "inner_content": 11
+ }
+}
diff --git a/example/raspi.jpg b/example/raspi.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..bfb43f6393df4261bebae4b88ca75843e137fc4a
Binary files /dev/null and b/example/raspi.jpg differ
diff --git a/example/raspi_extracted.png b/example/raspi_extracted.png
new file mode 100644
index 0000000000000000000000000000000000000000..acbf3a04fa9ea78cbedadf06b18df0357465778c
Binary files /dev/null and b/example/raspi_extracted.png differ
diff --git a/requirements.txt b/requirements.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b6cb49a13cefb4861109d3aaa9c91b713e2a6364
--- /dev/null
+++ b/requirements.txt
@@ -0,0 +1,3 @@
+numpy>=1.26.0
+opencv-contrib-python>=4.7.0.72
+scipy>=1.11.0
diff --git a/solve_lens.py b/solve_lens.py
new file mode 100644
index 0000000000000000000000000000000000000000..2e7f2e5a39cd626be1c85f7db9a706053adc309d
--- /dev/null
+++ b/solve_lens.py
@@ -0,0 +1,68 @@
+import numpy as np
+import scipy
+
+
+def undistort(params, uv, f):
+ k1, k2 = params[:2]
+ uv_c = params[2:4]
+ r2 = np.sum(np.square((uv - uv_c) / f), axis=1, keepdims=True)
+ coeff = 1 / (1 + k1 * r2 + k2 * r2 * r2)
+ # coeff = (1 + k1 * r2 + k2 * r2 * r2)
+
+ return uv_c + (uv - uv_c) * coeff
+
+
+def distort(params, uv, f, print_info=False):
+ n_points = len(uv)
+
+ err = lambda x: undistort(params, x, f) - uv
+ loss = lambda x: np.mean(np.sum(err(x.reshape(n_points, 2)) ** 2, axis=1))
+
+ x0 = uv.flatten()
+
+ result = scipy.optimize.minimize(loss, x0, method="COBYLA")
+
+ if print_info:
+ print(result)
+
+ uv_d = result.x.reshape(n_points, 2)
+ return uv_d
+
+
+def xy_error(xy, uv, P):
+ n_points = len(uv)
+
+ P_inv = np.linalg.inv(P)
+ P_inv /= P_inv[2, 2]
+
+ uv_ext = np.ones((n_points, 3))
+ uv_ext[:, :2] = uv
+ xy2_ext = P_inv @ uv_ext.T
+ xy2 = xy2_ext[:2, :].T / xy2_ext[2:, :].T
+
+ return xy2 - xy
+
+
+def xy_loss(params, xy, uv, proj, f, use_mae=False):
+ uv_u = undistort(params, uv, f)
+ err = xy_error(xy, uv_u, proj)
+ if use_mae:
+ return np.mean(np.linalg.norm(err, axis=1))
+ else:
+ return np.mean(np.sum(err * err, axis=1))
+
+
+def solve_distortion(xy, uv, proj, f, w, h, print_info=False):
+ x0 = np.array([0, 0, w / 2, h / 2])
+ result = scipy.optimize.minimize(xy_loss, x0, args=(xy, uv, proj, f))
+ xf = result.x
+
+ if print_info:
+ print("before:")
+ print(f"x={x0}")
+ print(f"loss={xy_loss(x0, uv, xy, proj, f)}")
+ print("after:")
+ print(f"x={xf}")
+ print(f"loss={xy_loss(xf, uv, xy, proj, f)}")
+
+ return xf
diff --git a/utils.py b/utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..066ec1dc6fd160e1e1d5fc331e1e5a355e83de45
--- /dev/null
+++ b/utils.py
@@ -0,0 +1,55 @@
+import cv2
+import zlib
+import struct
+import numpy as np
+
+
+def solve_affine(xy_array, uv_array):
+ if len(xy_array) < 4 or len(uv_array) != len(xy_array):
+ raise ValueError(f"Wrong input sizes: should be 4x2 and 4x2")
+ n_points = len(uv_array)
+ A = np.zeros((2 * n_points, 8))
+ b = np.zeros((2 * n_points,))
+ for i in range(n_points):
+ xy = xy_array[i, :]
+ uv = uv_array[i, :]
+
+ A[2 * i, 0:2] = xy
+ A[2 * i, 2] = 1
+ A[2 * i, 6:8] = -xy * uv[0]
+
+ A[2 * i + 1, 3:5] = xy
+ A[2 * i + 1, 5] = 1
+ A[2 * i + 1, 6:8] = -xy * uv[1]
+
+ b[2 * i] = uv[0]
+ b[2 * i + 1] = uv[1]
+
+ sol = np.ones((9,))
+ A_inv = np.linalg.pinv(A) if n_points > 4 else np.linalg.inv(A)
+ sol[:8] = A_inv @ b
+ return sol.reshape((3, 3))
+
+
+def writePNGwithdpi(filename, im, dpi=(72, 72)):
+ """Save the image as PNG with embedded dpi"""
+
+ # Encode as PNG into memory
+ retval, buffer = cv2.imencode(".png", im)
+ s = buffer.tobytes()
+
+ # Find start of IDAT chunk
+ IDAToffset = s.find(b'IDAT') - 4
+
+ # Create our lovely new pHYs chunk - https://www.w3.org/TR/2003/REC-PNG-20031110/#11pHYs
+ pHYs = b'pHYs' + struct.pack('!IIc', int(dpi[0] / 0.0254), int(dpi[1] / 0.0254), b"\x01")
+ pHYs = struct.pack('!I', 9) + pHYs + struct.pack('!I', zlib.crc32(pHYs))
+
+ # Open output filename and write...
+ # ... stuff preceding IDAT as created by OpenCV
+ # ... new pHYs as created by us above
+ # ... IDAT onwards as created by OpenCV
+ with open(filename, "wb") as out:
+ out.write(buffer[0:IDAToffset])
+ out.write(pHYs)
+ out.write(buffer[IDAToffset:])