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 find_plane_transformation(self, plane_pose):
"""Finds the homography transformation for a plane pose."""
points3d = calc.transform_points2d(self.targets, self.camera_pose)
points2d = self.project_3d_points(points3d, plane_pose)
homography = find_homography(points2d, self.targets)
return homography
