def show_calibration_3d(self):
print 'Camera calibration'
print self.camera_mat, self.dist_coef
print 'Laser pose'
print self.profile.pose
print 'Laser homography'
print self.profile.homography
print 'Laser transformation'
print self.profile.trans
mplot3d = MPlot3D(scale=0.005)
for k, img in enumerate(self.images):
grid, pattern_pose = self.grids[k], self.pattern_poses[k]
if grid is not None:
profile3d, profile2d = self.get_chessboard_laser(
img, grid, pattern_pose)
if len(profile2d) > 0:
line, inliers = self.find_best_line2d(profile2d)
#mplot3d.draw_frame(pattern_pose)
mplot3d.draw_points(profile3d[inliers], color=(1, 1, 1))
mplot3d.draw_points(fit.apply_transformation(self.profile.trans, profile2d[inliers]), color=(1, 0, 0))
mplot3d.draw_points(self.profile.profile_to_points3d(profile2d[inliers], self.profile.homography, self.profile.pose), color=(0, 0, 1))
plane, inliers = self.find_best_plane(self.profiles3d)
mplot3d.draw_plane(plane, self.profiles3d[inliers])
plane_pose = fit.get_plane_pose(plane)
points3d = calc.transform_points2d(self.targets, plane_pose)
mplot3d.draw_points(points3d, color=(1, 1, 1))
#mplot3d.draw_points(self.profiles3d, color=(1, 1, 0))
# Draws camera and laser poses
mplot3d.draw_frame(self.profile.pose, label='laser')
mplot3d.draw_camera(self.camera_pose, color=(0.8, 0.8, 0.8))
mplot3d.show()
def show_calibration_3d(self):
print 'Camera calibration'
print self.camera_mat, self.dist_coef
print 'Laser pose'
print self.profile.pose
print 'Laser homography'
print self.profile.homography
print 'Laser transformation'
print self.profile.trans
mplot3d = MPlot3D(scale=0.005)
for k, img in enumerate(self.images):
grid, pattern_pose = self.grids[k], self.pattern_poses[k]
if grid is not None:
profile3d, profile2d = self.get_chessboard_laser(
img, grid, pattern_pose)
if len(profile2d) > 0:
line, inliers = self.find_best_line2d(profile2d)
#mplot3d.draw_frame(pattern_pose)
mplot3d.draw_points(profile3d[inliers], color=(1, 1, 1))
mplot3d.draw_points(fit.apply_transformation(
self.profile.trans, profile2d[inliers]),
color=(1, 0, 0))
mplot3d.draw_points(self.profile.profile_to_points3d(
profile2d[inliers], self.profile.homography,
self.profile.pose),
color=(0, 0, 1))
plane, inliers = self.find_best_plane(self.profiles3d)
mplot3d.draw_plane(plane, self.profiles3d[inliers])
plane_pose = fit.get_plane_pose(plane)
points3d = calc.transform_points2d(self.targets, plane_pose)
mplot3d.draw_points(points3d, color=(1, 1, 1))
#mplot3d.draw_points(self.profiles3d, color=(1, 1, 0))
# Draws camera and laser poses
mplot3d.draw_frame(self.profile.pose, label='laser')
mplot3d.draw_camera(self.camera_pose, color=(0.8, 0.8, 0.8))
mplot3d.show()
mplot3d.draw_points(calc.points_transformation(W2K, pp), color=WHITE)
img, grid = images[k], laser_calibration.grids[k]
if grid is not None:
chessboard_pose = pattern_poses[k]
profile3d, profile2d = laser_calibration.get_chessboard_laser(
img, grid, chessboard_pose)
if len(profile2d) > 0:
mplot3d.draw_points(calc.points_transformation(
WC, profile3d)[10:-1:25],
color=WHITE)
# points3d = laser_profile.profile_to_points3d(profiles[k],
# laser_profile.homography,
# laser_profile.pose)
# print points3d, laser_profile.pose, laser_profile.homography
# mplot3d.draw_points(calc.points_transformation(WC, points3d[0:-1:50]), color=(1, 0, 0))
points3d = fit.apply_transformation(laser_profile.trans,
profiles[k])
# print points3d, laser_profile.trans
mplot3d.draw_points(calc.points_transformation(
WC, points3d[0:-1:25]),
color=GREEN)
mplot3d.show()
imgname = '../../data/fchecker.png'
posname = '../../data/pchecker.txt'
# Workobject
x, y, z = (1.596609, 0.101342, 0.936394
) # ERROR in this measure (z aprox 1.0)
qx, qy, qz, qw = (0.00185993, -0.00245949, -0.01122562, 0.99994981)
W2K = calc.quatpose_to_matrix(*(np.array([x, y, z]),
np.array([qx, qy, qz, qw])))
# rot1 = calc.rpypose_to_matrix(np.array([0, 0, 0]), np.array([0, 0, -np.pi/2]))
mplot3d.draw_points(calc.points_transformation(WK, pp), color=GREEN)
mplot3d.draw_frame(calc.matrix_to_pose(W2K))
mplot3d.draw_points(calc.points_transformation(W2K, pp), color=WHITE)
img, grid = images[k], laser_calibration.grids[k]
if grid is not None:
chessboard_pose = pattern_poses[k]
profile3d, profile2d = laser_calibration.get_chessboard_laser(img, grid, chessboard_pose)
if len(profile2d) > 0:
mplot3d.draw_points(calc.points_transformation(WC, profile3d)[10:-1:25],
color=WHITE)
# points3d = laser_profile.profile_to_points3d(profiles[k],
# laser_profile.homography,
# laser_profile.pose)
# print points3d, laser_profile.pose, laser_profile.homography
# mplot3d.draw_points(calc.points_transformation(WC, points3d[0:-1:50]), color=(1, 0, 0))
points3d = fit.apply_transformation(laser_profile.trans, profiles[k])
# print points3d, laser_profile.trans
mplot3d.draw_points(calc.points_transformation(WC, points3d[0:-1:25]), color=GREEN)
mplot3d.show()
imgname = '../../data/frame_checker.png'
posname = '../../data/pose_checker.txt'
x, y, z = (1.6544, 0.0536, 0.9355) # ERROR in this measure (z aprox 1.0)
qx, qy, qz, qw = (0.002736, -0.004958, -0.004507, 0.999974)
W2K = calc.quatpose_to_matrix(*(np.array([x, y, z]), np.array([qx, qy, qz, qw])))
# rot1 = calc.rpypose_to_matrix(np.array([0, 0, 0]), np.array([0, 0, -np.pi/2]))
# rot2 = calc.rpypose_to_matrix(np.array([0, 0, 0]), np.array([0, np.pi, 0]))
# W2K = calc.matrix_compose((W2K, rot1, rot2))
W2T = read_pose(posname)
img = read_image(imgname)
