Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Vision constraints #352

Draft
wants to merge 12 commits into
base: devel
Choose a base branch
from
Next Next commit
Adding python scripts to generate test cases.
oscarmendezm committed Dec 11, 2023
commit 5b22af0f921dd57e48af70a4d0e9d756c715fe86
192 changes: 192 additions & 0 deletions fuse_constraints/test/test_fixed_3d_landmark_constraint.py
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
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()