def test_align():
orig_points = np.array([
[3.36748406, 1.61036404, 3.55147255],
[3.58702265, 0.06676394, 3.64695356],
[0.28452026, -0.11188296, 3.78947735],
[0.25482713, 1.57828256, 3.6900808],
[3.54938525, 1.74057692, 5.13329681],
[3.6855626 , 0.10335229, 5.26344841],
[0.25025385, -0.06146044, 5.57085135],
[0.20742481, 1.71073272, 5.41823085],
]).T
ft2inch = 12.0
inch2cm = 2.54
cm2m = 0.01
ft2m = ft2inch * inch2cm * cm2m
x1,y1,z1=0,0,0
x2,y2,z2=np.array([10,5,5])*ft2m
new_points = np.array([
[x2, y2, z2],
[x2, y1, z2],
[x1, y1, z2],
[x1, y2, z2],
[x2, y2, z1],
[x2, y1, z1],
[x1, y1, z1],
[x1, y2, z1],
]).T
print(orig_points.T)
print(new_points.T)
s,R,t = estsimt(orig_points,new_points)
print('s=%s'%repr(s))
print('R=%s'%repr(R.tolist()))
print('t=%s'%repr(t.tolist()))
Xnew = align_points( s,R,t, orig_points )
# measure distance between elements
mean_absdiff = np.mean( abs(Xnew[:3]-new_points).flatten() )
assert mean_absdiff < 0.05
M_orig = np.array([[1,2,3,4],
[5,6,7,8],
[9,10,11,12]],dtype=np.float)
print('Xnew.T')
print(Xnew.T)
M_new = align_M( s,R,t, M_orig )
print('M_new')
print(M_new)
def test_align():
orig_points = np.array([
[3.36748406, 1.61036404, 3.55147255],
[3.58702265, 0.06676394, 3.64695356],
[0.28452026, -0.11188296, 3.78947735],
[0.25482713, 1.57828256, 3.6900808],
[3.54938525, 1.74057692, 5.13329681],
[3.6855626, 0.10335229, 5.26344841],
[0.25025385, -0.06146044, 5.57085135],
[0.20742481, 1.71073272, 5.41823085],
]).T
ft2inch = 12.0
inch2cm = 2.54
cm2m = 0.01
ft2m = ft2inch * inch2cm * cm2m
x1, y1, z1 = 0, 0, 0
x2, y2, z2 = np.array([10, 5, 5]) * ft2m
new_points = np.array([
[x2, y2, z2],
[x2, y1, z2],
[x1, y1, z2],
[x1, y2, z2],
[x2, y2, z1],
[x2, y1, z1],
[x1, y1, z1],
[x1, y2, z1],
]).T
print(orig_points.T)
print(new_points.T)
s, R, t = estsimt(orig_points, new_points)
print('s=%s' % repr(s))
print('R=%s' % repr(R.tolist()))
print('t=%s' % repr(t.tolist()))
Xnew = align_points(s, R, t, orig_points)
# measure distance between elements
mean_absdiff = np.mean(abs(Xnew[:3] - new_points).flatten())
assert mean_absdiff < 0.05
M_orig = np.array([[1, 2, 3, 4], [5, 6, 7, 8], [9, 10, 11, 12]],
dtype=np.float)
print('Xnew.T')
print(Xnew.T)
M_new = align_M(s, R, t, M_orig)
print('M_new')
print(M_new)
def get_aligned_copy(self, other):
"""return copy of self that is scaled, translated, and rotated to best match other"""
assert isinstance( other, MultiCameraSystem)
orig_names = self.get_names()
new_names = other.get_names()
names = set(orig_names).intersection( new_names )
if len(names) < 3:
raise ValueError('need 3 or more cameras in common to align.')
orig_points = np.array([ self._cameras[name].get_camcenter() for name in names ]).T
new_points = np.array([ other._cameras[name].get_camcenter() for name in names ]).T
s,R,t = estsimt(orig_points,new_points)
assert is_rotation_matrix(R)
new_cams = []
for name in self.get_names():
orig_cam = self._cameras[name]
new_cam = orig_cam.get_aligned_camera(s,R,t)
new_cams.append( new_cam )
result = MultiCameraSystem(new_cams)
return result
def get_aligned_copy(self, other):
"""return copy of self that is scaled, translated, and rotated to best match other"""
assert isinstance( other, MultiCameraSystem)
orig_names = self.get_names()
new_names = other.get_names()
names = set(orig_names).intersection( new_names )
if len(names) < 3:
raise ValueError('need 3 or more cameras in common to align.')
orig_points = np.array([ self._cameras[name].get_camcenter() for name in names ]).T
new_points = np.array([ other._cameras[name].get_camcenter() for name in names ]).T
s,R,t = estsimt(orig_points,new_points)
assert is_rotation_matrix(R)
new_cams = []
for name in self.get_names():
orig_cam = self._cameras[name]
new_cam = orig_cam.get_aligned_camera(s,R,t)
new_cams.append( new_cam )
result = MultiCameraSystem(new_cams)
return result
if __name__ == "__main__":
images, camids = take_images()
points = run_point_select(camids, images)
# np.save("points", points)
camera_system = MultiCameraSystem.from_pymvg_file(path.join(BASE_CALIBRATION, "camera_system.json"))
# points = np.load("points.npy")
final_points = triangulate_points(camera_system, points)
sorted_points = identify_points(final_points)
plot_reprojection(camera_system, images, sorted_points)
s, R, T = align.estsimt(sorted_points.T, CALIBRATED_COORDINATES.T)
new_system = transform_system(camera_system, s, R, T)
new_system.save_to_pymvg_file(path.join(BASE_CALIBRATION, "camera_system_aligned.json"))
# DEBUGGING:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plot_system(ax, new_system)
ax.scatter(CALIBRATED_COORDINATES[:, 0], CALIBRATED_COORDINATES[:, 1], CALIBRATED_COORDINATES[:, 2])
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
