Skip to content

Commit

Permalink
Adding python scripts to generate test cases.
Browse files Browse the repository at this point in the history
  • Loading branch information
@oscarmendezm
oscarmendezm committed Dec 11, 2023
1 parent 75ea820 commit 5b22af0
Show file tree
Hide file tree
Showing 2 changed files with 290 additions and 0 deletions.
192 changes: 192 additions & 0 deletions fuse_constraints/test/test_fixed_3d_landmark_constraint.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,192 @@
import numpy as np
from scipy.spatial.transform import Rotation as R

# From ROS Calibrator on Logitech C920 (Oscar's) at 640x480
# D = [0.13281739520782995, -0.17255676937880005, -0.0036963860577237523, -0.00884659526000406, 0.0]
# K = [622.2066360931567, 0.0, 315.6497225093459, 0.0, 623.201615897975, 239.80322845004648, 0.0, 0.0, 1.0]
# R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
# P = [638.3447875976562, 0.0, 310.2906045722684, 0.0, 0.0, 643.107177734375, 237.80861559081677, 0.0, 0.0, 0.0, 1.0, 0.0]

class PinholeCameraProjection():
def __init__(self,):
# Define Size
self.w = 640
self.h = 480

# Calib Parameters
self.fx = 638.34478759765620
self.fy = 643.10717773437500
self.cx = 310.29060457226840
self.cy = 237.80861559081677

# Make Matrix
self.K = np.eye(4)
self.K[0,0] = self.fx
self.K[1,1] = self.fy
self.K[0,2] = self.cx
self.K[1,2] = self.cy

# Define 4x4 Array of 3D points at corners of a square (e.g. ARTag)
self.X = np.zeros((4,4))
self.X[0,:] = np.array([-1, -1, 0, 1])
self.X[1,:] = np.array([-1, 1, 0, 1])
self.X[2,:] = np.array([ 1, -1, 0, 1])
self.X[3,:] = np.array([ 1, 1, 0, 1])
# print(self.X)

# Define 8x4 Array of 3D points (e.g. 2 AR Tags)
self.X2 = np.zeros((8,4))
scale = 3.0
lps = (1.0 * scale)/2
lpg = 0.08 * scale
self.X2[0,:] = np.array([ -lps, -lps, 0, 1])
self.X2[1,:] = np.array([ -lps, lps, 0, 1])
self.X2[2,:] = np.array([ lps, -lps, 0, 1])
self.X2[3,:] = np.array([ lps, lps, 0, 1])
self.X2[4,:] = np.array([2*lps + lpg, -lps, 0, 1])
self.X2[5,:] = np.array([2*lps + lpg, lps, 0, 1])
self.X2[6,:] = np.array([ lps + lpg, -lps, 0, 1])
self.X2[7,:] = np.array([ lps + lpg, lps, 0, 1])
print(self.X2)

# Define T for
self.T = np.eye(4)
self.T[0:3,0:3] = R.from_quat([0, -0.3826834, 0, 0.9238795]).as_matrix()
self.T[2,3] = 10
print(self.T)

# Define Camera Position (identity)
self.Xc = np.eye(4)

def print_points(self, pts2d, pts3d):
print(f"3D:\n{pts3d[:,0:3]}")
print(f"2D:\n{pts2d[:,0:2]}")

def project_points(self, camPos, pts3d):
x = np.matmul(self.K, np.matmul(camPos, pts3d.transpose())).transpose()
x[:,0]/=x[:,2]
x[:,1]/=x[:,2]
x[:,2]/=x[:,2]
return x

def plot(self, pts2d, pts3d):

import matplotlib.pyplot as plt

fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(1,2,1, projection='3d')

ax.scatter(pts3d[:,0], pts3d[:,1], pts3d[:,2])
ax = fig.add_subplot(1,2,2)

ax.scatter(pts2d[:,0], pts2d[:,1], pts2d[:,2])
ax.set_xlim([0, self.w])
ax.set_ylim([0, self.h])
plt.show()

def project_poses(self, pts3d):

Xc = np.eye(4)
for i in range(-2,3,1):

Xc[0, 3] = i
Xc[1, 3] = i

if i == 0:
Xc[0:3, 0:3] = R.from_quat([ 0, 0.0871558, 0, 0.9961947 ]).as_matrix()
print(Xc)

x = self.project_points(Xc, pts3d)

def Optimization(self):
print("Points for Optimization")
print(f"fx = {self.fx}")
print(f"fy = {self.fy}")
print(f"cx = {self.cx}")
print(f"cy = {self.cy}")
X = np.matmul(self.T, self.X.transpose()).transpose()
x = self.project_points(np.eye(4), X)
self.print_points(x, X)
self.plot(x, X)

def OptimizationScaledMarker(self):
print("Points for OptimizationScaledMarker")
print(f"fx = {self.fx}")
print(f"fy = {self.fy}")
print(f"cx = {self.cx}")
print(f"cy = {self.cy}")

# Scale X
s = 0.25
Y = s * self.X # Scale X
Y[:,3] = 1.0

# Define T
self.T = np.eye(4)
self.T[0:3,0:3] = R.from_quat([0, -0.3826834, 0, 0.9238795]).as_matrix()
self.T[2,3] = 10

# Apply T
Y = np.matmul(self.T, Y.transpose()).transpose()

x = self.project_points(np.eye(4), Y)
self.print_points(x, Y)
self.plot(x, Y)

def OptimizationPoints(self):
print("Points for OptimizationPoints")
print(f"fx = {self.fx}")
print(f"fy = {self.fy}")
print(f"cx = {self.cx}")
print(f"cy = {self.cy}")

# Define T
self.T = np.eye(4)
self.T[0:3,0:3] = R.from_quat([0, -0.3826834, 0, 0.9238795]).as_matrix()
self.T[2,3] = 10

# Apply T
X2 = np.matmul(self.T, self.X2.transpose()).transpose()

x = self.project_points(np.eye(4), X2)
self.print_points(x, self.X2)
self.plot(x, self.X2)

def MultiViewOptimization(self):
print("Points for MultiViewOptimization")
print(f"fx = {self.fx}")
print(f"fy = {self.fy}")
print(f"cx = {self.cx}")
print(f"cy = {self.cy}")

# Define T
self.T = np.eye(4)
self.T[0:3,0:3] = R.from_quat([0, -0.3826834, 0, 0.9238795]).as_matrix()
self.T[2,3] = 10

# Apply T
pts3d = np.matmul(self.T, self.X.transpose()).transpose()
print(self.X)

Xc = np.eye(4)
for i in range(-2,3,1):

print(f"\n{i+2}: \n")

Xc[0, 3] = i
Xc[1, 3] = i

if i == 0:
Xc[0:3, 0:3] = R.from_quat([ 0, 0.0871558, 0, 0.9961947 ]).as_matrix()

x = self.project_points(Xc, pts3d)
self.print_points(x, pts3d)
self.plot(x, pts3d)

if __name__ == '__main__':
tests = PinholeCameraProjection()

tests.Optimization()
tests.OptimizationScaledMarker()
tests.OptimizationPoints()
tests.MultiViewOptimization()
98 changes: 98 additions & 0 deletions fuse_variables/test/test_pinhole_camera.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
import numpy as np

# D = [0.13281739520782995, -0.17255676937880005, -0.0036963860577237523, -0.00884659526000406, 0.0]
# K = [622.2066360931567, 0.0, 315.6497225093459, 0.0, 623.201615897975, 239.80322845004648, 0.0, 0.0, 1.0]
# R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
# P = [638.3447875976562, 0.0, 310.2906045722684, 0.0, 0.0, 643.107177734375, 237.80861559081677, 0.0, 0.0, 0.0, 1.0, 0.0]

class PinholeCameraProjection():
def __init__(self,):
# Define Size
self.w = 640
self.h = 480

# Calib Parameters
self.fx = 638.34478759765620
self.fy = 643.10717773437500
self.cx = 310.29060457226840
self.cy = 237.80861559081677

# Make Matrix
self.K = np.eye(4)
self.K[0,0] = self.fx
self.K[1,1] = self.fy
self.K[0,2] = self.cx
self.K[1,2] = self.cy

# Define 8x4 Array of 3D points at corners of a cube (e.g. ARTag)
self.X = np.zeros((8,4))
self.X[0,:] = np.array([-1, -1, -1, 1])
self.X[1,:] = np.array([-1, -1, 1, 1])
self.X[2,:] = np.array([-1, 1, -1, 1])
self.X[3,:] = np.array([-1, 1, 1, 1])
self.X[4,:] = np.array([ 1, -1, -1, 1])
self.X[5,:] = np.array([ 1, -1, 1, 1])
self.X[6,:] = np.array([ 1, 1, -1, 1])
self.X[7,:] = np.array([ 1, 1, 1, 1])

# Offset on Z to move in front of camera
self.X[:,2] += 10

def print_calib(self):
print(f"fx = {self.fx}")
print(f"fy = {self.fy}")
print(f"cx = {self.cx}")
print(f"cy = {self.cy}")

def print_points(self, pts2d, pts3d):
print(pts3d[:,0:3])
print(pts2d[:,0:2])

def project_points(self, camPos, pts3d):
x = np.matmul(self.K, np.matmul(camPos, pts3d.transpose())).transpose()
x[:,0]/=x[:,2]
x[:,1]/=x[:,2]
x[:,2]/=x[:,2]
return x

def plot(self, pts2d, pts3d):

import matplotlib.pyplot as plt

fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(1,2,1, projection='3d')

ax.scatter(pts3d[:,0], pts3d[:,1], pts3d[:,2])
ax = fig.add_subplot(1,2,2)

ax.scatter(pts2d[:,0], pts2d[:,1], pts2d[:,2])
ax.set_xlim([0, self.w])
ax.set_ylim([0, self.h])
plt.show()

def project_and_print(self):
x = self.project_points(np.eye(4), self.X)
self.print_calib()
self.print_points(x, self.X)
self.plot(x, self.X)

def FuseProjectionOptimization(self):
print("Points for FuseProjectionOptimization")
self.project_and_print()

def ProjectionOptimization(self):
print("Points for ProjectionOptimization")
self.project_and_print()

def PerPointProjectionOptimization(self):
print("Points for PerPointProjectionOptimization")
self.project_and_print()

if __name__ == '__main__':
tests = PinholeCameraProjection()

# fuseProjectionOptimization Test
tests.FuseProjectionOptimization()
tests.ProjectionOptimization()
tests.PerPointProjectionOptimization()

0 comments on commit 5b22af0

Please sign in to comment.