def find_calibration_3d(self, images):
self.get_calibration(images)
self.images = images
self.pattern_poses = [
self.get_chessboard_pose(grid) for grid in self.grids
]
profiles3d, profiles2d = [], []
for k, img in enumerate(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)
profiles3d.append(profile3d[inliers])
profiles2d.append(profile2d[inliers])
self.profiles3d = np.vstack(profiles3d)
self.profiles2d = np.vstack(profiles2d)
points3d = self.profiles3d
points2d = self.profiles2d
plane, inliers = self.find_best_plane(points3d)
plane_pose = fit.get_plane_pose(plane)
print 'Plane pose', plane_pose
print 'L', len(points3d), len(points2d)
points3d = points3d[inliers]
points2d = points2d[inliers]
print 'l', len(points3d), len(points2d)
print '> search transformation from 2d to 3d'
self.profile.trans = fit.fit_transformation(points2d, points3d)
self.profile.pose, self.profile.homography = self.find_lightplane(
self.profiles3d)
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_calibration_3d(self, images):
self.get_calibration(images)
self.images = images
self.pattern_poses = [self.get_chessboard_pose(grid)
for grid in self.grids]
profiles3d, profiles2d = [], []
for k, img in enumerate(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)
profiles3d.append(profile3d[inliers])
profiles2d.append(profile2d[inliers])
self.profiles3d = np.vstack(profiles3d)
self.profiles2d = np.vstack(profiles2d)
points3d = self.profiles3d
points2d = self.profiles2d
plane, inliers = self.find_best_plane(points3d)
plane_pose = fit.get_plane_pose(plane)
print 'Plane pose', plane_pose
print 'L', len(points3d), len(points2d)
points3d = points3d[inliers]
points2d = points2d[inliers]
print 'l', len(points3d), len(points2d)
print '> search transformation from 2d to 3d'
self.profile.trans = fit.fit_transformation(points2d, points3d)
self.profile.pose, self.profile.homography = self.find_lightplane(self.profiles3d)
def find_lightplane(self, points3d):
"""Calculates the transformation between the image points and the
lightplane."""
plane, inliers = self.find_best_plane(points3d)
plane_pose = fit.get_plane_pose(plane)
print 'Plane pose', plane_pose
plane_homography = self.find_plane_transformation(plane_pose)
print 'Homography', plane_homography
return plane_pose, plane_homography
def find_lightplane(self, points3d):
"""Calculates the transformation between the image points and the
lightplane."""
plane, inliers = self.find_best_plane(points3d)
plane_pose = fit.get_plane_pose(plane)
print 'Plane pose', plane_pose
plane_homography = self.find_plane_transformation(plane_pose)
print 'Homography', plane_homography
return plane_pose, plane_homography
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()
