update

.gitignore

@@ -0,0 +1 @@
+build

build.bash

@@ -0,0 +1,27 @@
+#!/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

epipolar_geometry.py

@@ -170,16 +170,15 @@ def main():
     print("e1: " , e1)
     print("e2: " , e2)
 
-    # if True:
+    if True:
-    # this is wrong
+        t = D[:3, 3]
-    #     t = D[:3, 3]
+        t_2d = apply_intrinsics(K, t)
-    #     t_2d = apply_intrinsics(K, t)
+        # can we find e1's epipol by projecting p2 into p1 (which is effectively projecting t into p1)
-    #     # can we find e1's epipol by projecting p2 into p1 (which is effectively projecting t into p1)
+        print("t: ", t)
-    #     print("t: ", t)
+        print("t_2d: ", t_2d)
-    #     print("t_2d: ", t_2d)
+        print("e1: ", e1)
-    #     print("e1: ", e1)
+        print("F.T @ t_2d: ", F.T @ t_2d)
-    #     print("F.T @ t_2d: ", F.T @ t_2d)
+        return
-    #     return
 
     img1_with_lines, img2_with_lines = draw_epipolar_lines(img1, img2, pts1, pts2, F)
 

umeyama.cpp

@@ -0,0 +1,45 @@
+#include <iostream>
+#include <Eigen/Dense>
+#include <iostream>
+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;
+}

umeyama.py

@@ -0,0 +1,73 @@
+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)
+