From 09f73b704a3080fdb622a67fa4c91216d2dde0e8 Mon Sep 17 00:00:00 2001 From: sprague_rick Date: Sat, 21 Sep 2024 22:00:32 -0400 Subject: [PATCH] update --- build.bash | 27 ------ epipolar_geometry.py | 205 ------------------------------------------- umeyama.cpp | 45 ---------- umeyama.py | 73 --------------- wandbox.cpp | 29 ------ wandbox.jl | 5 -- wandbox.py | 8 -- 7 files changed, 392 deletions(-) delete mode 100755 build.bash delete mode 100644 epipolar_geometry.py delete mode 100644 umeyama.cpp delete mode 100644 umeyama.py delete mode 100644 wandbox.cpp delete mode 100644 wandbox.jl delete mode 100755 wandbox.py diff --git a/build.bash b/build.bash deleted file mode 100755 index 0fcb130..0000000 --- a/build.bash +++ /dev/null @@ -1,27 +0,0 @@ -#!/bin/bash - -# Check if the argument is provided -if [ -z "$1" ]; then - echo "No C++ file provided." - exit 1 -fi - -# Extract the filename without extension -filename=$(basename -- "$1") -filename="${filename%.*}" - -mkdir -p "build/$filename" - -# Compile the C++ file -g++ -o "build/$filename/$filename" "$1" - -# Check if the compilation was successful -if [ $? -eq 0 ]; then - echo "Compilation successful. Executable is located at build/$filename/$filename" -else - echo "Compilation failed." - exit 1 -fi - -echo "Running: $filename" -build/$filename/$filename diff --git a/epipolar_geometry.py b/epipolar_geometry.py deleted file mode 100644 index 31614e0..0000000 --- a/epipolar_geometry.py +++ /dev/null @@ -1,205 +0,0 @@ -import numpy as np -import cv2 -import matplotlib.pyplot as plt - -# ego_cv: x+ is to right, y+ downward, z+ forward -# ego_original: x+ forward, y+ to the left, z+ up -R_correct = np.array([ - [0, 0, 1], - [-1, 0, 0], - [0, -1, 0] -], np.float64) -t_correct = np.array([ - [0], - [0], - [0] -], np.float64) - -correction_matrix = np.block([ - [R_correct, t_correct], - [np.zeros((1, 3)), 1] -]) - -def skew_matrix(v): - """ - Generates a 3x3 skew-symmetric matrix from a 3D vector. - - Args: - v (np.array): A 3D vector (numpy array of shape (3,)) - - Returns: - np.array: A 3x3 skew-symmetric matrix - """ - return np.array([ - [0, -v[2], v[1]], - [v[2], 0, -v[0]], - [-v[1], v[0], 0] - ]) - -def pose_matrix(angle, translation): - def rotation_matrix(angle): - """ - Generates a 3x3 rotation matrix for a given angle (in radians) around the Z-axis. - """ - R = np.array([ - [np.cos(angle), -np.sin(angle), 0], - [np.sin(angle), np.cos(angle), 0], - [0, 0, 1] - ]) - return R - """ - Generates a 4x4 pose matrix given an angle and translation vector. - Pose matrix is in the form [R, T; 0^T, 1]. - """ - R = rotation_matrix(angle) - T = np.array(translation).reshape(3, 1) - pose = np.block([ - [R, T], - [np.zeros((1, 3)), np.array([[1]])] - ]) - - - return pose @ correction_matrix - -def apply_intrinsics(K, point_3d): - """ - Applies the intrinsic matrix to a 3D point to project it onto the image plane. - """ - point_2d_homogeneous = K @ point_3d[:3] # Apply intrinsic matrix (ignoring the last element, which is 1) - point_2d = point_2d_homogeneous / point_2d_homogeneous[2] # Convert from homogeneous to Cartesian coordinates - return point_2d - -def draw_epipolar_lines(img1, img2, pts1, pts2, F): - """ - Draws the epipolar lines on both images and marks the points. - img1, img2: Images on which to draw the lines - pts1, pts2: Points in the two images - F: Fundamental matrix - """ - # Convert points to homogeneous coordinates - pts1 = np.int32(pts1) - pts2 = np.int32(pts2) - - # Draw the epipolar lines for points in the first image - lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F) - lines1 = lines1.reshape(-1, 3) - img1_with_lines = img1.copy() - - for r in lines1: - color = tuple(np.random.randint(0, 255, 3).tolist()) - x0, y0 = map(int, [0, -r[2] / r[1]]) - x1, y1 = map(int, [img1.shape[1], -(r[2] + r[0] * img1.shape[1]) / r[1]]) - img1_with_lines = cv2.line(img1_with_lines, (x0, y0), (x1, y1), color, 1) - - # Draw the epipolar lines for points in the second image - lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F) - lines2 = lines2.reshape(-1, 3) - img2_with_lines = img2.copy() - - for r in lines2: - color = tuple(np.random.randint(0, 255, 3).tolist()) - x0, y0 = map(int, [0, -r[2] / r[1]]) - x1, y1 = map(int, [img2.shape[1], -(r[2] + r[0] * img2.shape[1]) / r[1]]) - img2_with_lines = cv2.line(img2_with_lines, (x0, y0), (x1, y1), color, 1) - - return img1_with_lines, img2_with_lines - -def find_epipoles(F): - """ - Finds the epipoles in the image given the Fundamental matrix. - """ - # Compute the left epipole - _, _, Vt = np.linalg.svd(F.T) - e1 = Vt[-1, :] - e1 = e1 / e1[2] # Normalize - - # Compute the right epipole - _, _, Vt = np.linalg.svd(F) - e2 = Vt[-1, :] - e2 = e2 / e2[2] # Normalize - - return e1, e2 - -def main(): - # Define pose matrices using rotation around Z-axis and translation - verge = 30 - P1 = pose_matrix(np.radians(-verge), [0, 1, 0]) # P1 rotated -30 degrees and translated (0, 1, 0) - P2 = pose_matrix(np.radians(+verge), [0, -1, 0]) # P2 rotated 30 degrees and translated (0, -1, 0) - D = np.linalg.inv(P1) @ P2 - - # Example 3D landmark point in homogeneous coordinates - X = np.array([10, 0, 0, 1]) - - # Project the 3D point into the two camera images - x1_ego = np.linalg.inv(P1) @ X # 3D point in camera 1's coordinate system - x2_ego = np.linalg.inv(P2) @ X # 3D point in camera 2's coordinate system - print("X: ", X) - print("x1_ego: ", x1_ego) - print("x2_ego: ", x2_ego) - - # Intrinsic matrix (example) - K = np.array([ - [800, 0, 250], - [0, 800, 250], - [0, 0, 1] - ]) - Kinv = np.linalg.inv(K) - - - # Apply the intrinsic matrix to get 2D points - x1_2d = apply_intrinsics(K, x1_ego) - x2_2d = apply_intrinsics(K, x2_ego) - - print("x1_2d: ", x1_2d) - print("x2_2d: ", x2_2d) - - # Fundamental matrix (example, normally computed from camera matrices) - E = skew_matrix(D[:3, 3]) @ D[:3, :3] - F = Kinv.T @ E @ Kinv - - # Example images - img1 = np.ones((500, 500, 3), dtype=np.uint8) * 255 - img2 = np.ones((500, 500, 3), dtype=np.uint8) * 255 - - # Draw epipolar lines - pts1 = np.array([x1_2d[:2]]) - pts2 = np.array([x2_2d[:2]]) - - # Find epipoles - e1, e2 = find_epipoles(F) - print("e1: " , e1) - print("e2: " , e2) - - if True: - t = D[:3, 3] - t_2d = apply_intrinsics(K, t) - # can we find e1's epipol by projecting p2 into p1 (which is effectively projecting t into p1) - print("t: ", t) - print("t_2d: ", t_2d) - print("e1: ", e1) - print("F.T @ t_2d: ", F.T @ t_2d) - return - - img1_with_lines, img2_with_lines = draw_epipolar_lines(img1, img2, pts1, pts2, F) - - # Plot the results - plt.figure(figsize=(10, 5)) - plt.subplot(1, 2, 1) - plt.imshow(img1_with_lines) - plt.scatter([e1[0]], [e1[1]], color='red', marker='x', label='Epipole') # Epipole in the first image - plt.scatter([x1_2d[0]], [x1_2d[1]], color='blue', marker='o', label='Landmark') # Landmark projection - plt.title("Image 1 with Epipolar Lines") - plt.legend() - - plt.subplot(1, 2, 2) - plt.imshow(img2_with_lines) - plt.scatter([e2[0]], [e2[1]], color='red', marker='x', label='Epipole') # Epipole in the second image - plt.scatter([x2_2d[0]], [x2_2d[1]], color='blue', marker='o', label='Landmark') # Landmark projection - plt.title("Image 2 with Epipolar Lines") - plt.legend() - - plt.show() - -if __name__ == "__main__": - main() - diff --git a/umeyama.cpp b/umeyama.cpp deleted file mode 100644 index cb87185..0000000 --- a/umeyama.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include -#include -#include -using std::cout; -using std::endl; - -int main() { - // Generate 20 random 2D points (source points) - Eigen::MatrixXd src_points(2, 20); - src_points = Eigen::MatrixXd::Random(2, 20); - - // Define a known rotation matrix R and translation vector t - double theta = M_PI / 4; // 45 degrees rotation - Eigen::Matrix2d R; - R << std::cos(theta), -std::sin(theta), - std::sin(theta), std::cos(theta); - Eigen::Vector2d t(1.0, 2.0); - - // Apply the transformation to generate the destination points - Eigen::MatrixXd dst_points = (R * src_points).colwise() + t; - - // Use Eigen's Umeyama function to estimate the transformation - Eigen::Matrix3d T = Eigen::umeyama(src_points, dst_points, true); - - // Print the estimated transformation matrix - std::cout << "Estimated transformation matrix:\n" << T << std::endl; - - // Apply the resulting transformation to the source points - Eigen::MatrixXd src_points_hom(3, 20); - src_points_hom.topRows(2) = src_points; - src_points_hom.row(2) = Eigen::RowVectorXd::Ones(20); - - Eigen::MatrixXd aligned_points = (T * src_points_hom).topRows(2); - - // Print the original, transformed, and recovered points - std::cout << "Original Source Points:\n" << src_points << std::endl; - std::cout << "Transformed Destination Points:\n" << dst_points << std::endl; - std::cout << "Recovered Aligned Points:\n" << aligned_points << std::endl; - - // Calculate the difference between the destination points and the aligned points - double difference = (dst_points - aligned_points).norm(); - std::cout << "\nDifference between destination and aligned points: " << difference << std::endl; - - return 0; -} diff --git a/umeyama.py b/umeyama.py deleted file mode 100644 index 339d749..0000000 --- a/umeyama.py +++ /dev/null @@ -1,73 +0,0 @@ -import numpy as np - -def umeyama(src, dst, estimate_scale=True): - """Umeyama algorithm to estimate similarity transformation.""" - assert src.shape == dst.shape - - # Compute the mean of the source and destination points - src_mean = np.mean(src, axis=0) - dst_mean = np.mean(dst, axis=0) - - # Subtract the means from the points - src_centered = src - src_mean - dst_centered = dst - dst_mean - - # Compute the covariance matrix - cov_matrix = np.dot(dst_centered.T, src_centered) / src.shape[0] - - # Singular Value Decomposition - U, D, Vt = np.linalg.svd(cov_matrix) - - # Compute the rotation matrix - R = np.dot(U, Vt) - if np.linalg.det(R) < 0: - Vt[-1, :] *= -1 - R = np.dot(U, Vt) - - # Compute the scale factor - if estimate_scale: - var_src = np.var(src_centered, axis=0).sum() - scale = 1.0 / var_src * np.sum(D) - else: - scale = 1.0 - - # Compute the translation vector - t = dst_mean - scale * np.dot(R, src_mean) - - # Create the transformation matrix - T = np.identity(3) - T[:2, :2] = scale * R - T[:2, 2] = t - - return T - -# Generate 20 random 2D points -np.random.seed(42) # For reproducibility -src_points = np.random.rand(20, 2) - -# Define a known rotation matrix R and translation vector t -theta = np.pi / 4 # 45 degrees rotation -R = np.array([ - [np.cos(theta), -np.sin(theta)], - [np.sin(theta), np.cos(theta)] -]) -t = np.array([1.0, 2.0]) - -# Apply the transformation to generate the destination points -dst_points = np.dot(src_points, R.T) + t - -# Perform Umeyama to estimate the transformation -T = umeyama(src_points, dst_points) - -# Apply the resulting transformation to the source points -src_points_hom = np.hstack((src_points, np.ones((src_points.shape[0], 1)))) -aligned_points = np.dot(T, src_points_hom.T).T[:, :2] - -# Calculate the difference between the destination points and the aligned points -difference = np.linalg.norm(dst_points - aligned_points) - -print("Original Source Points:\n", src_points) -print("Transformed Destination Points:\n", dst_points) -print("Recovered Aligned Points:\n", aligned_points) -print("\nDifference between destination and aligned points:", difference) - diff --git a/wandbox.cpp b/wandbox.cpp deleted file mode 100644 index e1f1a5d..0000000 --- a/wandbox.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include - -using std::cout; -using std::endl; - -int main(int argc, char* argp[]) { - Eigen::Vector2d X = Eigen::Vector2d::Ones(); - cout << "X: " << X.transpose() << endl; - int counter = 0; - int b = 10; - int x= 10; - if (x = 0) { - cout << "This should generate a lint error" << endl; - } - while (true) { - cout << "hello" << endl; - - cout << counter++ << endl; - sleep(1); - } - return 0; //-1; -} diff --git a/wandbox.jl b/wandbox.jl deleted file mode 100644 index f50f5fd..0000000 --- a/wandbox.jl +++ /dev/null @@ -1,5 +0,0 @@ -struct X - x -end - - diff --git a/wandbox.py b/wandbox.py deleted file mode 100755 index d29a044..0000000 --- a/wandbox.py +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python3 - -print("Entering") -for i in range(10): - print(i) - print(i) - -print("Done.")