mplot3d = MPlot3D()
mplot3d.draw_cloud(cloud)
mplot3d.show()
plane = PlaneFit()
models = plane.fit(cloud)
#modelr, inliers = plane.ransac(cloud, plane, 50, 0.0025)
plane_model, inliers = plane.ransac(cloud, plane, int(0.5*len(cloud)), 0.0025)
outliers = [k for k in range(len(cloud)) if k not in inliers]
plane_pose = get_plane_pose(plane_model)
print models, plane_model, plane_pose
#pose = (pose[0], np.zeros(3))
mplane = calc.pose_to_matrix(plane_pose)
implane = calc.matrix_invert(mplane)
tcloud = calc.points_transformation(implane, cloud)
mplot3d = MPlot3D(scale=0.0025)
#mplot3d.draw_frame((np.eye(3), np.zeros(3)))
mplot3d.draw_cloud(cloud)
#mplot3d.draw_points(cloud, color=WHITE)
mplot3d.draw_points(tcloud[inliers], color=RED)
mplot3d.draw_points(tcloud[outliers], color=BLUE)
mplot3d.show()
print np.std(tcloud[inliers][:, 2])
#test()
points3d = tcloud[outliers]
point_min = np.min(points3d, axis=0)
points_0 = (np.round(points3d - point_min, 3) / 0.0001).astype(np.int32)
BLUE = (0, 0, 1)
mplot3d = MPlot3D(scale=0.0025)
pp = laser_calibration.pattern_points
world_frame = calc.rpypose_to_matrix([0, 0, 0], [0, 0, 0])
mplot3d.draw_frame(calc.matrix_to_pose(world_frame), label='world_frame')
for k, tool_frame in enumerate(poses_tool):
WC = calc.matrix_compose((tool_frame, T2C))
mplot3d.draw_transformation(world_frame, tool_frame)
mplot3d.draw_transformation(tool_frame, WC, 'tool_pose%i' % k,
'camera_pose%i' % k)
WK = calc.matrix_compose((WC, poses_checker[k]))
# print 'Checker %i ->' % k, WK
# print np.allclose(W2K, WK, atol=0.0001)
mplot3d.draw_frame(calc.matrix_to_pose(WK))
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_cloud(cloud)
mplot3d.show()
plane = PlaneFit()
models = plane.fit(cloud)
#modelr, inliers = plane.ransac(cloud, plane, 50, 0.0025)
plane_model, inliers = plane.ransac(cloud, plane, int(0.5 * len(cloud)),
0.0025)
outliers = [k for k in range(len(cloud)) if k not in inliers]
plane_pose = get_plane_pose(plane_model)
print models, plane_model, plane_pose
#pose = (pose[0], np.zeros(3))
mplane = calc.pose_to_matrix(plane_pose)
implane = calc.matrix_invert(mplane)
tcloud = calc.points_transformation(implane, cloud)
mplot3d = MPlot3D(scale=0.0025)
#mplot3d.draw_frame((np.eye(3), np.zeros(3)))
mplot3d.draw_cloud(cloud)
#mplot3d.draw_points(cloud, color=WHITE)
mplot3d.draw_points(tcloud[inliers], color=RED)
mplot3d.draw_points(tcloud[outliers], color=BLUE)
mplot3d.show()
print np.std(tcloud[inliers][:, 2])
#test()
points3d = tcloud[outliers]
point_min = np.min(points3d, axis=0)
points_0 = (np.round(points3d - point_min, 3) / 0.0001).astype(np.int32)
BLUE = (0, 0, 1)
mplot3d = MPlot3D(scale=0.0025)
pp = laser_calibration.pattern_points
world_frame = calc.rpypose_to_matrix([0, 0, 0], [0, 0, 0])
mplot3d.draw_frame(calc.matrix_to_pose(world_frame), label='world_frame')
for k, tool_frame in enumerate(poses_tool):
WC = calc.matrix_compose((tool_frame, T2C))
mplot3d.draw_transformation(world_frame, tool_frame)
mplot3d.draw_transformation(
tool_frame, WC, 'tool_pose%i' % k, 'camera_pose%i' % k)
WK = calc.matrix_compose((WC, poses_checker[k]))
# print 'Checker %i ->' % k, WK
# print np.allclose(W2K, WK, atol=0.0001)
mplot3d.draw_frame(calc.matrix_to_pose(WK))
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 'World2Checker:', calc.matrix_to_rpypose(W2K)
mplot3d = MPlot3D(scale=0.0025)
pp = laser_calibration.pattern_points
world_frame = calc.rpypose_to_matrix([0, 0, 0], [0, 0, 0])
#mplot3d.draw_frame(calc.matrix_to_pose(world_frame), label='world_frame')
for k, tool_frame in enumerate(poses_tool):
WC = calc.matrix_compose((tool_frame, T2C))
#mplot3d.draw_transformation(world_frame, tool_frame)
#mplot3d.draw_transformation(tool_frame, WC, 'tool_pose%i' % k,
# 'camera_pose%i' % k)
WK = calc.matrix_compose((WC, poses_checker[k]))
print 'Checker %i ->' % k, WK
print np.allclose(W2K, WK, atol=0.0001)
mplot3d.draw_frame(calc.matrix_to_pose(WK))
mplot3d.draw_points(calc.points_transformation(WK, pp),
color=(1, 1, 0))
mplot3d.draw_frame(calc.matrix_to_pose(W2K))
mplot3d.draw_points(calc.points_transformation(W2K, pp),
color=(1, 1, 1))
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=(1, 1, 1))
# points3d = laser_profile.profile_to_points3d(profiles[k],
# laser_profile.homography,
# laser_profile.pose)
# print points3d, laser_profile.pose, laser_profile.homography
