From 4243698ab38e0c460649faa8399885082de8e6f0 Mon Sep 17 00:00:00 2001 From: philipqueen Date: Mon, 20 May 2024 18:18:39 -0600 Subject: [PATCH 1/3] Add untested functions from opencv sfm module --- computer_code/api/helpers.py | 97 +++++++++++++++++++++++++++++++++++- computer_code/api/index.py | 6 +-- 2 files changed, 99 insertions(+), 4 deletions(-) diff --git a/computer_code/api/helpers.py b/computer_code/api/helpers.py index 836b4d5..2807185 100644 --- a/computer_code/api/helpers.py +++ b/computer_code/api/helpers.py @@ -1,3 +1,4 @@ +from typing import List, Tuple import numpy as np from scipy import linalg, optimize, signal import cv2 as cv @@ -240,6 +241,100 @@ def calculate_reprojection_error(image_points, object_point, camera_poses): return errors.mean() +""" +Original license for opencv sfm functions (applies to essential_from_fundamental, fundamental_from_projections, and motion_from_essential): +Software License Agreement (BSD License) + +Copyright (c) 2009, Willow Garage, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + * Neither the name of Willow Garage, Inc. nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +""" + +def essential_from_fundamental(F: np.ndarray, K1: np.ndarray, K2: np.ndarray) -> np.ndarray: + """ + Calculate the essential matrix from the fundamental matrix (F) and camera matrices (K1, K2). + + Adapted and modified from OpenCV's essentialFromFundamental function in the opencv_sfm module + """ + + assert F.shape == (3, 3), "F must be a 3x3 matrix" + assert K1.shape == (3, 3), "K1 must be a 3x3 matrix" + assert K2.shape == (3, 3), "K2 must be a 3x3 matrix" + + E = np.dot(np.dot(K2.T, F), K1) + return E + +def fundamental_from_projections(P1: np.ndarray, P2: np.ndarray) -> np.ndarray: + """ + Calculate the fundamental matrix from the projection matrices (P1, P2). + + Adapted and modified from OpenCV's fundamentalFromProjections function in the opencv_sfm module + """ + + assert P1.shape == (3, 4), "P1 must be a 3x4 matrix" + assert P2.shape == (3, 4), "P2 must be a 3x4 matrix" + + F = np.zeros((3, 3)) + + X = np.array([ + np.vstack((P1[1, :], P1[2, :])), + np.vstack((P1[2, :], P1[0, :])), + np.vstack((P1[0, :], P1[1, :])) + ]) + + Y = np.array([ + np.vstack((P2[1, :], P2[2, :])), + np.vstack((P2[2, :], P2[0, :])), + np.vstack((P2[0, :], P2[1, :])) + ]) + + for i in range(3): + for j in range(3): + XY = np.vstack((X[j], Y[i])) + F[i, j] = np.linalg.det(XY) + + return F + +def motion_from_essential(E: np.ndarray) -> Tuple[List[np.ndarray], List[np.ndarray]]: + """ + Calculate the possible rotations and translations from the essential matrix (E). + + Adapted and modified from OpenCV's motionFromEssential function in the opencv_sfm module + """ + assert E.shape == (3, 3), "Essential matrix must be 3x3." + + _, R1, R2, t = cv.decomposeEssentialMat(E) + + rotations_matrices = [R1, R1, R2, R2] + translations = [t, -t, t, -t] + + return rotations_matrices, translations + def bundle_adjustment(image_points, camera_poses, socketio): cameras = Cameras.instance() @@ -359,7 +454,7 @@ def find_point_correspondance_and_object_points(image_points, camera_poses, fram for i in range(1, len(camera_poses)): epipolar_lines = [] for root_image_point in root_image_points: - F = cv.sfm.fundamentalFromProjections(Ps[root_image_point["camera"]], Ps[i]) + F = fundamental_from_projections(Ps[root_image_point["camera"]], Ps[i]) line = cv.computeCorrespondEpilines(np.array([root_image_point["point"]], dtype=np.float32), 1, F) epipolar_lines.append(line[0,0].tolist()) frames[i] = drawlines(frames[i], line[0]) diff --git a/computer_code/api/index.py b/computer_code/api/index.py index cfd2c52..ae94a1b 100644 --- a/computer_code/api/index.py +++ b/computer_code/api/index.py @@ -1,4 +1,4 @@ -from helpers import camera_pose_to_serializable, calculate_reprojection_errors, bundle_adjustment, Cameras, triangulate_points +from helpers import camera_pose_to_serializable, calculate_reprojection_errors, bundle_adjustment, Cameras, triangulate_points, essential_from_fundamental, motion_from_essential, fundamental_from_projections from KalmanFilter import KalmanFilter from flask import Flask, Response, request @@ -244,8 +244,8 @@ def calculate_camera_pose(data): camera2_image_points = np.take(camera2_image_points, not_none_indicies, axis=0).astype(np.float32) F, _ = cv.findFundamentalMat(camera1_image_points, camera2_image_points, cv.FM_RANSAC, 1, 0.99999) - E = cv.sfm.essentialFromFundamental(F, cameras.get_camera_params(0)["intrinsic_matrix"], cameras.get_camera_params(1)["intrinsic_matrix"]) - possible_Rs, possible_ts = cv.sfm.motionFromEssential(E) + E = essential_from_fundamental(F, cameras.get_camera_params(0)["intrinsic_matrix"], cameras.get_camera_params(1)["intrinsic_matrix"]) + possible_Rs, possible_ts = motion_from_essential(E) R = None t = None From 5e3f5ede821a30efbbe5aeb918205fb4c5c9af51 Mon Sep 17 00:00:00 2001 From: philipqueen Date: Mon, 20 May 2024 18:19:27 -0600 Subject: [PATCH 2/3] remove opencv_sfm discussion from README --- README.md | 4 ---- 1 file changed, 4 deletions(-) diff --git a/README.md b/README.md index 25dad79..ce4e6ce 100644 --- a/README.md +++ b/README.md @@ -5,10 +5,6 @@ ## Dependencies Install the pseyepy python library: [https://github.com/bensondaled/pseyepy](https://github.com/bensondaled/pseyepy) -This project requires the sfm (structure from motion) OpenCV module, which requires you to compile OpenCV from source. This is a bit of a pain, but these links should help you get started: [SFM dependencies](https://docs.opencv.org/4.x/db/db8/tutorial_sfm_installation.html) [OpenCV module installation guide](https://github.com/opencv/opencv_contrib/blob/master/README.md) - -`cv.sfm` is only used 3 times in the codebase for the following functions: `fundamentalFromProjections`, `essentialFromFundamental`, `motionFromEssential`. So really, those functions should just be reimplemented in Python so the sfm module isn't needed. [Issue](https://github.com/jyjblrd/Mocap-Drones/issues/4). - install npm and yarn ## Runing the code From 9cec4f9ad4c1998f4225a8b97a96f63536a78712 Mon Sep 17 00:00:00 2001 From: philipqueen Date: Mon, 20 May 2024 18:20:08 -0600 Subject: [PATCH 3/3] remove unused import --- computer_code/api/index.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/computer_code/api/index.py b/computer_code/api/index.py index ae94a1b..b82f101 100644 --- a/computer_code/api/index.py +++ b/computer_code/api/index.py @@ -1,4 +1,4 @@ -from helpers import camera_pose_to_serializable, calculate_reprojection_errors, bundle_adjustment, Cameras, triangulate_points, essential_from_fundamental, motion_from_essential, fundamental_from_projections +from helpers import camera_pose_to_serializable, calculate_reprojection_errors, bundle_adjustment, Cameras, triangulate_points, essential_from_fundamental, motion_from_essential from KalmanFilter import KalmanFilter from flask import Flask, Response, request