| 40 | |
| 41 | |
| 42 | class LineMesh(object): |
| 43 | |
| 44 | def __init__(self, points, lines=None, colors=[0, 1, 0], radius=0.15): |
| 45 | """Creates a line represented as sequence of cylinder triangular |
| 46 | meshes. |
| 47 | |
| 48 | Arguments: |
| 49 | points {ndarray} -- Numpy array of ponts Nx3. |
| 50 | |
| 51 | Keyword Arguments: |
| 52 | lines {list[list] or None} -- List of point index pairs denoting |
| 53 | line segments. If None, implicit lines from ordered pairwise |
| 54 | points. (default: {None}) |
| 55 | colors {list} -- list of colors, or single color of the line |
| 56 | (default: {[0, 1, 0]}) |
| 57 | radius {float} -- radius of cylinder (default: {0.15}) |
| 58 | """ |
| 59 | self.points = np.array(points) |
| 60 | self.lines = np.array( |
| 61 | lines) if lines is not None else self.lines_from_ordered_points( |
| 62 | self.points) |
| 63 | self.colors = np.array(colors) |
| 64 | self.radius = radius |
| 65 | self.cylinder_segments = [] |
| 66 | |
| 67 | self.create_line_mesh() |
| 68 | |
| 69 | @staticmethod |
| 70 | def lines_from_ordered_points(points): |
| 71 | lines = [[i, i + 1] for i in range(0, points.shape[0] - 1, 1)] |
| 72 | return np.array(lines) |
| 73 | |
| 74 | def create_line_mesh(self): |
| 75 | first_points = self.points[self.lines[:, 0], :] |
| 76 | second_points = self.points[self.lines[:, 1], :] |
| 77 | line_segments = second_points - first_points |
| 78 | line_segments_unit, line_lengths = normalized(line_segments) |
| 79 | |
| 80 | z_axis = np.array([0, 0, 1]) |
| 81 | # Create triangular mesh cylinder segments of line |
| 82 | for i in range(line_segments_unit.shape[0]): |
| 83 | line_segment = line_segments_unit[i, :] |
| 84 | line_length = line_lengths[i] |
| 85 | # get axis angle rotation to allign cylinder with line segment |
| 86 | axis, angle = align_vector_to_another(z_axis, line_segment) |
| 87 | # Get translation vector |
| 88 | translation = first_points[i, :] + \ |
| 89 | line_segment * line_length * 0.5 |
| 90 | # create cylinder and apply transformations |
| 91 | cylinder_segment = o3d.geometry.TriangleMesh.create_cylinder( |
| 92 | self.radius, line_length) |
| 93 | cylinder_segment = cylinder_segment.translate(translation, |
| 94 | relative=False) |
| 95 | if axis is not None: |
| 96 | axis_a = axis * angle |
| 97 | cylinder_segment = cylinder_segment.rotate( |
| 98 | R=o3d.geometry.get_rotation_matrix_from_axis_angle( |
| 99 | axis_a)) # center=True) |
no outgoing calls
no test coverage detected