diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..bf8098b Binary files /dev/null and b/.DS_Store differ diff --git a/Final_4.ipynb b/Final_4.ipynb new file mode 100644 index 0000000..1db42ce --- /dev/null +++ b/Final_4.ipynb @@ -0,0 +1,926 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Project 4: Sequential Structure from Motion\n", + "\n", + "### Due 4/3/2019\n", + "\n", + "### Graduate Students: Our next reading is [Snavely, 2006](http://195.130.87.21:8080/dspace/bitstream/123456789/636/1/Photo%20tourism%20exploring%20photo%20collections%20in%203D.pdf). We'll have a written report on this one: these methods papers aren't as good for discussions as I'd hoped.\n", + "\n", + "## Problem Statement\n", + "\n", + "You have now developed code that takes two photographs, finds matching locations in both, determines the relative motion between the cameras that took both photos, and solves for the 3D position of those points using epipolar geometry. **The next (and final for our purposes) stage is to extend this analysis to more than two images**, such that we can build 3D models of objects on the ground with just about as much detail as we'd like.\n", + "\n", + "## Adding the third photo\n", + "How do we add these additional photos? To be perfectly honest, at this point it's mostly an exercise in match housekeeping: we've already developed most of the code that we need. First, let's consider what we've got after matching our first two images, $I_1$ and $I_2$. First, we have a set of keypoints in each image, associated with a set of matches. These matches have been quality controlled twice: first by the ratio test, then by RANSAC in conjunction with the recovery of the essential matrix. Assuming that we've used our known camera matrix to convert our pixel-wise coordinates to generalized coordinates, let's call these keypoints $\\mathbf{x}_1$ and $\\mathbf{x}_2$. In practice, we can drop all of those keypoints for which there is not an associated accepted match. Then, for each of our kept matches, we have the essential matrix $E_{12}$, from which we can extract a pair of projection matrices $\\mathbf{P}_1 = [\\mathbf{I}|\\mathbf{0}]$ and $\\mathbf{P}_2 = [\\mathbf{R}_2|\\mathbf{t}_2]$. Using these projection matrices, we generated 3D, world coordinate location of the corresponding features that showed up robustly in both images. We'll call these coordinates $\\mathbf{X}_{12}$. \n", + "\n", + "To add a third image $\\mathbf{I}_3$ to the mix, consider that the situation outlined above is sort of analogous to the information that we have when we want to do pose estimation with ground control points: we have 3D world coordinates as well as the image coordinates of a set of points (a bunch of them, usually!), and we want to recover the camera pose. The problem is that the feature generalized coordinates that we've computed are for $I_1$ and $I_2$, but not $I_3$. Is this a big problem? Of course not! We can simply find $\\mathbf{x}_3$ in $I_3$ that correspond to $\\mathbf{x}_2$, the keypoints in the second image. Then we identify these keypoints with the 3D poi nts $\\mathbf{X}_{12}$. Thus we have image coordinates of features in the third image and the corresponding world coordinates: we can now perform pose estimation, just as we did in Project 1. \n", + "\n", + "Of course there are a few minor caveats: first, we need to filter out spurious matches between $\\mathbf{x}_2$ and $\\mathbf{x}_3$. To do this, we can utilize a tool that we already have: RANSAC estimation of the essential matrix. Because $I_2$ and $I_3$ are related by epipolar geometry just as $I_1$ and $I_2$ are, we can use the same subroutine to compute the essential matrix $\\mathbf{E}_{23}$, and (critically) identify and filter outliers, i.e. we'll discard matches that don't don't correspond to the consensus view of the essential matrix. This also leads to the next caveat, namely that we need an initial guess (call it $P_3^0$) for pose estimation to converge properly. Where should this initial guess come from? The $\\mathbf{E}_{23}$ provides a rotation given as if camera 2 were canonical, i.e. $\\mathbf{P_2'}=[\\mathbf{I}|\\mathbf{0}]$, $\\mathbf{P_3}'=[\\mathbf{R}_3'|\\mathbf{t}_3']$. We'll call it $P_3'$. We need to convert this projection matrix to a coordinate system in which $I_1$ (not $I_2$) is canonical. Fortunately, this is easy:\n", + "$$\n", + "P_3 \\approx P_2 P_3'.\n", + "$$\n", + "This $P_3$, is a an excellent initial guess for pose estimation (in principle, it's rotation matrix should actually be correct). Note that the translation component is only good up to a constant: however, this isn't too problematic because its direction is close to correct, and any optimization just needs to perform what amounts to a line search (a univariate optimization problem) to find the correct scaling. \n", + "\n", + "Once we have a robust estimation of the third camera's pose, we can use it do point triangulation on the correspondences between $I_2$ and $I_3$ not associated with an already-known world coordinate point, which allows us to augment our 3D model with new points. Additionally, we can perform triangulation with *3 views*, potentially improving our accuracy. Moreover, we can apply the process above iteratively, adding more and more images to generate a highly featured 3D model from (for example) 360 degrees worth of view angles. \n", + "\n", + "## Application\n", + "**Generate code that performs the above process for a third image. Apply it to one of the 3D image datasets that we generated in class. Note that we will be collecting aerial imagery from drones as well. Apply this method to a sequence of drone imagery as well.** As a challenge, can you implement code that sequentially adds an arbitrary number of images?\n" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "%matplotlib notebook\n", + "import pointgen\n", + "import importlib\n", + "importlib.reload(pointgen)\n", + "pg = pointgen\n", + "\n", + "class cloud_generator:\n", + " def __init__(self, ifnames, imgs):\n", + " self.ifnames = ifnames\n", + " self.imgs = imgs\n", + " self.point_cloud = None\n", + "\n", + " def generate(self):\n", + " self.point_cloud = pg.get_pt_cloud(self.ifnames, self.imgs)" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [], + "source": [ + "import matplotlib.pyplot as plt\n", + "\n", + "ifnames = [\n", + " \"falcon01.jpg\",\n", + " \"falcon02.jpg\",\n", + " \"falcon03.jpg\",\n", + " \"falcon04.jpg\",\n", + " \"falcon05.jpg\",\n", + " \"falcon06.jpg\",\n", + " \"falcon07.jpg\",\n", + " \"falcon08.jpg\",\n", + "]\n", + "imgs = [plt.imread(f) for f in ifnames]" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Common Keypoints: 200\n", + "Estimate:\n", + "[[ 0.4218987 -0.74015321 0.52361695 -1.79752032]\n", + " [ 0.73765354 0.61601151 0.2764002 -0.6788313 ]\n", + " [-0.52713257 0.26963501 0.80587109 0.45708198]]\n", + "Optimized:\n", + "[[ 0.41761609 -0.74185307 0.52464351 -1.4500891 ]\n", + " [ 0.74172162 0.61184942 0.27475321 -0.74756505]\n", + " [-0.52482934 0.27439808 0.80576663 0.51620814]]\n" + ] + }, + { + "data": { + "application/javascript": [ + "/* Put everything inside the global mpl namespace */\n", + "window.mpl = {};\n", + "\n", + "\n", + "mpl.get_websocket_type = function() {\n", + " if (typeof(WebSocket) !== 'undefined') {\n", + " return WebSocket;\n", + " } else if (typeof(MozWebSocket) !== 'undefined') {\n", + " return MozWebSocket;\n", + " } else {\n", + " alert('Your browser does not have WebSocket support.' +\n", + " 'Please try Chrome, Safari or Firefox ≥ 6. ' +\n", + " 'Firefox 4 and 5 are also supported but you ' +\n", + " 'have to enable WebSockets in about:config.');\n", + " };\n", + "}\n", + "\n", + "mpl.figure = function(figure_id, websocket, ondownload, parent_element) {\n", + " this.id = figure_id;\n", + "\n", + " this.ws = websocket;\n", + "\n", + " this.supports_binary = (this.ws.binaryType != undefined);\n", + "\n", + " if (!this.supports_binary) {\n", + " var warnings = document.getElementById(\"mpl-warnings\");\n", + " if (warnings) {\n", + " warnings.style.display = 'block';\n", + " warnings.textContent = (\n", + " \"This browser does not support binary websocket messages. \" +\n", + " \"Performance may be slow.\");\n", + " }\n", + " }\n", + "\n", + " this.imageObj = new Image();\n", + "\n", + " this.context = undefined;\n", + " this.message = undefined;\n", + " this.canvas = undefined;\n", + " this.rubberband_canvas = undefined;\n", + " this.rubberband_context = undefined;\n", + " this.format_dropdown = undefined;\n", + "\n", + " this.image_mode = 'full';\n", + "\n", + " this.root = $('
');\n", + " this._root_extra_style(this.root)\n", + " this.root.attr('style', 'display: inline-block');\n", + "\n", + " $(parent_element).append(this.root);\n", + "\n", + " this._init_header(this);\n", + " this._init_canvas(this);\n", + " this._init_toolbar(this);\n", + "\n", + " var fig = this;\n", + "\n", + " this.waiting = false;\n", + "\n", + " this.ws.onopen = function () {\n", + " fig.send_message(\"supports_binary\", {value: fig.supports_binary});\n", + " fig.send_message(\"send_image_mode\", {});\n", + " if (mpl.ratio != 1) {\n", + " fig.send_message(\"set_dpi_ratio\", {'dpi_ratio': mpl.ratio});\n", + " }\n", + " fig.send_message(\"refresh\", {});\n", + " }\n", + "\n", + " this.imageObj.onload = function() {\n", + " if (fig.image_mode == 'full') {\n", + " // Full images could contain transparency (where diff images\n", + " // almost always do), so we need to clear the canvas so that\n", + " // there is no ghosting.\n", + " fig.context.clearRect(0, 0, fig.canvas.width, fig.canvas.height);\n", + " }\n", + " fig.context.drawImage(fig.imageObj, 0, 0);\n", + " };\n", + "\n", + " this.imageObj.onunload = function() {\n", + " fig.ws.close();\n", + " }\n", + "\n", + " this.ws.onmessage = this._make_on_message_function(this);\n", + "\n", + " this.ondownload = ondownload;\n", + "}\n", + "\n", + "mpl.figure.prototype._init_header = function() {\n", + " var titlebar = $(\n", + " '
');\n", + " var titletext = $(\n", + " '
');\n", + " titlebar.append(titletext)\n", + " this.root.append(titlebar);\n", + " this.header = titletext[0];\n", + "}\n", + "\n", + "\n", + "\n", + "mpl.figure.prototype._canvas_extra_style = function(canvas_div) {\n", + "\n", + "}\n", + "\n", + "\n", + "mpl.figure.prototype._root_extra_style = function(canvas_div) {\n", + "\n", + "}\n", + "\n", + "mpl.figure.prototype._init_canvas = function() {\n", + " var fig = this;\n", + "\n", + " var canvas_div = $('
');\n", + "\n", + " canvas_div.attr('style', 'position: relative; clear: both; outline: 0');\n", + "\n", + " function canvas_keyboard_event(event) {\n", + " return fig.key_event(event, event['data']);\n", + " }\n", + "\n", + " canvas_div.keydown('key_press', canvas_keyboard_event);\n", + " canvas_div.keyup('key_release', canvas_keyboard_event);\n", + " this.canvas_div = canvas_div\n", + " this._canvas_extra_style(canvas_div)\n", + " this.root.append(canvas_div);\n", + "\n", + " var canvas = $('');\n", + " canvas.addClass('mpl-canvas');\n", + " canvas.attr('style', \"left: 0; top: 0; z-index: 0; outline: 0\")\n", + "\n", + " this.canvas = canvas[0];\n", + " this.context = canvas[0].getContext(\"2d\");\n", + "\n", + " var backingStore = this.context.backingStorePixelRatio ||\n", + "\tthis.context.webkitBackingStorePixelRatio ||\n", + "\tthis.context.mozBackingStorePixelRatio ||\n", + "\tthis.context.msBackingStorePixelRatio ||\n", + "\tthis.context.oBackingStorePixelRatio ||\n", + "\tthis.context.backingStorePixelRatio || 1;\n", + "\n", + " mpl.ratio = (window.devicePixelRatio || 1) / backingStore;\n", + "\n", + " var rubberband = $('');\n", + " rubberband.attr('style', \"position: absolute; left: 0; top: 0; z-index: 1;\")\n", + "\n", + " var pass_mouse_events = true;\n", + "\n", + " canvas_div.resizable({\n", + " start: function(event, ui) {\n", + " pass_mouse_events = false;\n", + " },\n", + " resize: function(event, ui) {\n", + " fig.request_resize(ui.size.width, ui.size.height);\n", + " },\n", + " stop: function(event, ui) {\n", + " pass_mouse_events = true;\n", + " fig.request_resize(ui.size.width, ui.size.height);\n", + " },\n", + " });\n", + "\n", + " function mouse_event_fn(event) {\n", + " if (pass_mouse_events)\n", + " return fig.mouse_event(event, event['data']);\n", + " }\n", + "\n", + " rubberband.mousedown('button_press', mouse_event_fn);\n", + " rubberband.mouseup('button_release', mouse_event_fn);\n", + " // Throttle sequential mouse events to 1 every 20ms.\n", + " rubberband.mousemove('motion_notify', mouse_event_fn);\n", + "\n", + " rubberband.mouseenter('figure_enter', mouse_event_fn);\n", + " rubberband.mouseleave('figure_leave', mouse_event_fn);\n", + "\n", + " canvas_div.on(\"wheel\", function (event) {\n", + " event = event.originalEvent;\n", + " event['data'] = 'scroll'\n", + " if (event.deltaY < 0) {\n", + " event.step = 1;\n", + " } else {\n", + " event.step = -1;\n", + " }\n", + " mouse_event_fn(event);\n", + " });\n", + "\n", + " canvas_div.append(canvas);\n", + " canvas_div.append(rubberband);\n", + "\n", + " this.rubberband = rubberband;\n", + " this.rubberband_canvas = rubberband[0];\n", + " this.rubberband_context = rubberband[0].getContext(\"2d\");\n", + " this.rubberband_context.strokeStyle = \"#000000\";\n", + "\n", + " this._resize_canvas = function(width, height) {\n", + " // Keep the size of the canvas, canvas container, and rubber band\n", + " // canvas in synch.\n", + " canvas_div.css('width', width)\n", + " canvas_div.css('height', height)\n", + "\n", + " canvas.attr('width', width * mpl.ratio);\n", + " canvas.attr('height', height * mpl.ratio);\n", + " canvas.attr('style', 'width: ' + width + 'px; height: ' + height + 'px;');\n", + "\n", + " rubberband.attr('width', width);\n", + " rubberband.attr('height', height);\n", + " }\n", + "\n", + " // Set the figure to an initial 600x600px, this will subsequently be updated\n", + " // upon first draw.\n", + " this._resize_canvas(600, 600);\n", + "\n", + " // Disable right mouse context menu.\n", + " $(this.rubberband_canvas).bind(\"contextmenu\",function(e){\n", + " return false;\n", + " });\n", + "\n", + " function set_focus () {\n", + " canvas.focus();\n", + " canvas_div.focus();\n", + " }\n", + "\n", + " window.setTimeout(set_focus, 100);\n", + "}\n", + "\n", + "mpl.figure.prototype._init_toolbar = function() {\n", + " var fig = this;\n", + "\n", + " var nav_element = $('
')\n", + " nav_element.attr('style', 'width: 100%');\n", + " this.root.append(nav_element);\n", + "\n", + " // Define a callback function for later on.\n", + " function toolbar_event(event) {\n", + " return fig.toolbar_button_onclick(event['data']);\n", + " }\n", + " function toolbar_mouse_event(event) {\n", + " return fig.toolbar_button_onmouseover(event['data']);\n", + " }\n", + "\n", + " for(var toolbar_ind in mpl.toolbar_items) {\n", + " var name = mpl.toolbar_items[toolbar_ind][0];\n", + " var tooltip = mpl.toolbar_items[toolbar_ind][1];\n", + " var image = mpl.toolbar_items[toolbar_ind][2];\n", + " var method_name = mpl.toolbar_items[toolbar_ind][3];\n", + "\n", + " if (!name) {\n", + " // put a spacer in here.\n", + " continue;\n", + " }\n", + " var button = $('');\n", + " button.click(method_name, toolbar_event);\n", + " button.mouseover(tooltip, toolbar_mouse_event);\n", + " nav_element.append(button);\n", + " }\n", + "\n", + " // Add the status bar.\n", + " var status_bar = $('');\n", + " nav_element.append(status_bar);\n", + " this.message = status_bar[0];\n", + "\n", + " // Add the close button to the window.\n", + " var buttongrp = $('
');\n", + " var button = $('');\n", + " button.click(function (evt) { fig.handle_close(fig, {}); } );\n", + " button.mouseover('Stop Interaction', toolbar_mouse_event);\n", + " buttongrp.append(button);\n", + " var titlebar = this.root.find($('.ui-dialog-titlebar'));\n", + " titlebar.prepend(buttongrp);\n", + "}\n", + "\n", + "mpl.figure.prototype._root_extra_style = function(el){\n", + " var fig = this\n", + " el.on(\"remove\", function(){\n", + "\tfig.close_ws(fig, {});\n", + " });\n", + "}\n", + "\n", + "mpl.figure.prototype._canvas_extra_style = function(el){\n", + " // this is important to make the div 'focusable\n", + " el.attr('tabindex', 0)\n", + " // reach out to IPython and tell the keyboard manager to turn it's self\n", + " // off when our div gets focus\n", + "\n", + " // location in version 3\n", + " if (IPython.notebook.keyboard_manager) {\n", + " IPython.notebook.keyboard_manager.register_events(el);\n", + " }\n", + " else {\n", + " // location in version 2\n", + " IPython.keyboard_manager.register_events(el);\n", + " }\n", + "\n", + "}\n", + "\n", + "mpl.figure.prototype._key_event_extra = function(event, name) {\n", + " var manager = IPython.notebook.keyboard_manager;\n", + " if (!manager)\n", + " manager = IPython.keyboard_manager;\n", + "\n", + " // Check for shift+enter\n", + " if (event.shiftKey && event.which == 13) {\n", + " this.canvas_div.blur();\n", + " event.shiftKey = false;\n", + " // Send a \"J\" for go to next cell\n", + " event.which = 74;\n", + " event.keyCode = 74;\n", + " manager.command_mode();\n", + " manager.handle_keydown(event);\n", + " }\n", + "}\n", + "\n", + "mpl.figure.prototype.handle_save = function(fig, msg) {\n", + " fig.ondownload(fig, null);\n", + "}\n", + "\n", + "\n", + "mpl.find_output_cell = function(html_output) {\n", + " // Return the cell and output element which can be found *uniquely* in the notebook.\n", + " // Note - this is a bit hacky, but it is done because the \"notebook_saving.Notebook\"\n", + " // IPython event is triggered only after the cells have been serialised, which for\n", + " // our purposes (turning an active figure into a static one), is too late.\n", + " var cells = IPython.notebook.get_cells();\n", + " var ncells = cells.length;\n", + " for (var i=0; i= 3 moved mimebundle to data attribute of output\n", + " data = data.data;\n", + " }\n", + " if (data['text/html'] == html_output) {\n", + " return [cell, data, j];\n", + " }\n", + " }\n", + " }\n", + " }\n", + "}\n", + "\n", + "// Register the function which deals with the matplotlib target/channel.\n", + "// The kernel may be null if the page has been refreshed.\n", + "if (IPython.notebook.kernel != null) {\n", + " IPython.notebook.kernel.comm_manager.register_target('matplotlib', mpl.mpl_figure_comm);\n", + "}\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "data": { + "text/html": [ + "" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "pcd = get_pt_cloud(ifnames, imgs)\n", + "plot_pcd(pcd)" + ] } ], "metadata": { @@ -55,7 +1183,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.6.5" + "version": "3.7.2" } }, "nbformat": 4, diff --git a/pointgen.py b/pointgen.py new file mode 100644 index 0000000..0d70a7b --- /dev/null +++ b/pointgen.py @@ -0,0 +1,253 @@ +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from collections import namedtuple +import numpy as np +import cv2 +import piexif +from scipy.optimize import least_squares + + +def plot_pcd(pcd, title="", marker=".", s=3): + fig = plt.figure(figsize=(6, 6)) + ax = fig.add_subplot(111, projection="3d", aspect="equal") + plt.title(title) + ax.scatter( + pcd[:, 0], pcd[:, 1], pcd[:, 2], c=pcd[:, 3:], marker=marker, s=s + ) + plt.show() + + +def extract_pixel_colors(img, uv): + return np.array([img[int(uvi[1]), int(uvi[0])] for uvi in uv]) / 255 + + +def create_point_cloud(Xs, imgs, uvs, colors=None): + m = np.sum([x.shape[0] for x in Xs]) + n = 6 + pcd = np.empty((m, n)) + mlast = 0 + for i, X in enumerate(Xs): + mi = X.shape[0] + mlast + pcd[mlast:mi, :3] = X + if colors is None: + pcd[mlast:mi, 3:] = extract_pixel_colors(imgs[i], uvs[i]) + else: + pcd[mlast:mi, 3:] = colors[i] + mlast = mi + return pcd + + +def triangulate(P0, P1, u1, u2): + X = [_triangulate(P0, P1, u1[i], u2[i]) for i in range(len(u1))] + X = np.array([(xi / xi[-1])[:3] for xi in X]) + return X + + +def _triangulate(P0, P1, x1, x2): + # P0,P1: projection matrices for each of two cameras/images + # x1,x1: corresponding points in each of two images (If using P that + # has been scaled by K, then use camera coordinates, otherwise use + # generalized coordinates) + A = np.array( + [ + [ + P0[2, 0] * x1[0] - P0[0, 0], + P0[2, 1] * x1[0] - P0[0, 1], + P0[2, 2] * x1[0] - P0[0, 2], + P0[2, 3] * x1[0] - P0[0, 3], + ], + [ + P0[2, 0] * x1[1] - P0[1, 0], + P0[2, 1] * x1[1] - P0[1, 1], + P0[2, 2] * x1[1] - P0[1, 2], + P0[2, 3] * x1[1] - P0[1, 3], + ], + [ + P1[2, 0] * x2[0] - P1[0, 0], + P1[2, 1] * x2[0] - P1[0, 1], + P1[2, 2] * x2[0] - P1[0, 2], + P1[2, 3] * x2[0] - P1[0, 3], + ], + [ + P1[2, 0] * x2[1] - P1[1, 0], + P1[2, 1] * x2[1] - P1[1, 1], + P1[2, 2] * x2[1] - P1[1, 2], + P1[2, 3] * x2[1] - P1[1, 3], + ], + ] + ) + u, s, vt = np.linalg.svd(A) + return vt[-1] + + +def get_focal_length(path, w): + exif = piexif.load(path) + return exif["Exif"][piexif.ExifIFD.FocalLengthIn35mmFilm] / 36 * w + + +ImgPair = namedtuple( + "ImgPair", + ( + "img1", + "img2", + "matched_kps", + "u1", + "u2", + "x1", + "x2", + "E", + "K", + "P_1", + "P_2", + ), +) +KeyPoint = namedtuple("KeyPoint", ("kp", "des")) +SiftImage = namedtuple("SiftImage", ("img", "kp", "des")) + + +def get_common_kps(pair1, pair2): + """Return the common keypoints and indices for each pair""" + # Use hash table instead of set to keep track of indices + # Don't rely on dict order as that is very new feature + hash1 = {k[1].kp: i for i, k in enumerate(pair1.matched_kps)} + hash2 = {k[0].kp: i for i, k in enumerate(pair2.matched_kps)} + + common = [] + idx1 = [] + idx2 = [] + for k in hash1: + if k in hash2: + common.append(pair1.matched_kps[hash1[k]][1]) + idx1.append(hash1[k]) + idx2.append(hash2[k]) + return common, idx1, idx2 + + +def affine_mult(P1, P2): + res = np.vstack((P1, [0, 0, 0, 1])) @ np.vstack((P2, [0, 0, 0, 1])) + return res[:-1] + + +def estimate_pose(pair1, pair2, cidx1, cidx2, X1, P3_est): + R = P3_est[:, :-1] + t0 = P3_est[:, -1].reshape((3, 1)) + P2 = pair1.P_2 + P2c = pair1.K @ P2 + + targets = X1[cidx1] + r0 = cv2.Rodrigues(R)[0] + p0 = list(r0.ravel()) + p0.extend(t0.ravel()) + u1 = pair2.u1[cidx2] + u2 = pair2.u2[cidx2] + + def residuals(p): + R = cv2.Rodrigues(p[:3])[0] + t = p[3:].reshape((3, 1)) + P3 = np.hstack((R, t)) + P3c = pair2.K @ P3 + Xest = triangulate(P2c, P3c, u1, u2) + return targets.ravel() - Xest.ravel() + + res = least_squares(residuals, p0) + p = res.x + R = cv2.Rodrigues(p[:3])[0] + t = p[3:].reshape((3, 1)) + P = np.hstack((R, t)) + return P + + +def compute_matches(des1, des2): + bf = cv2.BFMatcher() + matches = bf.knnMatch(des1, des2, k=2) + # Apply ratio test + good = [] + for i, (m, n) in enumerate(matches): + if m.distance < 0.8 * n.distance: + good.append(m) + return good + + +def process_img_pair(img1, kp1, des1, img2, kp2, des2, f): + h, w, _ = img1.shape + good = compute_matches(des1, des2) + u1 = [] + u2 = [] + matched_kps = [] + for m in good: + k1 = kp1[m.queryIdx] + d1 = des1[m.queryIdx] + k2 = kp2[m.trainIdx] + d2 = des2[m.trainIdx] + matched_kps.append((KeyPoint(k1, d1), KeyPoint(k2, d2))) + u1.append(k1.pt) + u2.append(k2.pt) + # u,v coords of keypoints in images + u1 = np.array(u1) + u2 = np.array(u2) + # Make homogeneous + u1 = np.c_[u1, np.ones(u1.shape[0])] + u2 = np.c_[u2, np.ones(u2.shape[0])] + + cu = w // 2 + cv = h // 2 + # Camera matrix + K_cam = np.array([[f, 0, cu], [0, f, cv], [0, 0, 1]]) + K_inv = np.linalg.inv(K_cam) + # Generalized image coords + x1 = u1 @ K_inv.T + x2 = u2 @ K_inv.T + # Compute essential matrix with RANSAC + E, inliers = cv2.findEssentialMat( + x1[:, :2], x2[:, :2], np.eye(3), method=cv2.RANSAC, threshold=1e-3 + ) + inliers = inliers.ravel().astype(bool) + + n_in, R, t, _ = cv2.recoverPose(E, x1[inliers, :2], x2[inliers, :2]) + # P_i = [R|t] with first image considered canonical + P_1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]]) + P_2 = np.hstack((R, t)) + matched_kps = [k for i, k in enumerate(matched_kps) if inliers[i]] + return ImgPair( + img1, + img2, + matched_kps, + u1[inliers], + u2[inliers], + x1[inliers], + x2[inliers], + E, + K_cam, + P_1, + P_2, + ) + + +def get_pt_cloud(ifnames, imgs): + h, w, _ = imgs[0].shape + f = get_focal_length(ifnames[0], w) + sift = cv2.xfeatures2d.SIFT_create() + simgs = [SiftImage(i, *sift.detectAndCompute(i, None)) for i in imgs[:3]] + pairs = [] + for i in range(len(simgs) - 1): + p = process_img_pair(*simgs[i], *simgs[i + 1], f) + pairs.append(p) + pair12, pair23 = pairs[:2] + + common_kps, idx1, idx2 = get_common_kps(*pairs[:2]) + print(f"Common Keypoints: {len(common_kps)}") + + P1c = pair12.K @ pair12.P_1 + P2c = pair12.K @ pair12.P_2 + X12 = triangulate(P1c, P2c, pair12.u1, pair12.u2) + + P3_est = affine_mult(pair12.P_2, pair23.P_2) + P3 = estimate_pose(pair12, pair23, idx1, idx2, X12, P3_est) + P3c = pair23.K @ P3 + X23 = triangulate(P2c, P3c, pair23.u1, pair23.u2) + print(f"Estimate:\n{P3_est}") + print(f"Optimized:\n{P3}") + + pcd = create_point_cloud((X12, X23), imgs[:2], (pair12.u1, pair23.u1)) + + return pcd