cloud = np.loadtxt(filename)
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]
imgc = draw_points(imgc,
profiles[k][inliers],
color=RED,
thickness=2)
imgc = draw_line(imgc, line, color=RED, thickness=2)
cv2.imwrite('../../data/board%i.png' % k, imgc)
show_images([imgc], wait=1000)
laser_calibration.show_calibration_3d()
laser_calibration.save_parameters('../../config/profile3d.yaml')
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()
line = ((0, int(line[0] * 0 + line[1])),
(int(img.shape[1]), int(line[0] * img.shape[1] + line[1])))
imgc = draw_points(imgc, profiles[k][inliers],
color=RED, thickness=2)
imgc = draw_line(imgc, line, color=RED, thickness=2)
cv2.imwrite('../../data/board%i.png' %k, imgc)
show_images([imgc], wait=1000)
laser_calibration.show_calibration_3d()
laser_calibration.save_parameters('../../config/profile3d.yaml')
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()
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]
def show(self):
mlab.show()
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])
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
