diff --git a/fuse_constraints/test/test_fixed_3d_landmark_constraint.py b/fuse_constraints/test/test_fixed_3d_landmark_constraint.py new file mode 100644 index 000000000..fd0f2b189 --- /dev/null +++ b/fuse_constraints/test/test_fixed_3d_landmark_constraint.py @@ -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() diff --git a/fuse_variables/test/test_pinhole_camera.py b/fuse_variables/test/test_pinhole_camera.py new file mode 100644 index 000000000..3d8964ab2 --- /dev/null +++ b/fuse_variables/test/test_pinhole_camera.py @@ -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() +