update
This commit is contained in:
parent
73e28b7ff8
commit
09f73b704a
27
build.bash
27
build.bash
|
|
@ -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
|
||||
|
|
@ -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()
|
||||
|
||||
45
umeyama.cpp
45
umeyama.cpp
|
|
@ -1,45 +0,0 @@
|
|||
#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;
|
||||
}
|
||||
73
umeyama.py
73
umeyama.py
|
|
@ -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)
|
||||
|
||||
29
wandbox.cpp
29
wandbox.cpp
|
|
@ -1,29 +0,0 @@
|
|||
#include <iostream>
|
||||
#include <list>
|
||||
#include <map>
|
||||
#include <unistd.h>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
struct X
|
||||
x
|
||||
end
|
||||
|
||||
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
print("Entering")
|
||||
for i in range(10):
|
||||
print(i)
|
||||
print(i)
|
||||
|
||||
print("Done.")
|
||||
Loading…
Reference in New Issue
Block a user