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)
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))