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)
poses_checker, poses_tool = [], []
for k in range(len(tool_poses)):
pose_checker, pose_tool0 = None, None
if pattern_poses[k] is not None:
pose_checker = calc.pose_to_matrix(pattern_poses[k])
pose_tool0 = tool_poses[k]
poses_checker.append(pose_checker)
poses_tool.append(pose_tool0)
pchecker, ptool = [], []
for k in range(len(poses_checker)):
if poses_checker[k] is not None:
pchecker.append(poses_checker[k])
ptool.append(poses_tool[k])
poses_checker, poses_tool = pchecker, ptool
poses_ichecker = [calc.matrix_invert(pose) for pose in poses_checker]
poses_itool = [calc.matrix_invert(pose) for pose in poses_tool]
print 'Hand Eye Calibration Solution'
tlc = HandEyeCalibration()
T2C = tlc.solve(poses_tool, poses_checker)
W2K = tlc.solve(poses_itool, poses_ichecker)
print 'Tool2Camera:', calc.matrix_to_rpypose(T2C)
print 'World2Checker:', calc.matrix_to_rpypose(W2K)
WHITE = (1, 1, 1)
RED = (1, 0, 0)
GREEN = (0, 1, 0)
BLUE = (0, 0, 1)
mplot3d = MPlot3D(scale=0.0025)
poses_checker, poses_tool = [], []
for k in range(len(tool_poses)):
pose_checker, pose_tool0 = None, None
if pattern_poses[k] is not None:
pose_checker = calc.pose_to_matrix(pattern_poses[k])
pose_tool0 = tool_poses[k]
poses_checker.append(pose_checker)
poses_tool.append(pose_tool0)
pchecker, ptool = [], []
for k in range(len(poses_checker)):
if poses_checker[k] is not None:
pchecker.append(poses_checker[k])
ptool.append(poses_tool[k])
poses_checker, poses_tool = pchecker, ptool
poses_ichecker = [calc.matrix_invert(pose) for pose in poses_checker]
poses_itool = [calc.matrix_invert(pose) for pose in poses_tool]
print 'Hand Eye Calibration Solution'
tlc = HandEyeCalibration()
T2C = tlc.solve(poses_tool, poses_checker)
W2K = tlc.solve(poses_itool, poses_ichecker)
print 'Tool2Camera:', calc.matrix_to_rpypose(T2C)
print 'World2Checker:', calc.matrix_to_rpypose(W2K)
WHITE = (1, 1, 1)
RED = (1, 0, 0)
GREEN = (0, 1, 0)
BLUE = (0, 0, 1)
mplot3d = MPlot3D(scale=0.0025)
if __name__ == '__main__':
import doctest
doctest.testmod()
frame = np.float32([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
point = np.float32([0, 0, 0])
import calculate as calc
arrow = np.float32([[0, 0, 0], [100, 0, 0]])
pose_arrow = calc.rpypose_to_pose((0, 0, 100), (0, np.deg2rad(90), 0))
trans_arrow = calc.matrix_invert(calc.pose_to_matrix(pose_arrow))
print 'arrow quat:', calc.matrix_to_quatpose(trans_arrow)
points = calc.transform_points(pose_arrow, arrow)
arrow2 = (points[0], points[1]-points[0])
print pose_arrow, points
position = ((0, 20, 0), (np.deg2rad(-45), np.deg2rad(45), np.deg2rad(0)))
pose = calc.rpypose_to_pose(*position)
print 'quat:', calc.rpypose_to_quatpose(*position)
trans = calc.matrix_compose([calc.pose_to_matrix(pose), calc.pose_to_matrix(pose_arrow)])
print 'final pose:', calc.matrix_to_quatpose(trans)
points = calc.transform_points(calc.matrix_to_pose(trans), arrow)
arrow3 = (points[0], points[1]-points[0])
print pose, points
mplot3d = MPlot3D()
mplot3d = MPlot3D(scale=0.0025)
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)
poses_checker, poses_tool = [], []
for k in range(len(tool_poses)):
pose_checker, pose_tool0 = None, None
if pattern_poses[k] is not None:
pose_checker = calc.pose_to_matrix(pattern_poses[k])
pose_tool0 = tool_poses[k]
poses_checker.append(pose_checker)
poses_tool.append(pose_tool0)
pchecker, ptool = [], []
for k in range(len(poses_checker)):
if poses_checker[k] is not None:
pchecker.append(poses_checker[k])
ptool.append(poses_tool[k])
poses_checker, poses_tool = pchecker, ptool
poses_ichecker = [calc.matrix_invert(pose) for pose in poses_checker]
poses_itool = [calc.matrix_invert(pose) for pose in poses_tool]
print 'Hand Eye Calibration Solution'
tlc = HandEyeCalibration()
T2C = tlc.solve(poses_tool, poses_checker)
W2K = tlc.solve(poses_itool, poses_ichecker)
print 'Tool2Camera:', calc.matrix_to_rpypose(T2C)
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))
