├── Python Scripts ├── farm_generation.py ├── height.py ├── segment.py └── voting.py ├── README.md ├── pics ├── figure_5.PNG ├── kent_filtered.png └── youtube_screenshot.png └── point_cloud_processing ├── CMakeLists.txt ├── launch ├── hardware_transform.launch └── sim_transform.launch ├── package.xml └── src ├── bounding_box.cpp ├── box_filter.cpp ├── bt.cpp ├── cloud_size.cpp ├── cluster_extraction.cpp ├── cluster_height_estimate.cpp ├── concatenate_cloud.cpp ├── crop_clusters.cpp ├── dji_tf_broadcast.cpp ├── fill_clusters.cpp ├── icp.cpp ├── known_ground_plane_max_height_estimate.cpp ├── map_height.cpp ├── max_height_estimate.cpp ├── nnsearch.cpp ├── non_dji_tf_listener.cpp ├── perimeter_heights.cpp ├── plane_height_estimate.cpp ├── plot_heights.cpp ├── recenter_cloud.cpp ├── save_cloud.cpp ├── simulation_tf_listener.cpp ├── single_box_filter.cpp ├── single_crop_box_filter.cpp ├── single_save_cloud.cpp ├── tf_listener.cpp ├── transform_and_plane_extraction.cpp ├── transform_cloud_and_save.cpp ├── transform_sim_and_save.cpp ├── turfgrass_heights.cpp ├── velodyne_tf_broadcast.cpp ├── view_colored_plot.cpp ├── view_grid.cpp ├── view_oriented_grid.cpp ├── view_single_cloud.cpp ├── voting_scheme.cpp ├── voxel_filter.cpp └── zed_cloud_view.cpp /Python Scripts/farm_generation.py: -------------------------------------------------------------------------------- 1 | from scipy.spatial import Delaunay 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from sympy import Plane, Point3D 5 | import math 6 | from collada import * 7 | from random import seed 8 | from random import random 9 | from random import randint 10 | from math import pi 11 | import xml.etree.ElementTree as ET 12 | 13 | seed(1) 14 | 15 | 16 | def sign(x1, y1, x2, y2, x3, y3): 17 | return (x1 - x3) * (y2 - y3) - (x2 - x3) * (y1 - y3) 18 | 19 | 20 | def isInside(x1, y1, x2, y2, x3, y3, x, y): 21 | 22 | d1 = sign(x, y, x1, y1, x2, y2) 23 | d2 = sign(x, y, x2, y2, x3, y3) 24 | d3 = sign(x, y, x3, y3, x1, y1) 25 | 26 | has_neg = (d1 < 0) or (d2 < 0) or (d3 < 0) 27 | has_pos = (d1 > 0) or (d2 > 0) or (d3 > 0) 28 | 29 | return not(has_neg and has_pos) 30 | 31 | 32 | height_variation = 0 33 | grid_size = 0 34 | num_of_vertices = 0 35 | 36 | ground_param_file = open('ground_params.txt', 'r') 37 | lines = ground_param_file.readlines() 38 | 39 | count = 0 40 | for line in lines: 41 | if count == 0: 42 | grid_size = int(line) 43 | elif count == 1: 44 | num_of_vertices = int(line) 45 | elif count == 2: 46 | height_variation = float(line) 47 | count = count + 1 48 | 49 | print('Generating 2D Delaunay triangulation') 50 | points = np.random.random([num_of_vertices, 2]) 51 | end_vertices = np.array([[-0.5 * grid_size, -0.5 * grid_size], [-0.5 * grid_size, 0.5 * grid_size], [0.5 * grid_size, -0.5 * grid_size], [0.5 * grid_size, 0.5 * grid_size]]) 52 | points = points * grid_size 53 | points = points - (grid_size / 2) 54 | points = np.append(points, end_vertices, axis=0) 55 | tri = Delaunay(points) 56 | 57 | indices = np.asarray(tri.simplices) 58 | 59 | #plt.triplot(points[:,0], points[:,1], tri.simplices) 60 | #plt.plot(points[:,0], points[:,1], 'o') 61 | #plt.show() 62 | 63 | print('Randomizing Z values') 64 | points_3d = np.random.random((points.shape[0], 3)) 65 | points_3d = (-1 * height_variation / 2) + (points_3d * height_variation) 66 | 67 | points_3d[:,0] = points[:,0] 68 | points_3d[:,1] = points[:,1] 69 | 70 | all_normals = np.zeros((1,3)) 71 | 72 | count = 0 73 | 74 | print('Computing Normal vectors') 75 | for item in tri.simplices: 76 | point1 = points_3d[item[0]] 77 | point2 = points_3d[item[1]] 78 | point3 = points_3d[item[2]] 79 | triangle = Plane(Point3D(point1[0], point1[1], point1[2]), Point3D(point2[0], point2[1], point2[2]), Point3D(point3[0], point3[1], point3[2])) 80 | normal = np.zeros((1,3)) 81 | normal[0][0] = float(triangle.normal_vector[0]) 82 | normal[0][1] = float(triangle.normal_vector[1]) 83 | normal[0][2] = float(triangle.normal_vector[2]) 84 | 85 | mag = (normal[0][0] * normal[0][0]) + (normal[0][1] * normal[0][1]) + (normal[0][2] * normal[0][2]) 86 | mag = math.sqrt(mag) 87 | normal[0][0] = normal[0][0] / mag 88 | normal[0][1] = normal[0][1] / mag 89 | normal[0][2] = normal[0][2] / mag 90 | 91 | if count == 0: 92 | count = count + 1 93 | all_normals[0,:] = normal 94 | else: 95 | all_normals = np.append(all_normals, normal, axis=0) 96 | 97 | reformed_ind = np.zeros((all_normals.shape[0], 3, 2)) 98 | 99 | for i in range(0, all_normals.shape[0]): 100 | reformed_ind[i][0][0] = indices[i][0] 101 | reformed_ind[i][1][0] = indices[i][1] 102 | reformed_ind[i][2][0] = indices[i][2] 103 | reformed_ind[i][0][1] = 4 * i 104 | reformed_ind[i][1][1] = (4 * i) + 1 105 | reformed_ind[i][2][1] = (4 * i) + 2 106 | 107 | normals_4 = np.zeros((all_normals.shape[0] * 4, all_normals.shape[1])) 108 | for i in range(0, all_normals.shape[0]): 109 | normals_4[4 * i] = all_normals[i] 110 | normals_4[(4*i) + 1] = all_normals[i] 111 | normals_4[(4*i) + 2] = all_normals[i] 112 | normals_4[(4*i) + 3] = all_normals[i] 113 | 114 | points_3d_ravel = np.ravel(points_3d, order='C') 115 | normals_4_ravel = np.ravel(normals_4, order='C') 116 | reformed_ind_ravel = np.ravel(reformed_ind, order='C') 117 | reformed_ind_ravel = reformed_ind_ravel.astype(int) 118 | 119 | print('Creating ground Collada/.dae model') 120 | mesh = Collada() 121 | effect = material.Effect("effect0", [], "phong", diffuse=(1,1,1), specular=(0,1,0)) 122 | mat = material.Material("material0", "mymaterial", effect) 123 | mesh.effects.append(effect) 124 | mesh.materials.append(mat) 125 | 126 | vert_src = source.FloatSource("verts-array", np.array(points_3d_ravel), ('X', 'Y', 'Z')) 127 | normal_src = source.FloatSource("normals-array", np.array(normals_4_ravel), ('X', 'Y', 'Z')) 128 | 129 | geom = geometry.Geometry(mesh, "geometry0", "myground", [vert_src, normal_src]) 130 | 131 | input_list = source.InputList() 132 | input_list.addInput(0, 'VERTEX', "#verts-array") 133 | input_list.addInput(1, 'NORMAL', "#normals-array") 134 | 135 | triset = geom.createTriangleSet(reformed_ind_ravel, input_list, "materialref") 136 | 137 | geom.primitives.append(triset) 138 | mesh.geometries.append(geom) 139 | 140 | matnode = scene.MaterialNode("materialref", mat, inputs=[]) 141 | geomnode = scene.GeometryNode(geom, [matnode]) 142 | node = scene.Node("node0", children=[geomnode]) 143 | 144 | myscene = scene.Scene("myscene", [node]) 145 | mesh.scenes.append(myscene) 146 | mesh.scene = myscene 147 | 148 | mesh.write('generated_ground.dae') 149 | 150 | print(mesh) 151 | 152 | indices = indices.astype(int) 153 | plot_width = 0 154 | plot_length = 0 155 | row_width = 0 156 | row_length = 0 157 | plants_per_plot = 0 158 | low_scale = 0 159 | high_scale = 0 160 | row_params = [] 161 | model_locations = [] 162 | model_heights = [] 163 | 164 | param_file = open('params.txt', 'r') 165 | lines = param_file.readlines() 166 | 167 | count = 0 168 | for line in lines: 169 | if count == 0: 170 | items = line.split() 171 | count2 = 0 172 | for item in items: 173 | if count2 == 0: 174 | plot_width = float(item) 175 | elif count2 == 1: 176 | plot_length = float(item) 177 | count2 = count2 + 1 178 | elif count == 1: 179 | items = line.split() 180 | count2 = 0 181 | for item in items: 182 | if count2 == 0: 183 | row_width = float(item) 184 | elif count2 == 1: 185 | row_length = float(item) 186 | count2 = count2 + 1 187 | elif count == 2: 188 | plants_per_plot = int(line) 189 | elif count == 3: 190 | items = line.split() 191 | count2 = 0 192 | for item in items: 193 | if count2 == 0: 194 | low_scale = float(item) 195 | elif count2 == 1: 196 | high_scale = float(item) 197 | count2 = count2 + 1 198 | else: 199 | row_params.append(line) 200 | count = count + 1 201 | 202 | model_location_file = open('models.txt', 'r') 203 | lines = model_location_file.readlines() 204 | for line in lines: 205 | model_locations.append(line.rstrip('\n')) 206 | 207 | model_heights_file = open("3D_Models/height.txt", "r") 208 | lines = model_heights_file.readlines() 209 | for line in lines: 210 | model_heights.append(float(line)) 211 | 212 | tree = ET.parse('empty.world') 213 | root = tree.getroot() 214 | model = ET.parse('soy.model') 215 | model_root = model.getroot() 216 | 217 | x_origin = 0 218 | y_origin = 0 219 | 220 | print('Generating soy models') 221 | sim_heights_file = open("simulated_heights.csv", "w") 222 | count = 1 223 | y_count = 1 224 | for item in row_params: 225 | params = item.split() 226 | 227 | num_of_plots = int(params[0]) 228 | row_offset = float(params[1]) 229 | x_origin = row_offset 230 | for i in range(0, num_of_plots): 231 | heights = "" 232 | for j in range(0, plants_per_plot): 233 | x_val = (random() * plot_width) + x_origin 234 | y_val = (random() * plot_length) + y_origin 235 | z_val = 0.0 236 | for k in range(0, indices.shape[0]): 237 | x1 = points_3d[indices[k][0]][0] 238 | y1 = points_3d[indices[k][0]][1] 239 | x2 = points_3d[indices[k][1]][0] 240 | y2 = points_3d[indices[k][1]][1] 241 | x3 = points_3d[indices[k][2]][0] 242 | y3 = points_3d[indices[k][2]][1] 243 | z1 = points_3d[indices[k][0]][2] 244 | z2 = points_3d[indices[k][1]][2] 245 | z3 = points_3d[indices[k][2]][2] 246 | 247 | if isInside(x1, y1, x2, y2, x3, y3, x_val, y_val): 248 | normal = all_normals[k] 249 | d = (normal[0] * x1) + (normal[1] * y1) + (normal[2] * z1) 250 | z_val = (d - (normal[0] * x_val) - (normal[1] * y_val)) / normal[2] 251 | #print(z_val > 0.0) 252 | #print(str(z1) + " " + str(z2) + " " + str(z3) + " " + str(z_val)) 253 | break 254 | 255 | x_scale = low_scale + (random() * (high_scale - low_scale)) 256 | y_scale = low_scale + (random() * (high_scale - low_scale)) 257 | z_scale = low_scale + (random() * (high_scale - low_scale)) 258 | yaw = random() * (2 * pi) 259 | model_num = randint(0, 4) 260 | model_file = model_locations[model_num] 261 | pose_val = str(x_val) + " " + str(y_val) + " " + str(z_val) + " 0 0 " + str(yaw) 262 | scale_val = str(x_scale) + " " + str(y_scale) + " " + str(z_scale) 263 | soy_name = "soybean_" + str(count) 264 | link_name = "soybean_" + str(count) + "_link" 265 | col_name = "soybean_" + str(count) + "_collision" 266 | heights = heights + str(model_heights[model_num] * z_scale) + "," 267 | for model_name in model_root.iter('model'): 268 | model_name.set('name', soy_name) 269 | for model_name in model_root.iter('link'): 270 | model_name.set('name', link_name) 271 | for pose in model_root.iter('pose'): 272 | pose.text = pose_val 273 | for model_name in model_root.iter('collision'): 274 | model_name.set('name', col_name) 275 | for model_name in model_root.iter('visual'): 276 | model_name.set('name', soy_name) 277 | for scale_name in model_root.iter('scale'): 278 | scale_name.text = scale_val 279 | for model_location in model_root.iter('uri'): 280 | model_location.text = model_file 281 | for world in root.findall('world'): 282 | world.append(model_root) 283 | #print(str(x_val) + " " + str(y_val)) 284 | count = count + 1 285 | tree.write('soy_row.world') 286 | tree = ET.parse('soy_row.world') 287 | root = tree.getroot() 288 | x_origin = x_origin + plot_width + row_width 289 | heights = heights[:-1] + "\n" 290 | sim_heights_file.write(heights) 291 | x_origin = 0 292 | y_origin = y_origin - plot_length - row_length 293 | y_count = y_count + 1 294 | 295 | ground_param_file.close() 296 | param_file.close() 297 | model_location_file.close() 298 | -------------------------------------------------------------------------------- /Python Scripts/height.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d 3 | import os 4 | import math 5 | 6 | folder_path = "/home/user01/Data/CorrectCluster/" 7 | 8 | pc_gp = open3d.io.read_point_cloud('/home/user01/Data/PointCloud/ground_plane.pcd') 9 | ground_plane = np.asarray(pc_gp.points) 10 | 11 | centroid = np.mean(ground_plane, axis=0) 12 | 13 | r = ground_plane - centroid 14 | 15 | xx_a = np.multiply(r[:, 0], r[:, 0]) 16 | xx = np.sum(xx_a) 17 | xy_a = np.multiply(r[:, 0], r[:, 1]) 18 | xy = np.sum(xy_a) 19 | xz_a = np.multiply(r[:, 0], r[:, 2]) 20 | xz = np.sum(xz_a) 21 | yy_a = np.multiply(r[:, 1], r[:, 1]) 22 | yy = np.sum(yy_a) 23 | yz_a = np.multiply(r[:, 1], r[:, 2]) 24 | yz = np.sum(yz_a) 25 | zz_a = np.multiply(r[:, 2], r[:, 2]) 26 | zz = np.sum(zz_a) 27 | 28 | det_x = yy*zz - yz*yz 29 | det_y = xx*zz - xz*xz 30 | det_z = xx*yy - xy*xy 31 | 32 | max = det_x 33 | max_letter = 1 34 | 35 | if det_y > max: 36 | max = det_y 37 | max_letter = 2 38 | if det_z > max: 39 | max = det_z 40 | max_letter = 3 41 | 42 | dir = np.zeros(3) 43 | if max_letter == 1: 44 | dir[0] = det_x 45 | dir[1] = xz * yz - xy * zz 46 | dir[2] = xy * yz - xz * yy 47 | elif max_letter == 2: 48 | dir[0] = xz*yz - xy*zz 49 | dir[1] = det_y 50 | dir[2] = xy*xz - yz*xx 51 | elif max_letter == 3: 52 | dir[0] = xy * yz - xz * yy 53 | dir[1] = xy * xz - yz * xx 54 | dir[2] = det_z 55 | 56 | length = (dir[0] * dir[0]) + (dir[1] * dir[1]) + (dir[2] * dir[2]) 57 | length = math.sqrt(length) 58 | 59 | dir = np.divide(dir, length) 60 | 61 | for filename in os.listdir(folder_path): 62 | file = folder_path + filename 63 | pc = open3d.io.read_point_cloud(file) 64 | cloud = np.asarray(pc.points) 65 | subtract_x = cloud[:, 0] - centroid[0] 66 | subtract_y = cloud[:, 1] - centroid[1] 67 | subtract_z = cloud[:, 2] - centroid[2] 68 | subtract_x = subtract_x * dir[0] 69 | subtract_y = subtract_y * dir[1] 70 | subtract_z = subtract_z * dir[2] 71 | 72 | temp_distance = np.add(subtract_x, subtract_y) 73 | temp_distance = np.add(temp_distance, subtract_z) 74 | temp_distance = np.absolute(temp_distance) 75 | max_height = np.amax(temp_distance) 76 | print("Max height: " + str(max_height) + " in meters.") 77 | 78 | -------------------------------------------------------------------------------- /Python Scripts/segment.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d 3 | from sklearn.cluster import KMeans 4 | from UliEngineering.Math.Coordinates import BoundingBox 5 | import cv2 6 | 7 | num_clusters = 112 8 | pc = open3d.io.read_point_cloud('/home/user01/Data/PointCloud/6-4-2019/recentered.pcd') 9 | cloud = np.asarray(pc.points) 10 | 11 | filtered_cloud = cloud.transpose() 12 | filtered_cloud = filtered_cloud[:, filtered_cloud[2] > 0.15] 13 | filtered_cloud = filtered_cloud.transpose() 14 | 15 | overall_filename = "/home/user01/Data/PointCloud/complete.pcd" 16 | pcd_total = open3d.geometry.PointCloud() 17 | pcd_total.points = open3d.utility.Vector3dVector(filtered_cloud) 18 | open3d.io.write_point_cloud(overall_filename, pcd_total) 19 | 20 | y = np.full(filtered_cloud.shape[0], 255) 21 | color = np.zeros(filtered_cloud.shape) 22 | color[:, 2] = y 23 | 24 | kmeans = KMeans(n_clusters=num_clusters, random_state=0, n_jobs=-1).fit(filtered_cloud) 25 | 26 | rectangle_list = [] 27 | for i in range(0, num_clusters): 28 | cluster_cloud = filtered_cloud.transpose() 29 | cluster_cloud = cluster_cloud[:, kmeans.labels_ == i] 30 | cluster_cloud = cluster_cloud.transpose() 31 | filename = "/home/user01/Data/PointCloud/Clusters/cluster_" + str(i) + ".pcd" 32 | pcd_cluster = open3d.geometry.PointCloud() 33 | pcd_cluster.points = open3d.utility.Vector3dVector(cluster_cloud) 34 | open3d.io.write_point_cloud(filename, pcd_cluster) 35 | #average = np.mean(cluster_cloud, axis=0) 36 | #box_points = np.delete(cluster_cloud, 2, 1) 37 | #box_points = np.float32(box_points) 38 | #box = BoundingBox(box_points) 39 | #rect = cv2.minAreaRect(box_points) 40 | #points = [[box.center[0] - box.width/2, box.center[1] - box.height/2, average[2]], [box.center[0] - box.width/2, box.center[1] + box.height/2, average[2]], 41 | # [box.center[0] + box.width/2, box.center[1] - box.height/2, average[2]], [box.center[0] + box.width/2, box.center[1] + box.height/2, average[2]]] 42 | #lines = [[0, 1], [0, 2], [1, 3], [2, 3]] 43 | #colors = [[1, 0, 0] for i in range(len(lines))] 44 | #line_set = open3d.geometry.LineSet() 45 | #line_set.points = open3d.utility.Vector3dVector(points) 46 | #line_set.lines = open3d.utility.Vector2iVector(lines) 47 | #line_set.colors = open3d.utility.Vector3dVector(colors) 48 | #rectangle_list.append(line_set) 49 | 50 | # test_cloud = filtered_cloud[kmeans.labels_ == 100] 51 | 52 | centers = np.asarray(kmeans.cluster_centers_) 53 | 54 | cluster_colors = np.zeros(centers.shape) 55 | x = np.full(num_clusters, 255); 56 | cluster_colors[:, 0] = x 57 | 58 | filtered_cloud = np.concatenate((filtered_cloud, centers)) 59 | color = np.concatenate((color, cluster_colors)) 60 | 61 | pcd_filtered = open3d.geometry.PointCloud() 62 | pcd_filtered.points = open3d.utility.Vector3dVector(filtered_cloud) 63 | pcd_filtered.colors = open3d.utility.Vector3dVector(color) 64 | 65 | 66 | 67 | vis = open3d.visualization.Visualizer() 68 | vis.create_window() 69 | vis.add_geometry(pcd_filtered) 70 | #for i in range(0, num_clusters): 71 | # vis.add_geometry(rectangle_list[i]) 72 | vis.run() 73 | vis.destroy_window() 74 | -------------------------------------------------------------------------------- /Python Scripts/voting.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import numpy as np 3 | import open3d 4 | 5 | pc = open3d.io.read_point_cloud('/home/user01/Data/PointCloud/6-4-2019/recentered.pcd') 6 | 7 | with open("/home/user01/Data/clusters_data.csv", 'r') as dest_f: 8 | data_iter = csv.reader(dest_f, 9 | delimiter = ',', 10 | quotechar = '"') 11 | data = [data for data in data_iter] 12 | 13 | orientation_range = 0.3 14 | width_range = 0.3 15 | height_range = 0.5 16 | 17 | voting_scheme_orientation = dict() 18 | voting_scheme_width = dict() 19 | voting_scheme_height = dict() 20 | 21 | for line in data: 22 | orientation = float(line[1]) 23 | count = 0 24 | while orientation > 0: 25 | orientation -= orientation_range 26 | count = count + 1 27 | key = count * orientation_range 28 | if key in voting_scheme_orientation: 29 | voting_scheme_orientation[key].append(float(line[1])) 30 | else: 31 | voting_scheme_orientation[key] = [float(line[1])] 32 | 33 | width = float(line[2]) 34 | count = 0 35 | while width > 0: 36 | width -= width_range 37 | count = count + 1 38 | key = count * width_range 39 | if key in voting_scheme_width: 40 | voting_scheme_width[key].append(float(line[2])) 41 | else: 42 | voting_scheme_width[key] = [float(line[2])] 43 | 44 | height = float(line[3]) 45 | count = 0 46 | while height > 0: 47 | height -= height_range 48 | count = count + 1 49 | key = count * height_range 50 | if key in voting_scheme_height: 51 | voting_scheme_height[key].append(float(line[3])) 52 | else: 53 | voting_scheme_height[key] = [float(line[3])] 54 | 55 | max_orientation_key = -100.0 56 | print("ORIENTATION") 57 | for key, value in voting_scheme_orientation.items(): 58 | if max_orientation_key == -100.0: 59 | max_orientation_key = key 60 | else: 61 | if len(voting_scheme_orientation[key]) > len(voting_scheme_orientation[max_orientation_key]): 62 | max_orientation_key = key 63 | print(key, len(value)) 64 | 65 | max_width_key = -100.0 66 | print("\nWIDTH") 67 | for key, value in voting_scheme_width.items(): 68 | if max_width_key == -100.0: 69 | max_width_key = key 70 | else: 71 | if len(voting_scheme_width[key]) > len(voting_scheme_width[max_width_key]): 72 | max_width_key = key 73 | print(key, len(value)) 74 | 75 | max_height_key = -100.0 76 | print("\nHEIGHT") 77 | for key, value in voting_scheme_height.items(): 78 | if max_height_key == -100.0: 79 | max_height_key = key 80 | else: 81 | if len(voting_scheme_height[key]) > len(voting_scheme_height[max_height_key]): 82 | max_height_key = key 83 | print(key, len(value)) 84 | 85 | print("\n") 86 | print(len(voting_scheme_orientation[max_orientation_key])) 87 | print("\n") 88 | print(len(voting_scheme_width[max_width_key])) 89 | print("\n") 90 | print(len(voting_scheme_height[max_height_key])) 91 | 92 | orientation_values = np.asarray(voting_scheme_orientation[max_orientation_key], dtype=np.float32) 93 | width_values = np.asarray(voting_scheme_width[max_width_key], dtype=np.float32) 94 | height_values = np.asarray(voting_scheme_height[max_height_key], dtype=np.float32) 95 | 96 | orientation_average = np.average(orientation_values) 97 | width_average = np.average(width_values) 98 | height_average = np.average(height_values) 99 | 100 | with open('/home/user01/Data/averages.csv', mode='w') as average_file: 101 | average_writer = csv.writer(average_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL) 102 | average_writer.writerow([orientation_average, width_average, height_average]) 103 | 104 | # all_data = np.asarray(data) 105 | # 106 | # cloud = np.asarray(pc.points) 107 | # 108 | # filtered_cloud = cloud.transpose() 109 | # filtered_cloud = filtered_cloud[:, filtered_cloud[2] > 0.15] 110 | # filtered_cloud = filtered_cloud.transpose() 111 | # 112 | # pcd_filtered = open3d.geometry.PointCloud() 113 | # pcd_filtered.points = open3d.utility.Vector3dVector(filtered_cloud) 114 | # #pcd_filtered.colors = open3d.utility.Vector3dVector(color) 115 | # 116 | # vis = open3d.visualization.Visualizer() 117 | # vis.create_window() 118 | # vis.add_geometry(pcd_filtered) 119 | # #for i in range(0, num_clusters): 120 | # # vis.add_geometry(rectangle_list[i]) 121 | # vis.run() 122 | # vis.destroy_window() 123 | 124 | print("\n") 125 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![IMAGE ALT TEXT HERE](https://github.com/hsd1121/PointCloudProcessing/blob/master/pics/youtube_screenshot.png?raw=true)](https://www.youtube.com/watch?v=S3yJ5RLepR0) 2 | 3 | ## Citing this work 4 | PointCloudProcessing was developed for and used in the following paper 5 | [[DOI]](https://ieeexplore.ieee.org/document/9341343/citations?tabFilter=papers#citations) [[arXiv/PDF]](https://arxiv.org/abs/1910.14031): 6 | 7 | @INPROCEEDINGS{9341343, 8 | author={Dhami, Harnaik and Yu, Kevin and Xu, Tianshu and Zhu, Qian and Dhakal, Kshitiz and Friel, James and Li, Song and Tokekar, Pratap}, 9 | booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 10 | title={Crop Height and Plot Estimation for Phenotyping from Unmanned Aerial Vehicles using 3D LiDAR}, 11 | year={2020}, 12 | volume={}, 13 | number={}, 14 | pages={2643-2649}, 15 | doi={10.1109/IROS45743.2020.9341343} 16 | } 17 | 18 | Please cite this paper when using PointCloudProcessing. 19 | 20 | ![alt text](https://github.com/hsd1121/PointCloudProcessing/blob/master/pics/kent_filtered.png?raw=true) 21 | 22 | ROS Package is in the point_cloud_processing folder. 23 | 24 | Datasets: 25 | 26 | Kentland Wheat Farm 27 | 28 | https://drive.google.com/open?id=11Gq6jv23fsi0puPGos4_p8Nh1qeF4EdX 29 | 30 | Gazebo Soybean Farms 31 | 32 | https://drive.google.com/open?id=1cbuy1wDpMB792Pbp7qokSVROeLPORPQV 33 | 34 | This project is partly funded by SmartFarm Innovation Network and Virginia Tech iCAT institute. 35 | 36 | ![alt text](https://github.com/hsd1121/PointCloudProcessing/blob/master/pics/figure_5.PNG?raw=true) 37 | -------------------------------------------------------------------------------- /pics/figure_5.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hsd1121/PointCloudProcessing/b31a54c8dc0b29ecd9aa35df3a6624ef6106eca7/pics/figure_5.PNG -------------------------------------------------------------------------------- /pics/kent_filtered.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hsd1121/PointCloudProcessing/b31a54c8dc0b29ecd9aa35df3a6624ef6106eca7/pics/kent_filtered.png -------------------------------------------------------------------------------- /pics/youtube_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hsd1121/PointCloudProcessing/b31a54c8dc0b29ecd9aa35df3a6624ef6106eca7/pics/youtube_screenshot.png -------------------------------------------------------------------------------- /point_cloud_processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(point_cloud_processing) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | pcl_conversions 12 | pcl_ros 13 | octomap_ros 14 | roscpp 15 | sensor_msgs 16 | cmake_modules 17 | eigen_conversions 18 | tf_conversions 19 | ) 20 | find_package(Eigen REQUIRED) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a exec_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # sensor_msgs 80 | # ) 81 | 82 | ################################################ 83 | ## Declare ROS dynamic reconfigure parameters ## 84 | ################################################ 85 | 86 | ## To declare and build dynamic reconfigure parameters within this 87 | ## package, follow these steps: 88 | ## * In the file package.xml: 89 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "dynamic_reconfigure" to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 94 | ## and list every .cfg file to be processed 95 | 96 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 97 | # generate_dynamic_reconfigure_options( 98 | # cfg/DynReconf1.cfg 99 | # cfg/DynReconf2.cfg 100 | # ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if your package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | # INCLUDE_DIRS include 113 | # LIBRARIES point_cloud_processing 114 | # CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs 115 | # DEPENDS system_lib 116 | DEPENDS Eigen 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | # include 127 | ${catkin_INCLUDE_DIRS} 128 | ${Eigen_INCLUDE_DIRS} 129 | ) 130 | 131 | ## Declare a C++ library 132 | # add_library(${PROJECT_NAME} 133 | # src/${PROJECT_NAME}/point_cloud_processing.cpp 134 | # ) 135 | 136 | ## Add cmake target dependencies of the library 137 | ## as an example, code may need to be generated before libraries 138 | ## either from message generation or dynamic reconfigure 139 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Declare a C++ executable 142 | ## With catkin_make all packages are built within a single CMake context 143 | ## The recommended prefix ensures that target names across packages don't collide 144 | # add_executable(${PROJECT_NAME}_node src/point_cloud_processing_node.cpp) 145 | 146 | add_executable(max_height_estimate src/max_height_estimate.cpp) 147 | add_executable(plane_height_estimate src/plane_height_estimate.cpp) 148 | add_executable(map_height src/map_height.cpp) 149 | add_executable(view_single_cloud src/view_single_cloud.cpp) 150 | add_executable(save_cloud src/save_cloud.cpp) 151 | add_executable(zed_cloud_view src/zed_cloud_view.cpp) 152 | add_executable(single_save_cloud src/single_save_cloud.cpp) 153 | add_executable(box_filter src/box_filter.cpp) 154 | add_executable(single_box_filter src/single_box_filter.cpp) 155 | add_executable(single_crop_box_filter src/single_crop_box_filter.cpp) 156 | add_executable(dji_tf_broadcast src/dji_tf_broadcast.cpp) 157 | add_executable(velodyne_tf_broadcast src/velodyne_tf_broadcast.cpp) 158 | add_executable(tf_listener src/tf_listener.cpp) 159 | add_executable(transform_cloud_and_save src/transform_cloud_and_save.cpp) 160 | add_executable(non_dji_tf_listener src/non_dji_tf_listener.cpp) 161 | add_executable(simulation_tf_listener src/simulation_tf_listener.cpp) 162 | add_executable(transform_sim_and_save src/transform_sim_and_save.cpp) 163 | add_executable(concatenate_cloud src/concatenate_cloud.cpp) 164 | add_executable(icp src/icp.cpp) 165 | add_executable(transform_and_plane_extraction src/transform_and_plane_extraction.cpp) 166 | add_executable(voxel_filter src/voxel_filter.cpp) 167 | add_executable(recenter_cloud src/recenter_cloud.cpp) 168 | add_executable(cluster_extraction src/cluster_extraction.cpp) 169 | add_executable(bounding_box src/bounding_box.cpp) 170 | add_executable(known_ground_plane_max_height_estimate src/known_ground_plane_max_height_estimate.cpp) 171 | add_executable(voting_scheme src/voting_scheme.cpp) 172 | add_executable(fill_clusters src/fill_clusters.cpp) 173 | add_executable(cluster_height_estimate src/cluster_height_estimate.cpp) 174 | add_executable(crop_clusters src/crop_clusters.cpp) 175 | add_executable(turfgrass_heights src/turfgrass_heights.cpp) 176 | add_executable(plot_heights src/plot_heights.cpp) 177 | add_executable(cloud_size src/cloud_size.cpp) 178 | add_executable(perimeter_heights src/perimeter_heights.cpp) 179 | add_executable(view_colored_plot src/view_colored_plot.cpp) 180 | add_executable(view_grid src/view_grid.cpp) 181 | add_executable(view_oriented_grid src/view_oriented_grid.cpp) 182 | add_executable(nnsearch src/nnsearch.cpp) 183 | add_executable(bt src/bt.cpp) 184 | 185 | ## Rename C++ executable without prefix 186 | ## The above recommended prefix causes long target names, the following renames the 187 | ## target back to the shorter version for ease of user use 188 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 189 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 190 | 191 | ## Add cmake target dependencies of the executable 192 | ## same as for the library above 193 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 194 | 195 | ## Specify libraries to link a library or executable target against 196 | # target_link_libraries(${PROJECT_NAME}_node 197 | # ${catkin_LIBRARIES} 198 | # ) 199 | 200 | target_link_libraries(view_single_cloud ${catkin_LIBRARIES}) 201 | target_link_libraries(view_single_cloud ${catkin_LIBRARIES}) 202 | target_link_libraries(single_save_cloud ${catkin_LIBRARIES}) 203 | target_link_libraries(save_cloud ${catkin_LIBRARIES}) 204 | target_link_libraries(zed_cloud_view ${catkin_LIBRARIES}) 205 | target_link_libraries(map_height ${catkin_LIBRARIES}) 206 | target_link_libraries(max_height_estimate ${catkin_LIBRARIES}) 207 | target_link_libraries(plane_height_estimate ${catkin_LIBRARIES}) 208 | target_link_libraries(single_box_filter ${catkin_LIBRARIES}) 209 | target_link_libraries(single_crop_box_filter ${catkin_LIBRARIES}) 210 | target_link_libraries(box_filter ${catkin_LIBRARIES}) 211 | target_link_libraries(dji_tf_broadcast ${catkin_LIBRARIES}) 212 | target_link_libraries(velodyne_tf_broadcast ${catkin_LIBRARIES}) 213 | target_link_libraries(tf_listener ${catkin_LIBRARIES}) 214 | target_link_libraries(transform_cloud_and_save ${catkin_LIBRARIES}) 215 | target_link_libraries(non_dji_tf_listener ${catkin_LIBRARIES}) 216 | target_link_libraries(simulation_tf_listener ${catkin_LIBRARIES}) 217 | target_link_libraries(transform_sim_and_save ${catkin_LIBRARIES}) 218 | target_link_libraries(concatenate_cloud ${catkin_LIBRARIES}) 219 | target_link_libraries(icp ${catkin_LIBRARIES}) 220 | target_link_libraries(transform_and_plane_extraction ${catkin_LIBRARIES}) 221 | target_link_libraries(voxel_filter ${catkin_LIBRARIES}) 222 | target_link_libraries(recenter_cloud ${catkin_LIBRARIES}) 223 | target_link_libraries(cluster_extraction ${catkin_LIBRARIES}) 224 | target_link_libraries(bounding_box ${catkin_LIBRARIES}) 225 | target_link_libraries(known_ground_plane_max_height_estimate ${catkin_LIBRARIES}) 226 | target_link_libraries(voting_scheme ${catkin_LIBRARIES}) 227 | target_link_libraries(fill_clusters ${catkin_LIBRARIES}) 228 | target_link_libraries(cluster_height_estimate ${catkin_LIBRARIES}) 229 | target_link_libraries(crop_clusters ${catkin_LIBRARIES}) 230 | target_link_libraries(turfgrass_heights ${catkin_LIBRARIES}) 231 | target_link_libraries(plot_heights ${catkin_LIBRARIES}) 232 | target_link_libraries(cloud_size ${catkin_LIBRARIES}) 233 | target_link_libraries(perimeter_heights ${catkin_LIBRARIES}) 234 | target_link_libraries(view_colored_plot ${catkin_LIBRARIES}) 235 | target_link_libraries(view_grid ${catkin_LIBRARIES}) 236 | target_link_libraries(view_oriented_grid ${catkin_LIBRARIES}) 237 | target_link_libraries(nnsearch ${catkin_LIBRARIES}) 238 | target_link_libraries(bt ${catkin_LIBRARIES}) 239 | 240 | ############# 241 | ## Install ## 242 | ############# 243 | 244 | # all install targets should use catkin DESTINATION variables 245 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 246 | 247 | ## Mark executable scripts (Python etc.) for installation 248 | ## in contrast to setup.py, you can choose the destination 249 | # install(PROGRAMS 250 | # scripts/my_python_script 251 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 252 | # ) 253 | 254 | ## Mark executables and/or libraries for installation 255 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 256 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 257 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 258 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 259 | # ) 260 | 261 | ## Mark cpp header files for installation 262 | # install(DIRECTORY include/${PROJECT_NAME}/ 263 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 264 | # FILES_MATCHING PATTERN "*.h" 265 | # PATTERN ".svn" EXCLUDE 266 | # ) 267 | 268 | ## Mark other files for installation (e.g. launch and bag files, etc.) 269 | # install(FILES 270 | # # myfile1 271 | # # myfile2 272 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 273 | # ) 274 | 275 | ############# 276 | ## Testing ## 277 | ############# 278 | 279 | ## Add gtest based cpp test target and link libraries 280 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_point_cloud_processing.cpp) 281 | # if(TARGET ${PROJECT_NAME}-test) 282 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 283 | # endif() 284 | 285 | ## Add folders to be run by python nosetests 286 | # catkin_add_nosetests(test) 287 | -------------------------------------------------------------------------------- /point_cloud_processing/launch/hardware_transform.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /point_cloud_processing/launch/sim_transform.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /point_cloud_processing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | point_cloud_processing 4 | 0.0.0 5 | The point_cloud_processing package 6 | 7 | 8 | 9 | 10 | user 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | pcl_conversions 53 | pcl_ros 54 | roscpp 55 | sensor_msgs 56 | libpcl-all-dev 57 | cmake_modules 58 | tf_conversions 59 | eigen_conversions 60 | pcl_conversions 61 | pcl_ros 62 | roscpp 63 | sensor_msgs 64 | tf_conversions 65 | eigen_conversions 66 | libpcl-all 67 | pcl_conversions 68 | pcl_ros 69 | roscpp 70 | sensor_msgs 71 | cmake_modules 72 | tf_conversions 73 | eigen_conversions 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /point_cloud_processing/src/bounding_box.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | unsigned int microseconds = 10000; 16 | 17 | int main(int argc, char** argv) 18 | { 19 | std::list > cluster_list; 20 | if(argc != 3) 21 | { 22 | std::cerr << "Expecting input argument: folder #_of_.pcd_files " << std::endl; 23 | return -1; 24 | } 25 | 26 | std::ostringstream oss; 27 | oss << argv[1] << "cluster_"; 28 | 29 | std::istringstream ss(argv[2]); 30 | int x; 31 | if(!(ss >> x)) { 32 | std::cerr << "Invalid number: " << argv[1] << '\n'; 33 | } 34 | else if (!ss.eof()) { 35 | std::cerr << "Trailing characters after number: " << argv[1] << '\n'; 36 | } 37 | 38 | pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 39 | viewer->setBackgroundColor (0, 0, 0); 40 | viewer->addCoordinateSystem (1.0); 41 | viewer->initCameraParameters (); 42 | viewer->setCameraPosition(0, 0, 43, 0, 0, 0, 0, 0, 0); 43 | for(int i = 0; i < x; i++) { 44 | std::cout << "Count: " << i << std::endl; 45 | std::ostringstream oss2; 46 | oss2 << oss.str() << i << ".pcd"; 47 | pcl::PointCloud cloud; 48 | if(pcl::io::loadPCDFile (oss2.str(), cloud) == -1) 49 | { 50 | PCL_ERROR("Couldn't read file "); 51 | PCL_ERROR(oss.str().c_str()); 52 | PCL_ERROR("\n"); 53 | return (-1); 54 | } 55 | cluster_list.push_back(cloud); 56 | } 57 | 58 | int count = 0; 59 | std::list >::iterator it; 60 | for(it = cluster_list.begin(); it != cluster_list.end(); ++it) { 61 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 62 | pcl::copyPointCloud(*it, *cloud); 63 | 64 | std::ostringstream cluster_number; 65 | cluster_number << "cluster " << count; 66 | std::ostringstream obb_number; 67 | obb_number << "obb " << count; 68 | 69 | double average = 0; 70 | for(size_t i = 0; i < cloud->points.size(); i++) 71 | { 72 | average += cloud->points[i].z; 73 | } 74 | average /= cloud->points.size(); 75 | 76 | for(size_t i = 0; i < cloud->points.size(); i++) 77 | { 78 | cloud->points[i].z = average; 79 | } 80 | 81 | pcl::MomentOfInertiaEstimation feature_extractor; 82 | feature_extractor.setInputCloud (cloud); 83 | feature_extractor.compute (); 84 | 85 | std::vector moment_of_inertia; 86 | std::vector eccentricity; 87 | pcl::PointXYZ min_point_OBB; 88 | pcl::PointXYZ max_point_OBB; 89 | pcl::PointXYZ position_OBB; 90 | Eigen::Matrix3f rotational_matrix_OBB; 91 | float major_value, middle_value, minor_value; 92 | Eigen::Vector3f major_vector, middle_vector, minor_vector; 93 | Eigen::Vector3f mass_center; 94 | 95 | feature_extractor.getMomentOfInertia (moment_of_inertia); 96 | feature_extractor.getEccentricity (eccentricity); 97 | feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); 98 | feature_extractor.getEigenValues (major_value, middle_value, minor_value); 99 | feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); 100 | feature_extractor.getMassCenter (mass_center); 101 | 102 | 103 | viewer->addPointCloud (cloud, cluster_number.str()); 104 | 105 | Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); 106 | Eigen::Quaternionf quat (rotational_matrix_OBB); 107 | viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, obb_number.str()); 108 | 109 | count++; 110 | } 111 | 112 | unsigned int microseconds = 100000; 113 | //viewer->spinOnce (100); 114 | //usleep(microseconds); 115 | 116 | count = 0; 117 | for(it = cluster_list.begin(); it != cluster_list.end(); ++it) { 118 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 119 | //pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 120 | pcl::copyPointCloud(*it, *cloud); 121 | std::ostringstream cluster_number; 122 | cluster_number << "cluster " << count; 123 | std::ostringstream obb_number; 124 | obb_number << "obb " << count; 125 | viewer->removePointCloud(cluster_number.str()); 126 | 127 | for(int i = 0; i < cloud->points.size(); i++) { 128 | cloud->points[i].r = 255; 129 | cloud->points[i].r = 0; 130 | cloud->points[i].r = 0; 131 | } 132 | pcl::visualization::PointCloudColorHandlerCustom rgb(cloud, 255, 0, 0); 133 | viewer->addPointCloud (cloud, rgb, cluster_number.str()); 134 | viewer->spinOnce (100); 135 | 136 | 137 | bool valid = false; 138 | while(!valid) { 139 | viewer->spinOnce (100); 140 | std::cout << "Is this cluster right? (y/n): "; 141 | char input; 142 | std::cin >> input; 143 | if(input == 'y') { 144 | valid = true; 145 | viewer->removePointCloud(cluster_number.str()); 146 | viewer->removeShape(obb_number.str()); 147 | pcl::visualization::PointCloudColorHandlerCustom rgb(cloud, 0, 0, 255); 148 | viewer->addPointCloud (cloud, rgb, cluster_number.str()); 149 | std::ostringstream oss; 150 | oss << "/home/user01/Data/CorrectCluster/cluster_" << count << ".pcd"; 151 | pcl::io::savePCDFileASCII(oss.str(), *cloud); 152 | } 153 | else if(input == 'n') { 154 | valid = true; 155 | viewer->removePointCloud(cluster_number.str()); 156 | viewer->removeShape(obb_number.str()); 157 | pcl::visualization::PointCloudColorHandlerCustom rgb(cloud, 255, 255, 255); 158 | viewer->addPointCloud (cloud, rgb, cluster_number.str()); 159 | } 160 | else { 161 | valid = false; 162 | } 163 | } 164 | 165 | count++; 166 | //viewer->spinOnce(100); 167 | //usleep(microseconds * 3); 168 | } 169 | /* 170 | while(!viewer->wasStopped ()) 171 | { 172 | viewer->spinOnce (100); 173 | usleep(microseconds); 174 | } 175 | */ 176 | return (0); 177 | } 178 | -------------------------------------------------------------------------------- /point_cloud_processing/src/box_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | int count = 0; 18 | std::ostringstream oss; 19 | std::ostringstream oss2; 20 | 21 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 22 | { 23 | 24 | count++; 25 | pcl::PCLPointCloud2 pcl_pc2; 26 | pcl_conversions::toPCL(*input, pcl_pc2); 27 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 28 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 29 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 30 | std::cerr << "Count: " << count << std::endl; 31 | 32 | /* pcl::PointCloud::Ptr cloud_filtered_x(new pcl::PointCloud); 33 | pcl::PassThrough pass_x; 34 | pass_x.setInputCloud(temp_cloud); 35 | pass_x.setFilterFieldName("x"); 36 | pass_x.setFilterLimits(-5.0, 5.0); 37 | pass_x.filter(*cloud_filtered_x); 38 | */ 39 | pcl::PointCloud::Ptr cloud_filtered_y(new pcl::PointCloud); 40 | pcl::PassThrough pass_y; 41 | pass_y.setInputCloud(temp_cloud); 42 | pass_y.setFilterFieldName("y"); 43 | pass_y.setFilterLimits(-5.0, 5.0); 44 | pass_y.filter(*cloud_filtered_y); 45 | 46 | std::ostringstream oss3; 47 | oss3 << oss2.str() << "/filtered_point_cloud_" << count << ".pcd"; 48 | pcl::io::savePCDFileASCII(oss3.str(), *cloud_filtered_y); 49 | 50 | } 51 | 52 | int main(int argc, char** argv) 53 | { 54 | 55 | std::time_t t = std::time(0); 56 | std::tm* now = std::localtime(&t); 57 | 58 | oss << "/home/user01/Data/PointCloud/" << (now->tm_mon + 1) << "-" 59 | << (now->tm_mday) << "-" << (now->tm_year + 1900); 60 | mode_t nMode = 0733; 61 | int nError = 0; 62 | nError = mkdir(oss.str().c_str(), nMode); 63 | 64 | oss2 << "/home/user01/Data/PointCloud/" << (now->tm_mon + 1) << "-" 65 | << (now->tm_mday) << "-" << (now->tm_year + 1900) << "/filtered"; 66 | nError = mkdir(oss2.str().c_str(), nMode); 67 | 68 | ros::init(argc, argv, "point_cloud_processing"); 69 | ros::NodeHandle nh; 70 | 71 | ros::Subscriber sub = nh.subscribe("velodyne_points", 1, cloud_cb); 72 | 73 | ros::spin(); 74 | } 75 | -------------------------------------------------------------------------------- /point_cloud_processing/src/bt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | int count = 0; 9 | 10 | void octomap_cb(const octomap_msgs::Octomap& input) 11 | { 12 | 13 | count++; 14 | std::cout << count << std::endl; 15 | octomap::AbstractOcTree* tree = octomap_msgs::binaryMsgToMap(input); 16 | //tree->write("test.bt"); 17 | octomap::OcTree* ocTree = new octomap::OcTree(0.1); 18 | ocTree = dynamic_cast(tree); 19 | ocTree->writeBinary("test.bt"); 20 | } 21 | 22 | int main(int argc, char** argv) 23 | { 24 | 25 | ros::init(argc, argv, "point_cloud_processing"); 26 | ros::NodeHandle nh; 27 | 28 | ros::Subscriber sub = nh.subscribe("octomap_binary", 1, octomap_cb); 29 | 30 | ros::spin(); 31 | } 32 | -------------------------------------------------------------------------------- /point_cloud_processing/src/cloud_size.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | int main(int argc, char** argv) 9 | { 10 | if(argc != 2) 11 | { 12 | std::cerr << "Expecting input argument: file" << std::endl; 13 | return -1; 14 | } 15 | 16 | std::ostringstream oss; 17 | oss << argv[1]; 18 | 19 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 20 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 21 | { 22 | PCL_ERROR("Couldn't read file "); 23 | PCL_ERROR(oss.str().c_str()); 24 | PCL_ERROR("\n"); 25 | return (-1); 26 | } 27 | 28 | std::cout << temp_cloud->size() << std::endl; 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /point_cloud_processing/src/cluster_extraction.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | int 18 | main (int argc, char** argv) 19 | { 20 | if(argc != 2) 21 | { 22 | std::cerr << "Expecting input argument: file" << std::endl; 23 | return -1; 24 | } 25 | 26 | std::ostringstream oss; 27 | oss << argv[1]; 28 | 29 | pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 30 | unsigned int microseconds = 500000; 31 | 32 | // Read in the cloud data 33 | pcl::PCDReader reader; 34 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud), cloud_f (new pcl::PointCloud); 35 | if(pcl::io::loadPCDFile (oss.str(), *cloud) == -1) 36 | { 37 | PCL_ERROR("Couldn't read file "); 38 | PCL_ERROR(oss.str().c_str()); 39 | PCL_ERROR("\n"); 40 | return (-1); 41 | } 42 | std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* 43 | 44 | // Create the filtering object: downsample the dataset using a leaf size of 1cm 45 | /* 46 | pcl::VoxelGrid vg; 47 | pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); 48 | vg.setInputCloud (cloud); 49 | vg.setLeafSize (0.01f, 0.01f, 0.01f); 50 | vg.filter (*cloud_filtered); 51 | std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* 52 | */ 53 | pcl::PCDWriter writer; 54 | 55 | pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); 56 | cloud_filtered = cloud; 57 | // Create the segmentation object for the planar model and set all the parameters 58 | pcl::SACSegmentation seg; 59 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 60 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 61 | pcl::PointCloud::Ptr cloud_plane (new pcl::PointCloud ()); 62 | seg.setOptimizeCoefficients (true); 63 | seg.setModelType (pcl::SACMODEL_PLANE); 64 | seg.setMethodType (pcl::SAC_RANSAC); 65 | seg.setMaxIterations (100); 66 | seg.setDistanceThreshold (0.2); 67 | 68 | int i=0, nr_points = (int) cloud_filtered->points.size (); 69 | 70 | // Segment the largest planar component from the remaining cloud 71 | seg.setInputCloud (cloud_filtered); 72 | seg.segment (*inliers, *coefficients); 73 | if (inliers->indices.size () == 0) 74 | { 75 | std::cout << "Could not estimate a planar model for the given dataset." << std::endl; 76 | return 0; 77 | } 78 | 79 | // Extract the planar inliers from the input cloud 80 | pcl::ExtractIndices extract; 81 | extract.setInputCloud (cloud_filtered); 82 | extract.setIndices (inliers); 83 | extract.setNegative (false); 84 | 85 | // Get the points associated with the planar surface 86 | extract.filter (*cloud_plane); 87 | std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; 88 | 89 | // Remove the planar inliers, extract the rest 90 | extract.setNegative (true); 91 | extract.filter (*cloud_f); 92 | *cloud_filtered = *cloud_f; 93 | 94 | 95 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 96 | cloud_rgb->points.resize(cloud_filtered->size()); 97 | 98 | // Creating the KdTree object for the search method of the extraction 99 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); 100 | tree->setInputCloud (cloud_filtered); 101 | std::vector cluster_indices; 102 | pcl::EuclideanClusterExtraction ec; 103 | ec.setClusterTolerance (0.1); // 2cm 104 | ec.setMinClusterSize (25); 105 | ec.setMaxClusterSize (25000); 106 | ec.setSearchMethod (tree); 107 | ec.setInputCloud (cloud_filtered); 108 | 109 | ec.extract (cluster_indices); 110 | 111 | int j = 0; 112 | for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) 113 | { 114 | for(size_t i = 0; i < cloud_filtered->points.size(); i++) 115 | { 116 | cloud_rgb->points[i].x = cloud_filtered->points[i].x; 117 | cloud_rgb->points[i].y = cloud_filtered->points[i].y; 118 | cloud_rgb->points[i].z = cloud_filtered->points[i].z; 119 | cloud_rgb->points[i].r = 0; 120 | cloud_rgb->points[i].g = 0; 121 | cloud_rgb->points[i].b = 255; 122 | } 123 | pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); 124 | for (std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) { 125 | cloud_cluster->points.push_back (cloud_filtered->points[*pit]); 126 | cloud_rgb->points[*pit].r = 255; 127 | cloud_rgb->points[*pit].g = 0; 128 | cloud_rgb->points[*pit].b = 0; 129 | }//* 130 | cloud_cluster->width = cloud_cluster->points.size (); 131 | cloud_cluster->height = 1; 132 | cloud_cluster->is_dense = true; 133 | 134 | std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; 135 | std::stringstream ss; 136 | ss << "cloud_cluster_" << j << ".pcd"; 137 | writer.write (ss.str (), *cloud_cluster, false); //* 138 | viewer.showCloud(cloud_rgb); 139 | usleep(microseconds); 140 | j++; 141 | } 142 | 143 | return (0); 144 | } -------------------------------------------------------------------------------- /point_cloud_processing/src/cluster_height_estimate.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | //#include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | struct Vector3 22 | { 23 | double x; 24 | double y; 25 | double z; 26 | }; 27 | 28 | int main(int argc, char** argv) 29 | { 30 | if(argc != 4) 31 | { 32 | std::cerr << "param_file point_cloud_file clusters_file" << std::endl; 33 | return -1; 34 | } 35 | 36 | double distance_threshold = 0; 37 | 38 | std::ifstream infile(argv[1]); 39 | 40 | int count = 0; 41 | float param = 0; 42 | while(infile >> param) { 43 | switch(count) { 44 | case 0: 45 | distance_threshold = param; 46 | break; 47 | default: 48 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 49 | break; 50 | } 51 | count++; 52 | } 53 | 54 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 55 | if(pcl::io::loadPCDFile (argv[2], *temp_cloud) == -1) 56 | { 57 | PCL_ERROR("Couldn't read file "); 58 | PCL_ERROR(argv[2]); 59 | PCL_ERROR("\n"); 60 | return (-1); 61 | } 62 | 63 | pcl::PointCloud::Ptr bodyFiltered(new pcl::PointCloud); 64 | 65 | std::ofstream height_file; 66 | height_file.open("/home/naik/Data/PointCloud/heights.csv"); 67 | height_file << "Cluster #,Height (meters)\n"; 68 | 69 | std::ifstream infile2(argv[3]); 70 | std::string line; 71 | int linecount = 0; 72 | while(std::getline(infile2, line)) 73 | { 74 | if(linecount == 0) 75 | { 76 | linecount++; 77 | continue; 78 | } 79 | std::istringstream s(line); 80 | std::string field; 81 | int fieldcount = 0; 82 | int clustercount = -1; 83 | double rotation = -1; 84 | double height = -1; 85 | double width = -1; 86 | double box_x = -1; 87 | double box_y = -1; 88 | while(getline(s, field, ',')) 89 | { 90 | if(fieldcount == 0) 91 | clustercount = std::atoi(field.c_str()); 92 | else if(fieldcount == 1) 93 | rotation = std::atof(field.c_str()); 94 | else if(fieldcount == 2) 95 | width = std::atof(field.c_str()); 96 | else if(fieldcount == 3) 97 | height = std::atof(field.c_str()); 98 | else if(fieldcount == 4) 99 | box_x = std::atof(field.c_str()); 100 | else if(fieldcount == 5) 101 | box_y = std::atof(field.c_str()); 102 | /* std::cout << clustercount << std::endl; 103 | std::cout << rotation << std::endl; 104 | std::cout << width << std::endl; 105 | std::cout << height << std::endl; 106 | std::cout << box_x << std::endl; 107 | std::cout << box_y << std::endl; 108 | */ 109 | fieldcount++; 110 | } 111 | 112 | height_file << clustercount << ","; 113 | 114 | //height += 4; 115 | pcl::CropBox boxFilter; 116 | boxFilter.setTranslation(Eigen::Vector3f(box_x, box_y, 0)); 117 | boxFilter.setRotation(Eigen::Vector3f(0, 0, rotation)); 118 | boxFilter.setMin(Eigen::Vector4f(0 - (height/2), 0 - (width/2), -100, 1.0)); 119 | boxFilter.setMax(Eigen::Vector4f(0 + (height/2), 0 + (width/2), 100, 1.0)); 120 | boxFilter.setInputCloud(temp_cloud); 121 | boxFilter.filter(*bodyFiltered); 122 | 123 | 124 | 125 | 126 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 127 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 128 | // Create segmentation object 129 | pcl::SACSegmentation seg; 130 | //Optional 131 | seg.setOptimizeCoefficients(true); 132 | seg.setAxis(Eigen::Vector3f(0,0,1)); 133 | // Mandatory 134 | seg.setModelType(pcl::SACMODEL_PLANE); 135 | seg.setMethodType(pcl::SAC_RANSAC); 136 | seg.setDistanceThreshold(distance_threshold); 137 | 138 | seg.setInputCloud(bodyFiltered); 139 | seg.segment(*inliers, *coefficients); 140 | 141 | if(inliers->indices.size() == 0) 142 | { 143 | PCL_ERROR("Could not estimate a planar model for the given dataset."); 144 | return -1; 145 | } 146 | 147 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 148 | cloud_rgb->points.resize(bodyFiltered->size() + 2); 149 | for(size_t i = 0; i < bodyFiltered->points.size(); i++) 150 | { 151 | cloud_rgb->points[i].x = bodyFiltered->points[i].x; 152 | cloud_rgb->points[i].y = bodyFiltered->points[i].y; 153 | cloud_rgb->points[i].z = bodyFiltered->points[i].z; 154 | cloud_rgb->points[i].r = 0; 155 | cloud_rgb->points[i].g = 0; 156 | cloud_rgb->points[i].b = 255; 157 | } 158 | 159 | for(size_t i = 0; i < inliers->indices.size(); ++i) 160 | { 161 | cloud_rgb->points[inliers->indices[i]].r = 255; 162 | cloud_rgb->points[inliers->indices[i]].g = 0; 163 | cloud_rgb->points[inliers->indices[i]].b = 0; 164 | } 165 | 166 | Vector3 sum; 167 | sum.x = 0; 168 | sum.y = 0; 169 | sum.z = 0; 170 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 171 | { 172 | if(cloud_rgb->points[i].r == 255) 173 | { 174 | sum.x += cloud_rgb->points[i].x; 175 | sum.y += cloud_rgb->points[i].y; 176 | sum.z += cloud_rgb->points[i].z; 177 | } 178 | } 179 | 180 | Vector3 centroid; 181 | centroid.x = sum.x / inliers->indices.size(); 182 | centroid.y = sum.y / inliers->indices.size(); 183 | centroid.z = sum.z / inliers->indices.size(); 184 | 185 | double xx = 0.0; 186 | double xy = 0.0; 187 | double xz = 0.0; 188 | double yy = 0.0; 189 | double yz = 0.0; 190 | double zz = 0.0; 191 | 192 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 193 | { 194 | if(cloud_rgb->points[i].r == 255) 195 | { 196 | Vector3 r; 197 | r.x = cloud_rgb->points[i].x - centroid.x; 198 | r.y = cloud_rgb->points[i].y - centroid.y; 199 | r.z = cloud_rgb->points[i].z - centroid.z; 200 | xx += r.x * r.x; 201 | xy += r.x * r.y; 202 | xz += r.x * r.z; 203 | yy += r.y * r.y; 204 | yz += r.y * r.z; 205 | zz += r.z * r.z; 206 | } 207 | } 208 | 209 | double det_x = yy*zz - yz*yz; 210 | double det_y = xx*zz - xz*xz; 211 | double det_z = xx*yy - xy*xy; 212 | 213 | double max = det_x; 214 | int max_letter = 1; 215 | if(det_y > max) 216 | { 217 | max = det_y; 218 | max_letter = 2; 219 | } 220 | if(det_z > max) 221 | { 222 | max = det_z; 223 | max_letter = 3; 224 | } 225 | 226 | Vector3 dir; 227 | if(max_letter == 1) 228 | { 229 | dir.x = det_x; 230 | dir.y = xz*yz - xy*zz; 231 | dir.z = xy*yz - xz*yy; 232 | } 233 | else if(max_letter == 2) 234 | { 235 | dir.x = xz*yz - xy*zz; 236 | dir.y = det_y; 237 | dir.z = xy*xz - yz*xx; 238 | } 239 | else if(max_letter == 3) 240 | { 241 | dir.x = xy*yz - xz*yy; 242 | dir.y = xy*xz - yz*xx; 243 | dir.z = det_z; 244 | } 245 | 246 | double length = (dir.x * dir.x) + (dir.y * dir.y) + (dir.z * dir.z); 247 | length = sqrt(length); 248 | 249 | dir.x = dir.x / length; 250 | dir.y = dir.y / length; 251 | dir.z = dir.z / length; 252 | 253 | double distance = 0; 254 | Vector3 subtract; 255 | Vector3 point_max; 256 | int index = -1; 257 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 258 | { 259 | if(cloud_rgb->points[i].b == 255) 260 | { 261 | double temp_distance = 0; 262 | subtract.x = cloud_rgb->points[i].x - centroid.x; 263 | subtract.y = cloud_rgb->points[i].y - centroid.y; 264 | subtract.z = cloud_rgb->points[i].z - centroid.z; 265 | temp_distance = (dir.x * subtract.x) + (dir.y * subtract.y) + (dir.z * subtract.z); 266 | 267 | double mag_dist = std::abs(temp_distance); 268 | 269 | //height_file << mag_dist << ","; 270 | 271 | if(mag_dist > distance) { 272 | distance = mag_dist; 273 | point_max.x = cloud_rgb->points[i].x; 274 | point_max.y = cloud_rgb->points[i].y; 275 | point_max.z = cloud_rgb->points[i].z; 276 | index = i; 277 | } 278 | } 279 | } 280 | 281 | std::cerr << "Max distance in frame(meters): " << distance << " meters." << std::endl; 282 | //std::cerr << "Max distance in frame(inches): " << (distance * 39.3701) << " inches." << std::endl; 283 | height_file << distance << ","; 284 | height_file << std::endl; 285 | 286 | } 287 | 288 | /* 289 | int size = cloud_rgb->size(); 290 | cloud_rgb->points[size - 1].x = centroid.x; 291 | cloud_rgb->points[size - 1].y = centroid.y; 292 | cloud_rgb->points[size - 1].z = centroid.z; 293 | cloud_rgb->points[size - 1].r = 255; 294 | cloud_rgb->points[size - 1].g = 255; 295 | cloud_rgb->points[size - 1].b = 255; 296 | 297 | cloud_rgb->points[size - 2].x = dir.x; 298 | cloud_rgb->points[size - 2].y = dir.y; 299 | cloud_rgb->points[size - 2].z = dir.z; 300 | cloud_rgb->points[size - 2].r = 255; 301 | cloud_rgb->points[size - 2].g = 165; 302 | cloud_rgb->points[size - 2].b = 0; 303 | 304 | cloud_rgb->points[index].r = 0; 305 | cloud_rgb->points[index].g = 255; 306 | cloud_rgb->points[index].b = 0; 307 | 308 | pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 309 | viewer->setBackgroundColor (0, 0, 0); 310 | viewer->addCoordinateSystem (1.0); 311 | viewer->initCameraParameters (); 312 | viewer->setCameraPosition(0, 0, 43, 0, 0, 0, 0, 0, 0); 313 | pcl::visualization::PointCloudColorHandlerCustom rgb (temp_cloud, 0, 0, 255); 314 | viewer->addPointCloud (temp_cloud, rgb, "full"); 315 | //viewer->addPointCloud (cloud_rgb, rgb, "Cloud"); 316 | viewer->addPointCloud (cloud_rgb, "test"); 317 | 318 | unsigned int microseconds = 100000; 319 | 320 | while(!viewer->wasStopped ()) 321 | { 322 | viewer->spinOnce (100); 323 | usleep(microseconds); 324 | }*/ 325 | return 0; 326 | } 327 | -------------------------------------------------------------------------------- /point_cloud_processing/src/concatenate_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | int main(int argc, char** argv) 12 | { 13 | if(argc != 3) 14 | { 15 | std::cerr << "Expecting input argument: folder #_of_.pcd_files " << std::endl; 16 | return -1; 17 | } 18 | 19 | pcl::PointCloud concatenated_cloud; 20 | 21 | std::ostringstream oss; 22 | oss << argv[1] << "point_cloud_"; 23 | 24 | std::istringstream ss(argv[2]); 25 | int x; 26 | if(!(ss >> x)) { 27 | std::cerr << "Invalid number: " << argv[1] << '\n'; 28 | } 29 | else if (!ss.eof()) { 30 | std::cerr << "Trailing characters after number: " << argv[1] << '\n'; 31 | } 32 | 33 | for(int i = 1; i <= x; i++) { 34 | std::cout << "Count: " << i << std::endl; 35 | std::ostringstream oss2; 36 | oss2 << oss.str() << i << ".pcd"; 37 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 38 | if(pcl::io::loadPCDFile (oss2.str(), *temp_cloud) == -1) 39 | { 40 | PCL_ERROR("Couldn't read file "); 41 | PCL_ERROR(oss.str().c_str()); 42 | PCL_ERROR("\n"); 43 | return (-1); 44 | } 45 | 46 | concatenated_cloud += *temp_cloud; 47 | } 48 | 49 | std::cout << "Done concatenating." << std::endl; 50 | std::cout << "SAVING FILE! DO NOT EXIT YET!" << std::endl; 51 | 52 | pcl::io::savePCDFileASCII("/home/naik/Data/PointCloud/concatenated_cloud.pcd", concatenated_cloud); 53 | 54 | std::cout << "Done saving file." << std::endl; 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /point_cloud_processing/src/crop_clusters.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | struct Vector3 20 | { 21 | double x; 22 | double y; 23 | double z; 24 | }; 25 | 26 | int main(int argc, char** argv) 27 | { 28 | if(argc != 3) 29 | { 30 | std::cerr << "point_cloud_file clusters_file" << std::endl; 31 | return -1; 32 | } 33 | 34 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 35 | if(pcl::io::loadPCDFile (argv[1], *temp_cloud) == -1) 36 | { 37 | PCL_ERROR("Couldn't read file "); 38 | PCL_ERROR(argv[2]); 39 | PCL_ERROR("\n"); 40 | return (-1); 41 | } 42 | 43 | pcl::PointCloud::Ptr bodyFiltered(new pcl::PointCloud); 44 | 45 | std::ifstream infile2(argv[2]); 46 | std::string line; 47 | int linecount = 0; 48 | while(std::getline(infile2, line)) 49 | { 50 | if(linecount == 0) 51 | { 52 | linecount++; 53 | continue; 54 | } 55 | std::istringstream s(line); 56 | std::string field; 57 | int fieldcount = 0; 58 | int clustercount = -1; 59 | double rotation = -1; 60 | double height = -1; 61 | double width = -1; 62 | double box_x = -1; 63 | double box_y = -1; 64 | while(getline(s, field, ',')) 65 | { 66 | if(fieldcount == 0) 67 | clustercount = std::atoi(field.c_str()); 68 | else if(fieldcount == 1) 69 | rotation = std::atof(field.c_str()); 70 | else if(fieldcount == 2) 71 | width = std::atof(field.c_str()); 72 | else if(fieldcount == 3) 73 | height = std::atof(field.c_str()); 74 | else if(fieldcount == 4) 75 | box_x = std::atof(field.c_str()); 76 | else if(fieldcount == 5) 77 | box_y = std::atof(field.c_str()); 78 | /* std::cout << clustercount << std::endl; 79 | std::cout << rotation << std::endl; 80 | std::cout << width << std::endl; 81 | std::cout << height << std::endl; 82 | std::cout << box_x << std::endl; 83 | std::cout << box_y << std::endl; 84 | */ 85 | fieldcount++; 86 | } 87 | 88 | pcl::CropBox boxFilter; 89 | boxFilter.setTranslation(Eigen::Vector3f(box_x, box_y, 0)); 90 | boxFilter.setRotation(Eigen::Vector3f(0, 0, rotation)); 91 | boxFilter.setMin(Eigen::Vector4f(0 - (height/2), 0 - (width/2), -100, 1.0)); 92 | boxFilter.setMax(Eigen::Vector4f(0 + (height/2), 0 + (width/2), 100, 1.0)); 93 | boxFilter.setInputCloud(temp_cloud); 94 | boxFilter.filter(*bodyFiltered); 95 | 96 | std::ostringstream oss; 97 | oss << "/home/naik/Data/clusters/point_cloud_" << clustercount + 1 << ".pcd"; 98 | pcl::io::savePCDFileASCII(oss.str(), *bodyFiltered); 99 | 100 | 101 | } 102 | 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /point_cloud_processing/src/dji_tf_broadcast.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | void callback(const geometry_msgs::PointStampedConstPtr& position_data, const geometry_msgs::QuaternionStampedConstPtr& attitude_data) { 11 | 12 | std::cout << "Position Data: " << position_data->point.x << " " << position_data->point.y << " " << position_data->point.z << std::endl; 13 | std::cout << "Attitude Data: " << attitude_data->quaternion.x << " " << attitude_data->quaternion.y << " " << attitude_data->quaternion.z << " " << attitude_data->quaternion.w << std::endl; 14 | static tf::TransformBroadcaster br; 15 | tf::Transform transform; 16 | transform.setOrigin( tf::Vector3(position_data->point.x, position_data->point.y, position_data->point.z)); 17 | tf::Quaternion q(attitude_data->quaternion.x, attitude_data->quaternion.y, attitude_data->quaternion.z, attitude_data->quaternion.w); 18 | transform.setRotation(q); 19 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "dji")); 20 | } 21 | 22 | int main(int argc, char** argv){ 23 | ros::init(argc, argv, "dji_tf_broadcaster"); 24 | 25 | ros::NodeHandle nh; 26 | 27 | message_filters::Subscriber position_sub(nh, "/dji_sdk/local_position", 1); 28 | message_filters::Subscriber attitude_sub(nh, "/dji_sdk/attitude", 1); 29 | 30 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 31 | // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) 32 | message_filters::Synchronizer sync(MySyncPolicy(10), position_sub, attitude_sub); 33 | sync.registerCallback(boost::bind(&callback, _1, _2)); 34 | 35 | ros::spin(); 36 | return 0; 37 | }; -------------------------------------------------------------------------------- /point_cloud_processing/src/fill_clusters.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | unsigned int microseconds = 10000; 18 | 19 | int main(int argc, char** argv) 20 | { 21 | std::list > cluster_list; 22 | if(argc != 3) 23 | { 24 | std::cerr << "Expecting input argument: folder #_of_.pcd_files " << std::endl; 25 | return -1; 26 | } 27 | 28 | std::ostringstream oss; 29 | oss << argv[1] << "cluster_"; 30 | 31 | std::istringstream ss(argv[2]); 32 | int x; 33 | if(!(ss >> x)) { 34 | std::cerr << "Invalid number: " << argv[1] << '\n'; 35 | } 36 | else if (!ss.eof()) { 37 | std::cerr << "Trailing characters after number: " << argv[1] << '\n'; 38 | } 39 | 40 | pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 41 | viewer->setBackgroundColor (0, 0, 0); 42 | viewer->addCoordinateSystem (1.0); 43 | viewer->initCameraParameters (); 44 | viewer->setCameraPosition(0, 0, 43, 0, 0, 0, 0, 0, 0); 45 | for(int i = 0; i < x; i++) { 46 | std::cout << "Count: " << i << std::endl; 47 | std::ostringstream oss2; 48 | oss2 << oss.str() << i << ".pcd"; 49 | pcl::PointCloud cloud; 50 | if(pcl::io::loadPCDFile (oss2.str(), cloud) == -1) 51 | { 52 | PCL_ERROR("Couldn't read file "); 53 | PCL_ERROR(oss.str().c_str()); 54 | PCL_ERROR("\n"); 55 | return (-1); 56 | } 57 | cluster_list.push_back(cloud); 58 | } 59 | 60 | int count = 0; 61 | std::list >::iterator it; 62 | for(it = cluster_list.begin(); it != cluster_list.end(); ++it) { 63 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 64 | pcl::copyPointCloud(*it, *cloud); 65 | 66 | std::ostringstream cluster_number; 67 | cluster_number << "cluster " << count; 68 | std::ostringstream obb_number; 69 | obb_number << "obb " << count; 70 | 71 | double average = 0; 72 | for(size_t i = 0; i < cloud->points.size(); i++) 73 | { 74 | average += cloud->points[i].z; 75 | } 76 | average /= cloud->points.size(); 77 | 78 | for(size_t i = 0; i < cloud->points.size(); i++) 79 | { 80 | cloud->points[i].z = average; 81 | } 82 | 83 | pcl::MomentOfInertiaEstimation feature_extractor; 84 | feature_extractor.setInputCloud (cloud); 85 | feature_extractor.compute (); 86 | 87 | std::vector moment_of_inertia; 88 | std::vector eccentricity; 89 | pcl::PointXYZ min_point_OBB; 90 | pcl::PointXYZ max_point_OBB; 91 | pcl::PointXYZ position_OBB; 92 | Eigen::Matrix3f rotational_matrix_OBB; 93 | float major_value, middle_value, minor_value; 94 | Eigen::Vector3f major_vector, middle_vector, minor_vector; 95 | Eigen::Vector3f mass_center; 96 | 97 | feature_extractor.getMomentOfInertia (moment_of_inertia); 98 | feature_extractor.getEccentricity (eccentricity); 99 | feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); 100 | feature_extractor.getEigenValues (major_value, middle_value, minor_value); 101 | feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); 102 | feature_extractor.getMassCenter (mass_center); 103 | 104 | 105 | viewer->addPointCloud (cloud, cluster_number.str()); 106 | 107 | Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); 108 | Eigen::Quaternionf quat (rotational_matrix_OBB); 109 | //viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, obb_number.str()); 110 | 111 | count++; 112 | } 113 | 114 | std::ifstream average_file ("/home/user01/Data/averages.csv"); 115 | std::string value; 116 | int count2 = 0; 117 | double orientation_average = -1.0; 118 | double width_average = -1.0; 119 | double height_average = -1.0; 120 | 121 | while(average_file.good()) { 122 | getline(average_file, value, ','); 123 | std::cout << value << std::endl; 124 | if(count2 == 0) { 125 | orientation_average = std::atof(value.c_str()); 126 | } 127 | else if(count2 == 1) { 128 | width_average = std::atof(value.c_str()); 129 | } 130 | else if(count2 == 2) { 131 | height_average = std::atof(value.c_str()); 132 | } 133 | count2++; 134 | } 135 | 136 | orientation_average += 0.03; 137 | height_average -= 0.4; 138 | float roll = 0, pitch = 0, yaw = orientation_average; 139 | Eigen::Quaternionf q; 140 | q = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) 141 | * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) 142 | * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 143 | 144 | std::ofstream cluster_file; 145 | cluster_file.open("/home/user01/Data/correct_clusters_params.csv"); 146 | cluster_file << "Cluster #,Rotation,Height,Width,X,Y\n"; 147 | int cluster_count = 0; 148 | for(count2 = 0; count2 < 28; count2++) 149 | { 150 | std::ostringstream cube_number; 151 | cube_number << "cube_" << count2; 152 | double width = -10.54 + (count2 * 1.122222); 153 | double height = -18.07222222222 + (count2 * 0.955555); 154 | Eigen::Vector3f position (width, height, 0); 155 | viewer->addCube (position, q, width_average, height_average, 0, cube_number.str()); 156 | cluster_file << cluster_count << "," << orientation_average << "," << height_average << "," << width_average 157 | << "," << width << "," << height << "\n"; 158 | cluster_count++; 159 | for(int i = 1; i < 4; i++) { 160 | cube_number << "_" << i; 161 | double width2 = width + (i * -3.25); 162 | double height2 = height + (i * 3.85); 163 | Eigen::Vector3f position2 (width2, height2, 0); 164 | viewer->addCube (position2, q, width_average, height_average, 0, cube_number.str()); 165 | cluster_file << cluster_count << "," << orientation_average << "," << height_average << "," << width_average 166 | << "," << width2 << "," << height2 << "\n"; 167 | cluster_count++; 168 | } 169 | } 170 | 171 | cluster_file.close(); 172 | 173 | // Eigen::Vector3f position (10.9, 6.8, 0); 174 | // //Eigen::Quaternionf quat (rotational_matrix_OBB); 175 | // viewer->addCube (position, q, width_average, height_average, 0, "test"); 176 | // Eigen::Vector3f position2 (0.8, -1.8, 0); 177 | // //Eigen::Quaternionf quat (rotational_matrix_OBB); 178 | // viewer->addCube (position2, q, width_average, height_average, 0, "test2"); 179 | // Eigen::Vector3f position3 (8.656, 4.89, 0); 180 | // //Eigen::Quaternionf quat (rotational_matrix_OBB); 181 | // viewer->addCube (position3, q, width_average, height_average, 0, "test3"); 182 | 183 | unsigned int microseconds = 100000; 184 | 185 | while(!viewer->wasStopped ()) 186 | { 187 | viewer->spinOnce (100); 188 | usleep(microseconds); 189 | } 190 | 191 | return (0); 192 | } 193 | -------------------------------------------------------------------------------- /point_cloud_processing/src/icp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | int main (int argc, char** argv) 9 | { 10 | if(argc != 3) 11 | { 12 | std::cerr << "Expecting input argument: folder #_of_.pcd_files " << std::endl; 13 | return -1; 14 | } 15 | 16 | std::ostringstream oss; 17 | oss << argv[1] << "point_cloud_"; 18 | 19 | std::istringstream ss(argv[2]); 20 | int x; 21 | if(!(ss >> x)) { 22 | std::cerr << "Invalid number: " << argv[1] << '\n'; 23 | } 24 | else if (!ss.eof()) { 25 | std::cerr << "Trailing characters after number: " << argv[1] << '\n'; 26 | } 27 | 28 | pcl::PointCloud::Ptr Final(new pcl::PointCloud); 29 | std::ostringstream oss2; 30 | oss2 << oss.str() << 1 << ".pcd"; 31 | if(pcl::io::loadPCDFile (oss2.str(), *Final) == -1) 32 | { 33 | PCL_ERROR("Couldn't read file "); 34 | PCL_ERROR(oss.str().c_str()); 35 | PCL_ERROR("\n"); 36 | return (-1); 37 | } 38 | pcl::IterativeClosestPoint icp; 39 | pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 40 | 41 | for(int i = 2; i <= x; i++) { 42 | std::cout << "Count: " << i << std::endl; 43 | std::ostringstream oss2; 44 | oss2 << oss.str() << i << ".pcd"; 45 | pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); 46 | if(pcl::io::loadPCDFile (oss2.str(), *cloud_out) == -1) 47 | { 48 | PCL_ERROR("Couldn't read file "); 49 | PCL_ERROR(oss.str().c_str()); 50 | PCL_ERROR("\n"); 51 | return (-1); 52 | } 53 | icp.setInputSource(Final); 54 | icp.setInputTarget(cloud_out); 55 | icp.align(*Final); 56 | std::cout << "has converged:" << icp.hasConverged() << " score: " << 57 | icp.getFitnessScore() << std::endl; 58 | std::cout << icp.getFinalTransformation() << std::endl; 59 | viewer.showCloud(Final); 60 | } 61 | 62 | return (0); 63 | } -------------------------------------------------------------------------------- /point_cloud_processing/src/known_ground_plane_max_height_estimate.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | struct Vector3 19 | { 20 | double x; 21 | double y; 22 | double z; 23 | }; 24 | 25 | int main(int argc, char** argv) 26 | { 27 | if(argc != 3) 28 | { 29 | std::cerr << "ground_plane_file point_cloud_file" << std::endl; 30 | return -1; 31 | } 32 | 33 | double distance_threshold = 0; 34 | 35 | pcl::PointCloud::Ptr ground_plane(new pcl::PointCloud); 36 | if(pcl::io::loadPCDFile (argv[1], *ground_plane) == -1) 37 | { 38 | PCL_ERROR("Couldn't read file "); 39 | PCL_ERROR(argv[1]); 40 | PCL_ERROR("\n"); 41 | return (-1); 42 | } 43 | 44 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 45 | if(pcl::io::loadPCDFile (argv[2], *temp_cloud) == -1) 46 | { 47 | PCL_ERROR("Couldn't read file "); 48 | PCL_ERROR(argv[2]); 49 | PCL_ERROR("\n"); 50 | return (-1); 51 | } 52 | 53 | Vector3 sum; 54 | sum.x = 0; 55 | sum.y = 0; 56 | sum.z = 0; 57 | for(size_t i = 0; i < ground_plane->points.size(); i++) 58 | { 59 | sum.x += ground_plane->points[i].x; 60 | sum.y += ground_plane->points[i].y; 61 | sum.z += ground_plane->points[i].z; 62 | } 63 | 64 | Vector3 centroid; 65 | centroid.x = sum.x / ground_plane->points.size(); 66 | centroid.y = sum.y / ground_plane->points.size(); 67 | centroid.z = sum.z / ground_plane->points.size(); 68 | 69 | double xx = 0.0; 70 | double xy = 0.0; 71 | double xz = 0.0; 72 | double yy = 0.0; 73 | double yz = 0.0; 74 | double zz = 0.0; 75 | 76 | for(size_t i = 0; i < ground_plane->points.size(); i++) 77 | { 78 | 79 | Vector3 r; 80 | r.x = ground_plane->points[i].x - centroid.x; 81 | r.y = ground_plane->points[i].y - centroid.y; 82 | r.z = ground_plane->points[i].z - centroid.z; 83 | xx += r.x * r.x; 84 | xy += r.x * r.y; 85 | xz += r.x * r.z; 86 | yy += r.y * r.y; 87 | yz += r.y * r.z; 88 | zz += r.z * r.z; 89 | } 90 | 91 | double det_x = yy*zz - yz*yz; 92 | double det_y = xx*zz - xz*xz; 93 | double det_z = xx*yy - xy*xy; 94 | 95 | double max = det_x; 96 | int max_letter = 1; 97 | if(det_y > max) 98 | { 99 | max = det_y; 100 | max_letter = 2; 101 | } 102 | if(det_z > max) 103 | { 104 | max = det_z; 105 | max_letter = 3; 106 | } 107 | 108 | Vector3 dir; 109 | if(max_letter == 1) 110 | { 111 | dir.x = det_x; 112 | dir.y = xz*yz - xy*zz; 113 | dir.z = xy*yz - xz*yy; 114 | } 115 | else if(max_letter == 2) 116 | { 117 | dir.x = xz*yz - xy*zz; 118 | dir.y = det_y; 119 | dir.z = xy*xz - yz*xx; 120 | } 121 | else if(max_letter == 3) 122 | { 123 | dir.x = xy*yz - xz*yy; 124 | dir.y = xy*xz - yz*xx; 125 | dir.z = det_z; 126 | } 127 | 128 | double length = (dir.x * dir.x) + (dir.y * dir.y) + (dir.z * dir.z); 129 | length = sqrt(length); 130 | 131 | dir.x = dir.x / length; 132 | dir.y = dir.y / length; 133 | dir.z = dir.z / length; 134 | 135 | double distance = 0; 136 | Vector3 subtract; 137 | Vector3 point_max; 138 | int index = -1; 139 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 140 | { 141 | double temp_distance = 0; 142 | subtract.x = temp_cloud->points[i].x - centroid.x; 143 | subtract.y = temp_cloud->points[i].y - centroid.y; 144 | subtract.z = temp_cloud->points[i].z - centroid.z; 145 | temp_distance = (dir.x * subtract.x) + (dir.y * subtract.y) + (dir.z * subtract.z); 146 | 147 | std::cout << temp_distance << std::endl; 148 | //std::cout << subtract.y << std::endl; 149 | //std::cout << subtract.z << std::endl; 150 | //std::cout << yy << std::endl; 151 | //std::cout << yz << std::endl; 152 | double mag_dist = std::abs(temp_distance); 153 | if(mag_dist > distance) { 154 | distance = mag_dist; 155 | point_max.x = temp_cloud->points[i].x; 156 | point_max.y = temp_cloud->points[i].y; 157 | point_max.z = temp_cloud->points[i].z; 158 | index = i; 159 | } 160 | } 161 | 162 | std::cerr << "Max distance in frame(meters): " << distance << " meters." << std::endl; 163 | std::cerr << "Max distance in frame(inches): " << (distance * 39.3701) << " inches." << std::endl; 164 | 165 | pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 166 | viewer.showCloud(ground_plane); 167 | while(!viewer.wasStopped ()) 168 | { 169 | } 170 | 171 | return 0; 172 | } 173 | -------------------------------------------------------------------------------- /point_cloud_processing/src/map_height.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | int count = 0; 25 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 26 | 27 | struct Vector3 28 | { 29 | double x; 30 | double y; 31 | double z; 32 | }; 33 | 34 | void callback(const sensor_msgs::PointCloud2ConstPtr& pointcloud_data, const sensor_msgs::NavSatFixConstPtr& gps_data) 35 | { 36 | ros::Time time_data = ros::Time::now(); 37 | 38 | std::cout << time_data.sec << std::endl; 39 | count++; 40 | ofstream myfile; 41 | myfile.open("map_height_data.csv", ios::app); 42 | 43 | pcl::PCLPointCloud2 pcl_pc2; 44 | pcl_conversions::toPCL(*pointcloud_data, pcl_pc2); 45 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 46 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 47 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 48 | std::cerr << "Count: " << count << std::endl; 49 | 50 | pcl::PointCloud::Ptr cloud_filtered_y(new pcl::PointCloud); 51 | pcl::PassThrough pass_y; 52 | pass_y.setInputCloud(temp_cloud); 53 | pass_y.setFilterFieldName("y"); 54 | pass_y.setFilterLimits(-1.5, 1.5); 55 | pass_y.filter(*cloud_filtered_y); 56 | 57 | pcl::PointCloud::Ptr cloud_filtered_z(new pcl::PointCloud); 58 | pcl::PassThrough pass_z; 59 | pass_z.setInputCloud(cloud_filtered_y); 60 | pass_z.setFilterFieldName("z"); 61 | pass_z.setFilterLimits(-1.5, 1.5); 62 | pass_z.filter(*cloud_filtered_z); 63 | 64 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 65 | cloud_rgb->points.resize(cloud_filtered_z->size() + 1); 66 | for(size_t i = 0; i < cloud_filtered_z->points.size(); i++) 67 | { 68 | cloud_rgb->points[i].x = cloud_filtered_z->points[i].x; 69 | cloud_rgb->points[i].y = cloud_filtered_z->points[i].y; 70 | cloud_rgb->points[i].z = cloud_filtered_z->points[i].z; 71 | cloud_rgb->points[i].r = 0; 72 | cloud_rgb->points[i].g = 0; 73 | cloud_rgb->points[i].b = 255; 74 | } 75 | 76 | Vector3 sum; 77 | sum.x = 0; 78 | sum.y = 0; 79 | sum.z = 0; 80 | for(size_t i = 0; i < cloud_filtered_z->points.size(); i++) 81 | { 82 | sum.x += cloud_filtered_z->points[i].x; 83 | sum.y += cloud_filtered_z->points[i].y; 84 | sum.z += cloud_filtered_z->points[i].z; 85 | } 86 | 87 | Vector3 centroid; 88 | centroid.x = sum.x / cloud_filtered_z->points.size(); 89 | centroid.y = sum.y / cloud_filtered_z->points.size(); 90 | centroid.z = sum.z / cloud_filtered_z->points.size(); 91 | 92 | double distance = (centroid.x * centroid.x) + (centroid.y * centroid.y) + (centroid.z * centroid.z); 93 | distance = sqrt(distance); 94 | int size = cloud_rgb->size(); 95 | cloud_rgb->points[size - 1].x = centroid.x; 96 | cloud_rgb->points[size - 1].y = centroid.y; 97 | cloud_rgb->points[size - 1].z = centroid.z; 98 | cloud_rgb->points[size - 1].r = 255; 99 | cloud_rgb->points[size - 1].g = 255; 100 | cloud_rgb->points[size - 1].b = 255; 101 | 102 | //std::cout << sum.x << " " << sum.y << " " << sum.z << std::endl; 103 | //std::cout << centroid.x << " " << centroid.y << " " << centroid.z << std::endl; 104 | 105 | //std::cout << std::fixed << std::setprecision(8) << gps_data->latitude << " " << gps_data->longitude << std::endl; 106 | myfile << std::fixed << std::setprecision(8) << count << "," << gps_data->latitude << "," << gps_data->longitude << "," 107 | << gps_data->altitude << "," << distance << "," 108 | << time_data.sec << "," << time_data.nsec << std::endl; 109 | myfile.close(); 110 | 111 | //viewer.showCloud(cloud_rgb); 112 | 113 | } 114 | 115 | int main(int argc, char** argv) 116 | { 117 | ofstream myfile; 118 | myfile.open("map_height_data.csv"); 119 | myfile.close(); 120 | 121 | ros::init(argc, argv, "vision_node"); 122 | 123 | ros::NodeHandle nh; 124 | message_filters::Subscriber pointcloud_sub(nh, "velodyne_points", 1); 125 | message_filters::Subscriber gps_sub(nh, "/dji_sdk/gps_position", 1); 126 | 127 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 128 | // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) 129 | message_filters::Synchronizer sync(MySyncPolicy(10), pointcloud_sub, gps_sub); 130 | sync.registerCallback(boost::bind(&callback, _1, _2)); 131 | 132 | ros::spin(); 133 | 134 | return 0; 135 | } -------------------------------------------------------------------------------- /point_cloud_processing/src/max_height_estimate.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | struct Vector3 19 | { 20 | double x; 21 | double y; 22 | double z; 23 | }; 24 | 25 | int main(int argc, char** argv) 26 | { 27 | if(argc != 3) 28 | { 29 | std::cerr << "param_file point_cloud_file" << std::endl; 30 | return -1; 31 | } 32 | 33 | double distance_threshold = 0; 34 | 35 | std::ifstream infile(argv[1]); 36 | 37 | int count = 0; 38 | float param = 0; 39 | while(infile >> param) { 40 | switch(count) { 41 | case 0: 42 | distance_threshold = param; 43 | break; 44 | default: 45 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 46 | break; 47 | } 48 | count++; 49 | } 50 | 51 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 52 | if(pcl::io::loadPCDFile (argv[2], *temp_cloud) == -1) 53 | { 54 | PCL_ERROR("Couldn't read file "); 55 | PCL_ERROR(argv[2]); 56 | PCL_ERROR("\n"); 57 | return (-1); 58 | } 59 | 60 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 61 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 62 | // Create segmentation object 63 | pcl::SACSegmentation seg; 64 | //Optional 65 | seg.setOptimizeCoefficients(true); 66 | seg.setAxis(Eigen::Vector3f(0,0,1)); 67 | // Mandatory 68 | seg.setModelType(pcl::SACMODEL_PLANE); 69 | seg.setMethodType(pcl::SAC_RANSAC); 70 | seg.setDistanceThreshold(distance_threshold); 71 | 72 | seg.setInputCloud(temp_cloud); 73 | seg.segment(*inliers, *coefficients); 74 | 75 | if(inliers->indices.size() == 0) 76 | { 77 | PCL_ERROR("Could not estimate a planar model for the given dataset."); 78 | return -1; 79 | } 80 | 81 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 82 | cloud_rgb->points.resize(temp_cloud->size() + 2); 83 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 84 | { 85 | cloud_rgb->points[i].x = temp_cloud->points[i].x; 86 | cloud_rgb->points[i].y = temp_cloud->points[i].y; 87 | cloud_rgb->points[i].z = temp_cloud->points[i].z; 88 | cloud_rgb->points[i].r = 0; 89 | cloud_rgb->points[i].g = 0; 90 | cloud_rgb->points[i].b = 255; 91 | } 92 | 93 | for(size_t i = 0; i < inliers->indices.size(); ++i) 94 | { 95 | cloud_rgb->points[inliers->indices[i]].r = 255; 96 | cloud_rgb->points[inliers->indices[i]].g = 0; 97 | cloud_rgb->points[inliers->indices[i]].b = 0; 98 | } 99 | 100 | Vector3 sum; 101 | sum.x = 0; 102 | sum.y = 0; 103 | sum.z = 0; 104 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 105 | { 106 | if(cloud_rgb->points[i].r == 255) 107 | { 108 | sum.x += cloud_rgb->points[i].x; 109 | sum.y += cloud_rgb->points[i].y; 110 | sum.z += cloud_rgb->points[i].z; 111 | } 112 | } 113 | 114 | Vector3 centroid; 115 | centroid.x = sum.x / inliers->indices.size(); 116 | centroid.y = sum.y / inliers->indices.size(); 117 | centroid.z = sum.z / inliers->indices.size(); 118 | 119 | double xx = 0.0; 120 | double xy = 0.0; 121 | double xz = 0.0; 122 | double yy = 0.0; 123 | double yz = 0.0; 124 | double zz = 0.0; 125 | 126 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 127 | { 128 | if(cloud_rgb->points[i].r == 255) 129 | { 130 | Vector3 r; 131 | r.x = cloud_rgb->points[i].x - centroid.x; 132 | r.y = cloud_rgb->points[i].y - centroid.y; 133 | r.z = cloud_rgb->points[i].z - centroid.z; 134 | xx += r.x * r.x; 135 | xy += r.x * r.y; 136 | xz += r.x * r.z; 137 | yy += r.y * r.y; 138 | yz += r.y * r.z; 139 | zz += r.z * r.z; 140 | } 141 | } 142 | 143 | double det_x = yy*zz - yz*yz; 144 | double det_y = xx*zz - xz*xz; 145 | double det_z = xx*yy - xy*xy; 146 | 147 | double max = det_x; 148 | int max_letter = 1; 149 | if(det_y > max) 150 | { 151 | max = det_y; 152 | max_letter = 2; 153 | } 154 | if(det_z > max) 155 | { 156 | max = det_z; 157 | max_letter = 3; 158 | } 159 | 160 | Vector3 dir; 161 | if(max_letter == 1) 162 | { 163 | dir.x = det_x; 164 | dir.y = xz*yz - xy*zz; 165 | dir.z = xy*yz - xz*yy; 166 | } 167 | else if(max_letter == 2) 168 | { 169 | dir.x = xz*yz - xy*zz; 170 | dir.y = det_y; 171 | dir.z = xy*xz - yz*xx; 172 | } 173 | else if(max_letter == 3) 174 | { 175 | dir.x = xy*yz - xz*yy; 176 | dir.y = xy*xz - yz*xx; 177 | dir.z = det_z; 178 | } 179 | 180 | double length = (dir.x * dir.x) + (dir.y * dir.y) + (dir.z * dir.z); 181 | length = sqrt(length); 182 | 183 | dir.x = dir.x / length; 184 | dir.y = dir.y / length; 185 | dir.z = dir.z / length; 186 | 187 | double distance = 0; 188 | Vector3 subtract; 189 | Vector3 point_max; 190 | int index = -1; 191 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 192 | { 193 | if(cloud_rgb->points[i].b == 255) 194 | { 195 | double temp_distance = 0; 196 | subtract.x = cloud_rgb->points[i].x - centroid.x; 197 | subtract.y = cloud_rgb->points[i].y - centroid.y; 198 | subtract.z = cloud_rgb->points[i].z - centroid.z; 199 | temp_distance = (dir.x * subtract.x) + (dir.y * subtract.y) + (dir.z * subtract.z); 200 | 201 | double mag_dist = std::abs(temp_distance); 202 | if(mag_dist > distance) { 203 | distance = mag_dist; 204 | point_max.x = cloud_rgb->points[i].x; 205 | point_max.y = cloud_rgb->points[i].y; 206 | point_max.z = cloud_rgb->points[i].z; 207 | index = i; 208 | } 209 | } 210 | } 211 | 212 | std::cerr << "Max distance in frame(meters): " << distance << " meters." << std::endl; 213 | std::cerr << "Max distance in frame(inches): " << (distance * 39.3701) << " inches." << std::endl; 214 | 215 | int size = cloud_rgb->size(); 216 | cloud_rgb->points[size - 1].x = centroid.x; 217 | cloud_rgb->points[size - 1].y = centroid.y; 218 | cloud_rgb->points[size - 1].z = centroid.z; 219 | cloud_rgb->points[size - 1].r = 255; 220 | cloud_rgb->points[size - 1].g = 255; 221 | cloud_rgb->points[size - 1].b = 255; 222 | 223 | cloud_rgb->points[size - 2].x = dir.x; 224 | cloud_rgb->points[size - 2].y = dir.y; 225 | cloud_rgb->points[size - 2].z = dir.z; 226 | cloud_rgb->points[size - 2].r = 255; 227 | cloud_rgb->points[size - 2].g = 165; 228 | cloud_rgb->points[size - 2].b = 0; 229 | 230 | cloud_rgb->points[index].r = 0; 231 | cloud_rgb->points[index].g = 255; 232 | cloud_rgb->points[index].b = 0; 233 | 234 | pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 235 | viewer.showCloud(cloud_rgb); 236 | while(!viewer.wasStopped ()) 237 | { 238 | } 239 | 240 | return 0; 241 | } 242 | -------------------------------------------------------------------------------- /point_cloud_processing/src/nnsearch.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | int 12 | main (int argc, char** argv) 13 | { 14 | srand (time (NULL)); 15 | 16 | if(argc != 2) 17 | { 18 | std::cerr << "Expecting input argument: file" << std::endl; 19 | return -1; 20 | } 21 | 22 | std::ostringstream oss; 23 | oss << argv[1]; 24 | 25 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 26 | if(pcl::io::loadPCDFile (oss.str(), *cloud) == -1) 27 | { 28 | PCL_ERROR("Couldn't read file "); 29 | PCL_ERROR(oss.str().c_str()); 30 | PCL_ERROR("\n"); 31 | return (-1); 32 | } 33 | 34 | pcl::KdTreeFLANN kdtree; 35 | 36 | kdtree.setInputCloud (cloud); 37 | 38 | pcl::PointXYZ searchPoint; 39 | 40 | searchPoint.x = 0; 41 | searchPoint.y = 0; 42 | searchPoint.z = 0; 43 | 44 | // K nearest neighbor search 45 | 46 | int K = 1; 47 | 48 | std::vector pointIdxNKNSearch(K); 49 | std::vector pointNKNSquaredDistance(K); 50 | 51 | if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) 52 | { 53 | std::cout << " " << cloud->points[ pointIdxNKNSearch[0] ].x 54 | << " " << cloud->points[ pointIdxNKNSearch[0] ].y 55 | << " " << cloud->points[ pointIdxNKNSearch[0] ].z 56 | << " (squared distance: " << pointNKNSquaredDistance[0] << ")" << std::endl; 57 | } 58 | 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /point_cloud_processing/src/non_dji_tf_listener.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | int count = 0; 21 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 22 | tf::TransformListener *listener = NULL; 23 | 24 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 25 | { 26 | pcl::PCLPointCloud2 pcl_pc2; 27 | pcl_conversions::toPCL(*input, pcl_pc2); 28 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 29 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 30 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 31 | std::cerr << "Count: " << count << std::endl; 32 | 33 | tf::StampedTransform transform; 34 | Eigen::Affine3d transform_eigen; 35 | tf::Vector3 origin; 36 | tf::Quaternion rotation; 37 | try{ 38 | listener->lookupTransform("/dji", "/velodyne", 39 | ros::Time(0), transform); 40 | } 41 | catch (tf::TransformException ex){ 42 | ROS_ERROR("%s",ex.what()); 43 | return; 44 | } 45 | count++; 46 | 47 | origin = transform.getOrigin(); 48 | rotation = transform.getRotation(); 49 | 50 | std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 51 | std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 52 | tf::transformTFToEigen(transform, transform_eigen); 53 | 54 | pcl::PointCloud::Ptr transform_cloud(new pcl::PointCloud); 55 | transform_cloud->points.resize(temp_cloud->size()); 56 | 57 | pcl::transformPointCloud(*temp_cloud, *transform_cloud, transform_eigen); 58 | 59 | // viewer.showCloud(transform_cloud); 60 | } 61 | 62 | int main(int argc, char** argv){ 63 | 64 | ros::init(argc, argv, "my_tf_listener"); 65 | 66 | ros::NodeHandle node; 67 | // tf::TransformListener listener; 68 | listener = new(tf::TransformListener); 69 | 70 | ros::Subscriber sub = node.subscribe("velodyne_points", 1, cloud_cb); 71 | 72 | // ros::Rate rate(10.0); 73 | // while (node.ok()){ 74 | // tf::StampedTransform transform; 75 | // tf::Vector3 origin; 76 | // tf::Quaternion rotation; 77 | // Eigen::Affine3d transform_eigen; 78 | // try{ 79 | // listener->lookupTransform("/velodyne", "/world", 80 | // ros::Time(0), transform); 81 | // } 82 | // catch (tf::TransformException ex){ 83 | // ROS_ERROR("%s",ex.what()); 84 | // ros::Duration(1.0).sleep(); 85 | // } 86 | 87 | // origin = transform.getOrigin(); 88 | // rotation = transform.getRotation(); 89 | 90 | // std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 91 | // std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 92 | // tf::transformTFToEigen(transform, transform_eigen); 93 | 94 | // rate.sleep(); 95 | // } 96 | 97 | ros::spin(); 98 | return 0; 99 | } 100 | -------------------------------------------------------------------------------- /point_cloud_processing/src/perimeter_heights.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | struct Vector3 21 | { 22 | double x; 23 | double y; 24 | double z; 25 | }; 26 | 27 | int main(int argc, char** argv) 28 | { 29 | if(argc != 3) 30 | { 31 | std::cerr << "plot_only_file perimeter_file" << std::endl; 32 | return -1; 33 | } 34 | 35 | 36 | pcl::PointCloud::Ptr plot_only_cloud(new pcl::PointCloud); 37 | if(pcl::io::loadPCDFile (argv[1], *plot_only_cloud) == -1) 38 | { 39 | PCL_ERROR("Couldn't read file "); 40 | PCL_ERROR(argv[1]); 41 | PCL_ERROR("\n"); 42 | return (-1); 43 | } 44 | 45 | 46 | pcl::PointCloud::Ptr perimeter_cloud(new pcl::PointCloud); 47 | if(pcl::io::loadPCDFile (argv[2], *perimeter_cloud) == -1) 48 | { 49 | PCL_ERROR("Couldn't read file "); 50 | PCL_ERROR(argv[2]); 51 | PCL_ERROR("\n"); 52 | return (-1); 53 | } 54 | 55 | 56 | std::ofstream height_file; 57 | height_file.open("/home/naik/Data/turfgrass_heights.csv"); 58 | 59 | Vector3 sum; 60 | sum.x = 0; 61 | sum.y = 0; 62 | sum.z = 0; 63 | for(size_t i = 0; i < perimeter_cloud->points.size(); i++) 64 | { 65 | sum.x += perimeter_cloud->points[i].x; 66 | sum.y += perimeter_cloud->points[i].y; 67 | sum.z += perimeter_cloud->points[i].z; 68 | } 69 | 70 | Vector3 centroid; 71 | centroid.x = sum.x / perimeter_cloud->size(); 72 | centroid.y = sum.y / perimeter_cloud->size(); 73 | centroid.z = sum.z / perimeter_cloud->size(); 74 | 75 | double xx = 0.0; 76 | double xy = 0.0; 77 | double xz = 0.0; 78 | double yy = 0.0; 79 | double yz = 0.0; 80 | double zz = 0.0; 81 | 82 | for(size_t i = 0; i < perimeter_cloud->points.size(); i++) 83 | { 84 | Vector3 r; 85 | r.x = perimeter_cloud->points[i].x - centroid.x; 86 | r.y = perimeter_cloud->points[i].y - centroid.y; 87 | r.z = perimeter_cloud->points[i].z - centroid.z; 88 | xx += r.x * r.x; 89 | xy += r.x * r.y; 90 | xz += r.x * r.z; 91 | yy += r.y * r.y; 92 | yz += r.y * r.z; 93 | zz += r.z * r.z; 94 | } 95 | 96 | double det_x = yy*zz - yz*yz; 97 | double det_y = xx*zz - xz*xz; 98 | double det_z = xx*yy - xy*xy; 99 | 100 | double max = det_x; 101 | int max_letter = 1; 102 | if(det_y > max) 103 | { 104 | max = det_y; 105 | max_letter = 2; 106 | } 107 | if(det_z > max) 108 | { 109 | max = det_z; 110 | max_letter = 3; 111 | } 112 | 113 | Vector3 dir; 114 | if(max_letter == 1) 115 | { 116 | dir.x = det_x; 117 | dir.y = xz*yz - xy*zz; 118 | dir.z = xy*yz - xz*yy; 119 | } 120 | else if(max_letter == 2) 121 | { 122 | dir.x = xz*yz - xy*zz; 123 | dir.y = det_y; 124 | dir.z = xy*xz - yz*xx; 125 | } 126 | else if(max_letter == 3) 127 | { 128 | dir.x = xy*yz - xz*yy; 129 | dir.y = xy*xz - yz*xx; 130 | dir.z = det_z; 131 | } 132 | 133 | double length = (dir.x * dir.x) + (dir.y * dir.y) + (dir.z * dir.z); 134 | length = sqrt(length); 135 | 136 | dir.x = dir.x / length; 137 | dir.y = dir.y / length; 138 | dir.z = dir.z / length; 139 | 140 | double distance = 0; 141 | Vector3 subtract; 142 | Vector3 point_max; 143 | int index = -1; 144 | for(size_t i = 0; i < plot_only_cloud->points.size(); i++) 145 | { 146 | double temp_distance = 0; 147 | subtract.x = plot_only_cloud->points[i].x - centroid.x; 148 | subtract.y = plot_only_cloud->points[i].y - centroid.y; 149 | subtract.z = plot_only_cloud->points[i].z - centroid.z; 150 | temp_distance = (dir.x * subtract.x) + (dir.y * subtract.y) + (dir.z * subtract.z); 151 | 152 | double mag_dist = std::abs(temp_distance); 153 | 154 | height_file << mag_dist << std::endl; 155 | 156 | if(mag_dist > distance) { 157 | distance = mag_dist; 158 | point_max.x = plot_only_cloud->points[i].x; 159 | point_max.y = plot_only_cloud->points[i].y; 160 | point_max.z = plot_only_cloud->points[i].z; 161 | index = i; 162 | } 163 | } 164 | 165 | std::cerr << "Max distance in frame(meters): " << distance << " meters." << std::endl; 166 | //std::cerr << "Max distance in frame(inches): " << (distance * 39.3701) << " inches." << std::endl; 167 | 168 | 169 | return 0; 170 | } 171 | -------------------------------------------------------------------------------- /point_cloud_processing/src/plane_height_estimate.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | struct Vector3 19 | { 20 | double x; 21 | double y; 22 | double z; 23 | }; 24 | 25 | int main(int argc, char** argv) 26 | { 27 | if(argc != 3) 28 | { 29 | std::cerr << "param_file point_cloud_file" << std::endl; 30 | return -1; 31 | } 32 | 33 | double distance_threshold = 0; 34 | double y_leaf = 0; 35 | double z_leaf = 0; 36 | 37 | std::ifstream infile(argv[1]); 38 | 39 | int count = 0; 40 | float param = 0; 41 | while(infile >> param) { 42 | switch(count) { 43 | case 0: 44 | distance_threshold = param; 45 | break; 46 | default: 47 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 48 | break; 49 | } 50 | count++; 51 | } 52 | 53 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 54 | if(pcl::io::loadPCDFile (argv[2], *temp_cloud) == -1) 55 | { 56 | PCL_ERROR("Couldn't read file "); 57 | PCL_ERROR(argv[2]); 58 | PCL_ERROR("\n"); 59 | return (-1); 60 | } 61 | 62 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 63 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 64 | // Create segmentation object 65 | pcl::SACSegmentation seg; 66 | //Optional 67 | seg.setOptimizeCoefficients(true); 68 | // Mandatory 69 | seg.setModelType(pcl::SACMODEL_PLANE); 70 | seg.setMethodType(pcl::SAC_RANSAC); 71 | seg.setDistanceThreshold(distance_threshold); 72 | 73 | seg.setInputCloud(temp_cloud); 74 | seg.segment(*inliers, *coefficients); 75 | 76 | if(inliers->indices.size() == 0) 77 | { 78 | PCL_ERROR("Could not estimate a planar model for the given dataset."); 79 | return -1; 80 | } 81 | 82 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 83 | cloud_rgb->points.resize(temp_cloud->size() + 2); 84 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 85 | { 86 | cloud_rgb->points[i].x = temp_cloud->points[i].x; 87 | cloud_rgb->points[i].y = temp_cloud->points[i].y; 88 | cloud_rgb->points[i].z = temp_cloud->points[i].z; 89 | cloud_rgb->points[i].r = 0; 90 | cloud_rgb->points[i].g = 0; 91 | cloud_rgb->points[i].b = 255; 92 | } 93 | 94 | for(size_t i = 0; i < inliers->indices.size(); ++i) 95 | { 96 | cloud_rgb->points[inliers->indices[i]].r = 255; 97 | cloud_rgb->points[inliers->indices[i]].g = 0; 98 | cloud_rgb->points[inliers->indices[i]].b = 0; 99 | } 100 | 101 | Vector3 sum; 102 | sum.x = 0; 103 | sum.y = 0; 104 | sum.z = 0; 105 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 106 | { 107 | if(cloud_rgb->points[i].r == 255) 108 | { 109 | sum.x += cloud_rgb->points[i].x; 110 | sum.y += cloud_rgb->points[i].y; 111 | sum.z += cloud_rgb->points[i].z; 112 | } 113 | } 114 | 115 | Vector3 centroid; 116 | centroid.x = sum.x / inliers->indices.size(); 117 | centroid.y = sum.y / inliers->indices.size(); 118 | centroid.z = sum.z / inliers->indices.size(); 119 | 120 | double xx = 0.0; 121 | double xy = 0.0; 122 | double xz = 0.0; 123 | double yy = 0.0; 124 | double yz = 0.0; 125 | double zz = 0.0; 126 | 127 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 128 | { 129 | if(cloud_rgb->points[i].r == 255) 130 | { 131 | Vector3 r; 132 | r.x = cloud_rgb->points[i].x - centroid.x; 133 | r.y = cloud_rgb->points[i].y - centroid.y; 134 | r.z = cloud_rgb->points[i].z - centroid.z; 135 | xx += r.x * r.x; 136 | xy += r.x * r.y; 137 | xz += r.x * r.z; 138 | yy += r.y * r.y; 139 | yz += r.y * r.z; 140 | zz += r.z * r.z; 141 | } 142 | } 143 | 144 | double det_x = yy*zz - yz*yz; 145 | double det_y = xx*zz - xz*xz; 146 | double det_z = xx*yy - xy*xy; 147 | 148 | double max = det_x; 149 | int max_letter = 1; 150 | if(det_y > max) 151 | { 152 | max = det_y; 153 | max_letter = 2; 154 | } 155 | if(det_z > max) 156 | { 157 | max = det_z; 158 | max_letter = 3; 159 | } 160 | 161 | Vector3 dir; 162 | if(max_letter == 1) 163 | { 164 | dir.x = det_x; 165 | dir.y = xz*yz - xy*zz; 166 | dir.z = xy*yz - xz*yy; 167 | } 168 | else if(max_letter == 2) 169 | { 170 | dir.x = xz*yz - xy*zz; 171 | dir.y = det_y; 172 | dir.z = xy*xz - yz*xx; 173 | } 174 | else if(max_letter == 3) 175 | { 176 | dir.x = xy*yz - xz*yy; 177 | dir.y = xy*xz - yz*xx; 178 | dir.z = det_z; 179 | } 180 | 181 | double length = (dir.x * dir.x) + (dir.y * dir.y) + (dir.z * dir.z); 182 | length = sqrt(length); 183 | 184 | dir.x = dir.x / length; 185 | dir.y = dir.y / length; 186 | dir.z = dir.z / length; 187 | 188 | ofstream myfile; 189 | myfile.open("height_data.csv"); 190 | 191 | Vector3 subtract; 192 | Vector3 point_max; 193 | int index = -1; 194 | for(size_t i = 0; i < cloud_rgb->points.size(); i++) 195 | { 196 | if(cloud_rgb->points[i].b == 255) 197 | { 198 | double temp_distance = 0; 199 | subtract.x = cloud_rgb->points[i].x - centroid.x; 200 | subtract.y = cloud_rgb->points[i].y - centroid.y; 201 | subtract.z = cloud_rgb->points[i].z - centroid.z; 202 | temp_distance = (dir.x * subtract.x) + (dir.y * subtract.y) + (dir.z * subtract.z); 203 | 204 | double mag_dist = std::abs(temp_distance); 205 | myfile << mag_dist << std::endl; 206 | } 207 | } 208 | 209 | myfile.close(); 210 | 211 | return 0; 212 | } 213 | -------------------------------------------------------------------------------- /point_cloud_processing/src/plot_heights.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | struct Vector3 24 | { 25 | double x; 26 | double y; 27 | double z; 28 | }; 29 | 30 | int main(int argc, char** argv) 31 | { 32 | if(argc != 3) 33 | { 34 | std::cerr << "plot_only_param_file plot_all_pcd_file" << std::endl; 35 | return -1; 36 | } 37 | 38 | float negative_x_filter = 0; 39 | float positive_x_filter = 0; 40 | float negative_y_filter = 0; 41 | float positive_y_filter = 0; 42 | float negative_z_filter = 0; 43 | float positive_z_filter = 0; 44 | float rotation_x = 0; 45 | float rotation_y = 0; 46 | float rotation_z = 0; 47 | 48 | std::ifstream infile(argv[1]); 49 | 50 | int count = 0; 51 | float param = 0; 52 | while(infile >> param) { 53 | switch(count) { 54 | case 0: 55 | negative_x_filter = param; 56 | break; 57 | case 1: 58 | positive_x_filter = param; 59 | break; 60 | case 2: 61 | negative_y_filter = param; 62 | break; 63 | case 3: 64 | positive_y_filter = param; 65 | break; 66 | case 4: 67 | negative_z_filter = param; 68 | break; 69 | case 5: 70 | positive_z_filter = param; 71 | break; 72 | case 6: 73 | rotation_x = param; 74 | break; 75 | case 7: 76 | rotation_y = param; 77 | break; 78 | case 8: 79 | rotation_z = param; 80 | break; 81 | default: 82 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 83 | break; 84 | } 85 | count++; 86 | } 87 | 88 | std::cout << "Importing plot_all pcd file" << std::endl; 89 | 90 | pcl::PointCloud::Ptr plot_all_cloud(new pcl::PointCloud); 91 | if(pcl::io::loadPCDFile (argv[2], *plot_all_cloud) == -1) 92 | { 93 | PCL_ERROR("Couldn't read file "); 94 | PCL_ERROR(argv[2]); 95 | PCL_ERROR("\n"); 96 | return (-1); 97 | } 98 | 99 | /* 100 | int perimeter_count = 0; 101 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); 102 | pcl::ExtractIndices extract; 103 | for(size_t i = 0; i < plot_only_cloud->size(); i++) 104 | { 105 | 106 | for(size_t j = 0; j < plot_all_cloud->size(); j++) 107 | { 108 | //std::cout << plot_all_cloud->points[j].x << " " << plot_only_cloud->points[i].x << std::endl; 109 | //if(true) 110 | if(plot_all_cloud->points[j].x == plot_only_cloud->points[i].x && 111 | plot_all_cloud->points[j].y == plot_only_cloud->points[i].y && 112 | plot_all_cloud->points[j].z == plot_only_cloud->points[i].z) 113 | { 114 | //pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); 115 | inliers->indices.push_back(j); 116 | perimeter_count++; 117 | std::cout << "Perimeter points count: " << perimeter_count << std::endl; 118 | 119 | extract.setInputCloud(plot_all_cloud); 120 | extract.setIndices(inliers); 121 | extract.setNegative(true); 122 | extract.filter(*plot_all_cloud); 123 | 124 | break; 125 | } 126 | 127 | } 128 | 129 | } 130 | 131 | extract.setInputCloud(plot_all_cloud); 132 | extract.setIndices(inliers); 133 | extract.setNegative(true); 134 | extract.filter(*plot_all_cloud); 135 | */ 136 | 137 | std::cout << "Filtering plot out using parameters file" << std::endl; 138 | 139 | 140 | pcl::PointCloud::Ptr plot_only_cloud(new pcl::PointCloud); 141 | pcl::CropBox boxFilter; 142 | //boxFilter.setTranslation(Eigen::Vector3f(4, 1.5, 0)); 143 | boxFilter.setMin(Eigen::Vector4f(negative_x_filter, negative_y_filter, negative_z_filter, 1.0)); 144 | boxFilter.setMax(Eigen::Vector4f(positive_x_filter, positive_y_filter, positive_z_filter, 1.0)); 145 | //boxFilter.setMin(Eigen::Vector4f(-1, -3, -100, 1.0)); 146 | //boxFilter.setMax(Eigen::Vector4f(1, 3, 100, 1.0)); 147 | boxFilter.setRotation(Eigen::Vector3f(rotation_x, rotation_y, rotation_z)); 148 | boxFilter.setInputCloud(plot_all_cloud); 149 | //boxFilter.setNegative(true); 150 | boxFilter.filter(*plot_only_cloud); 151 | 152 | std::cout << "Filtering perimeter out using parameters file" << std::endl; 153 | //boxFilter.setTranslation(Eigen::Vector3f(4, 1.5, 0)); 154 | boxFilter.setMin(Eigen::Vector4f(negative_x_filter, negative_y_filter, negative_z_filter, 1.0)); 155 | boxFilter.setMax(Eigen::Vector4f(positive_x_filter, positive_y_filter, positive_z_filter, 1.0)); 156 | //boxFilter.setMin(Eigen::Vector4f(-1, -3, -100, 1.0)); 157 | //boxFilter.setMax(Eigen::Vector4f(1, 3, 100, 1.0)); 158 | boxFilter.setRotation(Eigen::Vector3f(rotation_x, rotation_y, rotation_z)); 159 | boxFilter.setInputCloud(plot_all_cloud); 160 | boxFilter.setNegative(true); 161 | boxFilter.filter(*plot_all_cloud); 162 | 163 | std::cout << "Saving perimeter and plot files in current directory" << std::endl; 164 | 165 | pcl::io::savePCDFileASCII("perimeter.pcd", *plot_all_cloud); 166 | pcl::io::savePCDFileASCII("plot_only.pcd", *plot_only_cloud); 167 | 168 | 169 | std::ofstream height_file; 170 | height_file.open("plot_heights.csv"); 171 | height_file << "X position" << "," << "Y position" << "," << "Z position" << "," << "Height" << std::endl; 172 | 173 | std::cout << "Computing heights of plot" << std::endl; 174 | Vector3 sum; 175 | sum.x = 0; 176 | sum.y = 0; 177 | sum.z = 0; 178 | for(size_t i = 0; i < plot_all_cloud->points.size(); i++) 179 | { 180 | sum.x += plot_all_cloud->points[i].x; 181 | sum.y += plot_all_cloud->points[i].y; 182 | sum.z += plot_all_cloud->points[i].z; 183 | } 184 | 185 | Vector3 centroid; 186 | centroid.x = sum.x / plot_all_cloud->size(); 187 | centroid.y = sum.y / plot_all_cloud->size(); 188 | centroid.z = sum.z / plot_all_cloud->size(); 189 | 190 | double xx = 0.0; 191 | double xy = 0.0; 192 | double xz = 0.0; 193 | double yy = 0.0; 194 | double yz = 0.0; 195 | double zz = 0.0; 196 | 197 | for(size_t i = 0; i < plot_all_cloud->points.size(); i++) 198 | { 199 | Vector3 r; 200 | r.x = plot_all_cloud->points[i].x - centroid.x; 201 | r.y = plot_all_cloud->points[i].y - centroid.y; 202 | r.z = plot_all_cloud->points[i].z - centroid.z; 203 | xx += r.x * r.x; 204 | xy += r.x * r.y; 205 | xz += r.x * r.z; 206 | yy += r.y * r.y; 207 | yz += r.y * r.z; 208 | zz += r.z * r.z; 209 | } 210 | 211 | double det_x = yy*zz - yz*yz; 212 | double det_y = xx*zz - xz*xz; 213 | double det_z = xx*yy - xy*xy; 214 | 215 | double max = det_x; 216 | int max_letter = 1; 217 | if(det_y > max) 218 | { 219 | max = det_y; 220 | max_letter = 2; 221 | } 222 | if(det_z > max) 223 | { 224 | max = det_z; 225 | max_letter = 3; 226 | } 227 | 228 | Vector3 dir; 229 | if(max_letter == 1) 230 | { 231 | dir.x = det_x; 232 | dir.y = xz*yz - xy*zz; 233 | dir.z = xy*yz - xz*yy; 234 | } 235 | else if(max_letter == 2) 236 | { 237 | dir.x = xz*yz - xy*zz; 238 | dir.y = det_y; 239 | dir.z = xy*xz - yz*xx; 240 | } 241 | else if(max_letter == 3) 242 | { 243 | dir.x = xy*yz - xz*yy; 244 | dir.y = xy*xz - yz*xx; 245 | dir.z = det_z; 246 | } 247 | 248 | double length = (dir.x * dir.x) + (dir.y * dir.y) + (dir.z * dir.z); 249 | length = sqrt(length); 250 | 251 | dir.x = dir.x / length; 252 | dir.y = dir.y / length; 253 | dir.z = dir.z / length; 254 | 255 | 256 | //double d = (dir.x * centroid.x * -1) + (dir.y * centroid.y * -1) + (dir.z * centroid.z * -1); 257 | 258 | double distance = 0; 259 | Vector3 subtract; 260 | int index = -1; 261 | for(size_t i = 0; i < plot_only_cloud->points.size(); i++) 262 | { 263 | double temp_distance = 0; 264 | subtract.x = plot_only_cloud->points[i].x - centroid.x; 265 | subtract.y = plot_only_cloud->points[i].y - centroid.y; 266 | subtract.z = plot_only_cloud->points[i].z - centroid.z; 267 | //temp_distance = (plot_only_cloud->points[i].x * dir.x) + (plot_only_cloud->points[i].y * dir.y) + (plot_only_cloud->points[i].z * dir.z) + d; 268 | //temp_distance = std::abs(temp_distance); 269 | temp_distance = (dir.x * subtract.x) + (dir.y * subtract.y) + (dir.z * subtract.z); 270 | 271 | //double mag_dist = std::abs(temp_distance); 272 | 273 | height_file << plot_only_cloud->points[i].x << "," << plot_only_cloud->points[i].y << "," << plot_only_cloud->points[i].z << "," << temp_distance << std::endl; 274 | 275 | } 276 | 277 | std::cout << "Saving plot_heights.csv file in current directory" << std::endl; 278 | height_file.close(); 279 | 280 | return 0; 281 | } 282 | -------------------------------------------------------------------------------- /point_cloud_processing/src/recenter_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | struct Vector3 9 | { 10 | double x; 11 | double y; 12 | double z; 13 | }; 14 | 15 | int main(int argc, char** argv) 16 | { 17 | if(argc != 2) 18 | { 19 | std::cerr << "Expecting input argument: file" << std::endl; 20 | return -1; 21 | } 22 | 23 | std::ostringstream oss; 24 | oss << argv[1]; 25 | 26 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 27 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 28 | { 29 | PCL_ERROR("Couldn't read file "); 30 | PCL_ERROR(oss.str().c_str()); 31 | PCL_ERROR("\n"); 32 | return (-1); 33 | } 34 | 35 | Vector3 sum; 36 | sum.x = 0; 37 | sum.y = 0; 38 | sum.z = 0; 39 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 40 | { 41 | sum.x += temp_cloud->points[i].x; 42 | sum.y += temp_cloud->points[i].y; 43 | sum.z += temp_cloud->points[i].z; 44 | } 45 | 46 | Vector3 centroid; 47 | centroid.x = sum.x / temp_cloud->points.size(); 48 | centroid.y = sum.y / temp_cloud->points.size(); 49 | centroid.z = sum.z / temp_cloud->points.size(); 50 | 51 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 52 | { 53 | 54 | temp_cloud->points[i].x -= centroid.x; 55 | temp_cloud->points[i].y -= centroid.y; 56 | temp_cloud->points[i].z -= centroid.z; 57 | } 58 | 59 | std::string new_name = oss.str().substr(0, oss.str().size()-4); 60 | std::ostringstream oss2; 61 | oss2 << new_name << "_recentered.pcd"; 62 | pcl::io::savePCDFileASCII(oss2.str(), *temp_cloud); 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /point_cloud_processing/src/save_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | int count = 0; 18 | std::ostringstream oss; 19 | std::ostringstream oss2; 20 | 21 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 22 | { 23 | 24 | count++; 25 | pcl::PCLPointCloud2 pcl_pc2; 26 | pcl_conversions::toPCL(*input, pcl_pc2); 27 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 28 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 29 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 30 | std::cerr << "Count: " << count << std::endl; 31 | 32 | std::ostringstream oss3; 33 | oss3 << oss2.str() << "/point_cloud_" << count << ".pcd"; 34 | pcl::io::savePCDFileASCII(oss3.str(), *temp_cloud); 35 | } 36 | 37 | int main(int argc, char** argv) 38 | { 39 | 40 | std::time_t t = std::time(0); 41 | std::tm* now = std::localtime(&t); 42 | oss << "/home/user01/Data/PointCloud/" << (now->tm_mon + 1) << "-" 43 | << (now->tm_mday) << "-" << (now->tm_year + 1900); 44 | mode_t nMode = 0733; 45 | int nError = 0; 46 | nError = mkdir(oss.str().c_str(), nMode); 47 | 48 | oss2 << "/home/user01/Data/PointCloud/" << (now->tm_mon + 1) << "-" 49 | << (now->tm_mday) << "-" << (now->tm_year + 1900) << "/unfiltered"; 50 | nError = mkdir(oss2.str().c_str(), nMode); 51 | 52 | ros::init(argc, argv, "point_cloud_processing"); 53 | ros::NodeHandle nh; 54 | 55 | ros::Subscriber sub = nh.subscribe("velodyne_points", 1, cloud_cb); 56 | 57 | ros::spin(); 58 | } 59 | -------------------------------------------------------------------------------- /point_cloud_processing/src/simulation_tf_listener.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | int count = 0; 21 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 22 | tf::TransformListener *listener = NULL; 23 | 24 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 25 | { 26 | pcl::PCLPointCloud2 pcl_pc2; 27 | pcl_conversions::toPCL(*input, pcl_pc2); 28 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 29 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 30 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 31 | std::cerr << "Count: " << count << std::endl; 32 | 33 | tf::StampedTransform transform; 34 | Eigen::Affine3d transform_eigen; 35 | tf::Vector3 origin; 36 | tf::Quaternion rotation; 37 | try{ 38 | listener->lookupTransform("/world", "/laser0_frame", 39 | ros::Time(0), transform); 40 | } 41 | catch (tf::TransformException ex){ 42 | ROS_ERROR("%s",ex.what()); 43 | return; 44 | } 45 | count++; 46 | 47 | origin = transform.getOrigin(); 48 | rotation = transform.getRotation(); 49 | 50 | std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 51 | std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 52 | tf::transformTFToEigen(transform, transform_eigen); 53 | 54 | } 55 | 56 | int main(int argc, char** argv){ 57 | 58 | ros::init(argc, argv, "my_tf_listener"); 59 | 60 | ros::NodeHandle node; 61 | // tf::TransformListener listener; 62 | listener = new(tf::TransformListener); 63 | 64 | ros::Subscriber sub = node.subscribe("velodyne_pointcloud", 1, cloud_cb); 65 | 66 | // ros::Rate rate(10.0); 67 | // while (node.ok()){ 68 | // tf::StampedTransform transform; 69 | // tf::Vector3 origin; 70 | // tf::Quaternion rotation; 71 | // Eigen::Affine3d transform_eigen; 72 | // try{ 73 | // listener->lookupTransform("/velodyne", "/world", 74 | // ros::Time(0), transform); 75 | // } 76 | // catch (tf::TransformException ex){ 77 | // ROS_ERROR("%s",ex.what()); 78 | // ros::Duration(1.0).sleep(); 79 | // } 80 | 81 | // origin = transform.getOrigin(); 82 | // rotation = transform.getRotation(); 83 | 84 | // std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 85 | // std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 86 | // tf::transformTFToEigen(transform, transform_eigen); 87 | 88 | // rate.sleep(); 89 | // } 90 | 91 | ros::spin(); 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /point_cloud_processing/src/single_box_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | int main(int argc, char** argv) 14 | { 15 | 16 | if(argc != 3) 17 | { 18 | std::cerr << "param_file point_cloud_file" << std::endl; 19 | return -1; 20 | } 21 | 22 | double negative_x_filter = 0; 23 | double positive_x_filter = 0; 24 | double negative_y_filter = 0; 25 | double positive_y_filter = 0; 26 | double negative_z_filter = 0; 27 | double positive_z_filter = 0; 28 | 29 | std::ifstream infile(argv[1]); 30 | 31 | int count = 0; 32 | double param = 0; 33 | while(infile >> param) { 34 | switch(count) { 35 | case 0: 36 | negative_x_filter = param; 37 | break; 38 | case 1: 39 | positive_x_filter = param; 40 | break; 41 | case 2: 42 | negative_y_filter = param; 43 | break; 44 | case 3: 45 | positive_y_filter = param; 46 | break; 47 | case 4: 48 | negative_z_filter = param; 49 | break; 50 | case 5: 51 | positive_z_filter = param; 52 | break; 53 | default: 54 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 55 | break; 56 | } 57 | count++; 58 | } 59 | 60 | std::ostringstream oss; 61 | oss << argv[2]; 62 | 63 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 64 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 65 | { 66 | PCL_ERROR("Couldn't read file "); 67 | PCL_ERROR(oss.str().c_str()); 68 | PCL_ERROR("\n"); 69 | return (-1); 70 | } 71 | 72 | pcl::PointCloud::Ptr cloud_filtered_x(new pcl::PointCloud); 73 | pcl::PassThrough pass_x; 74 | pass_x.setInputCloud(temp_cloud); 75 | pass_x.setFilterFieldName("x"); 76 | pass_x.setFilterLimits(negative_x_filter, positive_x_filter); 77 | pass_x.filter(*cloud_filtered_x); 78 | 79 | pcl::PointCloud::Ptr cloud_filtered_y(new pcl::PointCloud); 80 | pcl::PassThrough pass_y; 81 | pass_y.setInputCloud(cloud_filtered_x); 82 | pass_y.setFilterFieldName("y"); 83 | pass_y.setFilterLimits(negative_y_filter, positive_y_filter); 84 | pass_y.filter(*cloud_filtered_y); 85 | 86 | pcl::PointCloud::Ptr cloud_filtered_z(new pcl::PointCloud); 87 | pcl::PassThrough pass_z; 88 | pass_z.setInputCloud(cloud_filtered_y); 89 | pass_z.setFilterFieldName("z"); 90 | pass_z.setFilterLimits(negative_z_filter, positive_z_filter); 91 | pass_z.filter(*cloud_filtered_z); 92 | 93 | std::string new_name = oss.str().substr(0, oss.str().size()-4); 94 | std::ostringstream oss2; 95 | oss2 << new_name << "_filtered.pcd"; 96 | pcl::io::savePCDFileASCII(oss2.str(), *cloud_filtered_z); 97 | return 0; 98 | } 99 | -------------------------------------------------------------------------------- /point_cloud_processing/src/single_crop_box_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | int main(int argc, char** argv) 14 | { 15 | if(argc != 3) 16 | { 17 | std::cerr << "param_file point_cloud_file" << std::endl; 18 | return -1; 19 | } 20 | 21 | float negative_x_filter = 0; 22 | float positive_x_filter = 0; 23 | float negative_y_filter = 0; 24 | float positive_y_filter = 0; 25 | float negative_z_filter = 0; 26 | float positive_z_filter = 0; 27 | float rotation_x = 0; 28 | float rotation_y = 0; 29 | float rotation_z = 0; 30 | 31 | std::ifstream infile(argv[1]); 32 | 33 | int count = 0; 34 | float param = 0; 35 | while(infile >> param) { 36 | switch(count) { 37 | case 0: 38 | negative_x_filter = param; 39 | break; 40 | case 1: 41 | positive_x_filter = param; 42 | break; 43 | case 2: 44 | negative_y_filter = param; 45 | break; 46 | case 3: 47 | positive_y_filter = param; 48 | break; 49 | case 4: 50 | negative_z_filter = param; 51 | break; 52 | case 5: 53 | positive_z_filter = param; 54 | break; 55 | case 6: 56 | rotation_x = param; 57 | break; 58 | case 7: 59 | rotation_y = param; 60 | break; 61 | case 8: 62 | rotation_z = param; 63 | break; 64 | default: 65 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 66 | break; 67 | } 68 | count++; 69 | } 70 | 71 | std::ostringstream oss; 72 | oss << argv[2]; 73 | 74 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 75 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 76 | { 77 | PCL_ERROR("Couldn't read file "); 78 | PCL_ERROR(oss.str().c_str()); 79 | PCL_ERROR("\n"); 80 | return (-1); 81 | } 82 | 83 | pcl::PointCloud::Ptr bodyFiltered(new pcl::PointCloud); 84 | pcl::CropBox boxFilter; 85 | //boxFilter.setTranslation(Eigen::Vector3f(4, 1.5, 0)); 86 | boxFilter.setMin(Eigen::Vector4f(negative_x_filter, negative_y_filter, negative_z_filter, 1.0)); 87 | boxFilter.setMax(Eigen::Vector4f(positive_x_filter, positive_y_filter, positive_z_filter, 1.0)); 88 | //boxFilter.setMin(Eigen::Vector4f(-1, -3, -100, 1.0)); 89 | //boxFilter.setMax(Eigen::Vector4f(1, 3, 100, 1.0)); 90 | boxFilter.setRotation(Eigen::Vector3f(rotation_x, rotation_y, rotation_z)); 91 | boxFilter.setInputCloud(temp_cloud); 92 | boxFilter.filter(*bodyFiltered); 93 | 94 | std::string new_name = oss.str().substr(0, oss.str().size()-4); 95 | std::ostringstream oss2; 96 | oss2 << new_name << "_crop_box_filtered.pcd"; 97 | pcl::io::savePCDFileASCII(oss2.str(), *bodyFiltered); 98 | return 0; 99 | } 100 | -------------------------------------------------------------------------------- /point_cloud_processing/src/single_save_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | std::ostringstream oss; 17 | std::ostringstream oss2; 18 | 19 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 20 | { 21 | 22 | std::time_t t = std::time(0); 23 | std::tm* now = std::localtime(&t); 24 | pcl::PCLPointCloud2 pcl_pc2; 25 | pcl_conversions::toPCL(*input, pcl_pc2); 26 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 27 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 28 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 29 | 30 | std::ostringstream oss3; 31 | oss3 << oss2.str() << "/point_cloud_" << now->tm_hour << "-" 32 | << now->tm_min << "-" 33 | << now->tm_sec << ".pcd"; 34 | pcl::io::savePCDFileASCII(oss3.str(), *temp_cloud); 35 | ros::shutdown(); 36 | } 37 | 38 | int main(int argc, char** argv) 39 | { 40 | 41 | std::time_t t = std::time(0); 42 | std::tm* now = std::localtime(&t); 43 | oss << "/home/user/Data/PointCloud/" << (now->tm_mon + 1) << "-" 44 | << (now->tm_mday) << "-" << (now->tm_year + 1900); 45 | mode_t nMode = 0733; 46 | int nError = 0; 47 | nError = mkdir(oss.str().c_str(), nMode); 48 | 49 | oss2 << "/home/user/Data/PointCloud/" << (now->tm_mon + 1) << "-" 50 | << (now->tm_mday) << "-" << (now->tm_year + 1900) << "/single"; 51 | nError = mkdir(oss2.str().c_str(), nMode); 52 | 53 | ros::init(argc, argv, "point_cloud_processing"); 54 | ros::NodeHandle nh; 55 | 56 | ros::Subscriber sub = nh.subscribe("velodyne_points", 1, cloud_cb); 57 | 58 | ros::spin(); 59 | } 60 | -------------------------------------------------------------------------------- /point_cloud_processing/src/tf_listener.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | int count = 0; 21 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 22 | tf::TransformListener *listener = NULL; 23 | 24 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 25 | { 26 | pcl::PCLPointCloud2 pcl_pc2; 27 | pcl_conversions::toPCL(*input, pcl_pc2); 28 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 29 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 30 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 31 | std::cerr << "Count: " << count << std::endl; 32 | 33 | tf::StampedTransform transform; 34 | Eigen::Affine3d transform_eigen; 35 | tf::Vector3 origin; 36 | tf::Quaternion rotation; 37 | try{ 38 | listener->lookupTransform("/world", "/velodyne", 39 | ros::Time(0), transform); 40 | } 41 | catch (tf::TransformException ex){ 42 | ROS_ERROR("%s",ex.what()); 43 | return; 44 | } 45 | count++; 46 | 47 | origin = transform.getOrigin(); 48 | rotation = transform.getRotation(); 49 | 50 | std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 51 | std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 52 | tf::transformTFToEigen(transform, transform_eigen); 53 | 54 | } 55 | 56 | int main(int argc, char** argv){ 57 | 58 | ros::init(argc, argv, "my_tf_listener"); 59 | 60 | ros::NodeHandle node; 61 | // tf::TransformListener listener; 62 | listener = new(tf::TransformListener); 63 | 64 | ros::Subscriber sub = node.subscribe("velodyne_points", 1, cloud_cb); 65 | 66 | // ros::Rate rate(10.0); 67 | // while (node.ok()){ 68 | // tf::StampedTransform transform; 69 | // tf::Vector3 origin; 70 | // tf::Quaternion rotation; 71 | // Eigen::Affine3d transform_eigen; 72 | // try{ 73 | // listener->lookupTransform("/velodyne", "/world", 74 | // ros::Time(0), transform); 75 | // } 76 | // catch (tf::TransformException ex){ 77 | // ROS_ERROR("%s",ex.what()); 78 | // ros::Duration(1.0).sleep(); 79 | // } 80 | 81 | // origin = transform.getOrigin(); 82 | // rotation = transform.getRotation(); 83 | 84 | // std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 85 | // std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 86 | // tf::transformTFToEigen(transform, transform_eigen); 87 | 88 | // rate.sleep(); 89 | // } 90 | 91 | ros::spin(); 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /point_cloud_processing/src/transform_and_plane_extraction.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | struct Vector3 28 | { 29 | double x; 30 | double y; 31 | double z; 32 | }; 33 | 34 | int count = 0; 35 | pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 36 | tf::TransformListener *listener = NULL; 37 | std::ostringstream oss; 38 | std::ostringstream oss2; 39 | 40 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 41 | { 42 | pcl::PCLPointCloud2 pcl_pc2; 43 | pcl_conversions::toPCL(*input, pcl_pc2); 44 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 45 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 46 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 47 | std::cerr << "Count: " << count << std::endl; 48 | 49 | tf::StampedTransform transform; 50 | Eigen::Affine3d transform_eigen; 51 | tf::Vector3 origin; 52 | tf::Quaternion rotation; 53 | try{ 54 | listener->lookupTransform("/world", "/velodyne", 55 | ros::Time(0), transform); 56 | } 57 | catch (tf::TransformException ex){ 58 | ROS_ERROR("%s",ex.what()); 59 | return; 60 | } 61 | count++; 62 | 63 | pcl::PointCloud::Ptr cloud_filtered_z(new pcl::PointCloud); 64 | pcl::PassThrough pass_z; 65 | pass_z.setInputCloud(temp_cloud); 66 | pass_z.setFilterFieldName("z"); 67 | pass_z.setFilterLimits(-3, 3); 68 | pass_z.filter(*cloud_filtered_z); 69 | 70 | pcl::PointCloud::Ptr cloud_filtered_y(new pcl::PointCloud); 71 | pcl::PassThrough pass_y; 72 | pass_y.setInputCloud(cloud_filtered_z); 73 | pass_y.setFilterFieldName("y"); 74 | pass_y.setFilterLimits(-3, 3); 75 | pass_y.filter(*cloud_filtered_y); 76 | 77 | origin = transform.getOrigin(); 78 | rotation = transform.getRotation(); 79 | 80 | std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 81 | std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 82 | tf::transformTFToEigen(transform, transform_eigen); 83 | 84 | pcl::PointCloud::Ptr transform_cloud(new pcl::PointCloud); 85 | transform_cloud->points.resize(cloud_filtered_y->size()); 86 | 87 | pcl::transformPointCloud(*cloud_filtered_y, *transform_cloud, transform_eigen); 88 | 89 | 90 | std::ostringstream oss3; 91 | oss3 << oss2.str() << "/point_cloud_" << count << ".pcd"; 92 | pcl::io::savePCDFileASCII(oss3.str(), *transform_cloud); 93 | viewer.showCloud(transform_cloud); 94 | 95 | } 96 | 97 | int main(int argc, char** argv){ 98 | 99 | std::time_t t = std::time(0); 100 | std::tm* now = std::localtime(&t); 101 | oss << "/home/user01/Data/PointCloud/" << (now->tm_mon + 1) << "-" 102 | << (now->tm_mday) << "-" << (now->tm_year + 1900); 103 | mode_t nMode = 0733; 104 | int nError = 0; 105 | nError = mkdir(oss.str().c_str(), nMode); 106 | 107 | oss2 << "/home/user01/Data/PointCloud/" << (now->tm_mon + 1) << "-" 108 | << (now->tm_mday) << "-" << (now->tm_year + 1900) << "/unfiltered"; 109 | nError = mkdir(oss2.str().c_str(), nMode); 110 | 111 | ros::init(argc, argv, "my_tf_listener"); 112 | 113 | ros::NodeHandle node; 114 | // tf::TransformListener listener; 115 | listener = new(tf::TransformListener); 116 | 117 | ros::Subscriber sub = node.subscribe("velodyne_points", 1, cloud_cb); 118 | 119 | // ros::Rate rate(10.0); 120 | // while (node.ok()){ 121 | // tf::StampedTransform transform; 122 | // tf::Vector3 origin; 123 | // tf::Quaternion rotation; 124 | // Eigen::Affine3d transform_eigen; 125 | // try{ 126 | // listener->lookupTransform("/velodyne", "/world", 127 | // ros::Time(0), transform); 128 | // } 129 | // catch (tf::TransformException ex){ 130 | // ROS_ERROR("%s",ex.what()); 131 | // ros::Duration(1.0).sleep(); 132 | // } 133 | 134 | // origin = transform.getOrigin(); 135 | // rotation = transform.getRotation(); 136 | 137 | // std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 138 | // std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 139 | // tf::transformTFToEigen(transform, transform_eigen); 140 | 141 | // rate.sleep(); 142 | // } 143 | 144 | ros::spin(); 145 | return 0; 146 | } 147 | -------------------------------------------------------------------------------- /point_cloud_processing/src/transform_cloud_and_save.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | int count = 0; 21 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 22 | tf::TransformListener *listener = NULL; 23 | std::ostringstream oss; 24 | std::ostringstream oss2; 25 | 26 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 27 | { 28 | pcl::PCLPointCloud2 pcl_pc2; 29 | pcl_conversions::toPCL(*input, pcl_pc2); 30 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 31 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 32 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 33 | std::cerr << "Count: " << count << std::endl; 34 | 35 | tf::StampedTransform transform; 36 | Eigen::Affine3d transform_eigen; 37 | tf::Vector3 origin; 38 | tf::Quaternion rotation; 39 | try{ 40 | listener->lookupTransform("/world", "/velodyne", 41 | ros::Time(0), transform); 42 | } 43 | catch (tf::TransformException ex){ 44 | ROS_ERROR("%s",ex.what()); 45 | return; 46 | } 47 | count++; 48 | 49 | origin = transform.getOrigin(); 50 | rotation = transform.getRotation(); 51 | 52 | std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 53 | std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 54 | tf::transformTFToEigen(transform, transform_eigen); 55 | 56 | pcl::PointCloud::Ptr transform_cloud(new pcl::PointCloud); 57 | transform_cloud->points.resize(temp_cloud->size()); 58 | 59 | pcl::transformPointCloud(*temp_cloud, *transform_cloud, transform_eigen); 60 | 61 | std::ostringstream oss3; 62 | std::cout << oss2.str() << std::endl; 63 | oss3 << oss2.str() << "/point_cloud_" << count << ".pcd"; 64 | pcl::io::savePCDFileASCII(oss3.str(), *transform_cloud); 65 | // viewer.showCloud(transform_cloud); 66 | } 67 | 68 | int main(int argc, char** argv){ 69 | 70 | std::time_t t = std::time(0); 71 | std::tm* now = std::localtime(&t); 72 | oss << "/home/naik/Data/PointCloud/" << (now->tm_mon + 1) << "-" 73 | << (now->tm_mday) << "-" << (now->tm_year + 1900); 74 | mode_t nMode = 0733; 75 | int nError = 0; 76 | nError = mkdir(oss.str().c_str(), nMode); 77 | 78 | oss2 << "/home/naik/Data/PointCloud/" << (now->tm_mon + 1) << "-" 79 | << (now->tm_mday) << "-" << (now->tm_year + 1900) << "/unfiltered"; 80 | nError = mkdir(oss2.str().c_str(), nMode); 81 | 82 | ros::init(argc, argv, "my_tf_listener"); 83 | 84 | ros::NodeHandle node; 85 | // tf::TransformListener listener; 86 | listener = new(tf::TransformListener); 87 | 88 | ros::Subscriber sub = node.subscribe("velodyne_points", 1, cloud_cb); 89 | 90 | // ros::Rate rate(10.0); 91 | // while (node.ok()){ 92 | // tf::StampedTransform transform; 93 | // tf::Vector3 origin; 94 | // tf::Quaternion rotation; 95 | // Eigen::Affine3d transform_eigen; 96 | // try{ 97 | // listener->lookupTransform("/velodyne", "/world", 98 | // ros::Time(0), transform); 99 | // } 100 | // catch (tf::TransformException ex){ 101 | // ROS_ERROR("%s",ex.what()); 102 | // ros::Duration(1.0).sleep(); 103 | // } 104 | 105 | // origin = transform.getOrigin(); 106 | // rotation = transform.getRotation(); 107 | 108 | // std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 109 | // std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 110 | // tf::transformTFToEigen(transform, transform_eigen); 111 | 112 | // rate.sleep(); 113 | // } 114 | 115 | ros::spin(); 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /point_cloud_processing/src/transform_sim_and_save.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | int count = 0; 21 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 22 | tf::TransformListener *listener = NULL; 23 | std::ostringstream oss; 24 | std::ostringstream oss2; 25 | 26 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 27 | { 28 | pcl::PCLPointCloud2 pcl_pc2; 29 | pcl_conversions::toPCL(*input, pcl_pc2); 30 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 31 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 32 | std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 33 | std::cerr << "Count: " << count << std::endl; 34 | 35 | tf::StampedTransform transform; 36 | Eigen::Affine3d transform_eigen; 37 | tf::Vector3 origin; 38 | tf::Quaternion rotation; 39 | try{ 40 | listener->lookupTransform("/world", "/laser0_frame", 41 | ros::Time(0), transform); 42 | } 43 | catch (tf::TransformException ex){ 44 | ROS_ERROR("%s",ex.what()); 45 | return; 46 | } 47 | count++; 48 | 49 | origin = transform.getOrigin(); 50 | rotation = transform.getRotation(); 51 | 52 | std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 53 | std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 54 | tf::transformTFToEigen(transform, transform_eigen); 55 | 56 | pcl::PointCloud::Ptr transform_cloud(new pcl::PointCloud); 57 | transform_cloud->points.resize(temp_cloud->size()); 58 | 59 | pcl::transformPointCloud(*temp_cloud, *transform_cloud, transform_eigen); 60 | 61 | std::ostringstream oss3; 62 | std::cout << oss2.str() << std::endl; 63 | oss3 << oss2.str() << "/point_cloud_" << count << ".pcd"; 64 | pcl::io::savePCDFileASCII(oss3.str(), *transform_cloud); 65 | // viewer.showCloud(transform_cloud); 66 | } 67 | 68 | int main(int argc, char** argv){ 69 | 70 | std::time_t t = std::time(0); 71 | std::tm* now = std::localtime(&t); 72 | oss << "/home/naik/Data/PointCloud/" << (now->tm_mon + 1) << "-" 73 | << (now->tm_mday) << "-" << (now->tm_year + 1900); 74 | mode_t nMode = 0733; 75 | int nError = 0; 76 | nError = mkdir(oss.str().c_str(), nMode); 77 | 78 | oss2 << "/home/naik/Data/PointCloud/" << (now->tm_mon + 1) << "-" 79 | << (now->tm_mday) << "-" << (now->tm_year + 1900) << "/unfiltered"; 80 | nError = mkdir(oss2.str().c_str(), nMode); 81 | 82 | ros::init(argc, argv, "my_tf_listener"); 83 | 84 | ros::NodeHandle node; 85 | // tf::TransformListener listener; 86 | listener = new(tf::TransformListener); 87 | 88 | ros::Subscriber sub = node.subscribe("velodyne_points", 1, cloud_cb); 89 | 90 | // ros::Rate rate(10.0); 91 | // while (node.ok()){ 92 | // tf::StampedTransform transform; 93 | // tf::Vector3 origin; 94 | // tf::Quaternion rotation; 95 | // Eigen::Affine3d transform_eigen; 96 | // try{ 97 | // listener->lookupTransform("/velodyne", "/world", 98 | // ros::Time(0), transform); 99 | // } 100 | // catch (tf::TransformException ex){ 101 | // ROS_ERROR("%s",ex.what()); 102 | // ros::Duration(1.0).sleep(); 103 | // } 104 | 105 | // origin = transform.getOrigin(); 106 | // rotation = transform.getRotation(); 107 | 108 | // std::cout << "Origin: " << origin.x() << " " << origin.y() << " " << origin.z() << " " << origin.w() << std::endl; 109 | // std::cout << "Quaternion: " << rotation.x() << " " << origin.y() << " " << rotation.z() << " " << rotation.w() << std::endl; 110 | // tf::transformTFToEigen(transform, transform_eigen); 111 | 112 | // rate.sleep(); 113 | // } 114 | 115 | ros::spin(); 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /point_cloud_processing/src/turfgrass_heights.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | struct Vector3 21 | { 22 | double x; 23 | double y; 24 | double z; 25 | }; 26 | 27 | int main(int argc, char** argv) 28 | { 29 | if(argc != 3) 30 | { 31 | std::cerr << "plot_only_file plot_all_file" << std::endl; 32 | return -1; 33 | } 34 | 35 | 36 | pcl::PointCloud::Ptr plot_only_cloud(new pcl::PointCloud); 37 | if(pcl::io::loadPCDFile (argv[1], *plot_only_cloud) == -1) 38 | { 39 | PCL_ERROR("Couldn't read file "); 40 | PCL_ERROR(argv[1]); 41 | PCL_ERROR("\n"); 42 | return (-1); 43 | } 44 | 45 | 46 | pcl::PointCloud::Ptr plot_all_cloud(new pcl::PointCloud); 47 | if(pcl::io::loadPCDFile (argv[2], *plot_all_cloud) == -1) 48 | { 49 | PCL_ERROR("Couldn't read file "); 50 | PCL_ERROR(argv[2]); 51 | PCL_ERROR("\n"); 52 | return (-1); 53 | } 54 | 55 | 56 | pcl::ExtractIndices extract; 57 | for(size_t i = 0; i < plot_only_cloud->size(); i++) 58 | { 59 | 60 | for(size_t j = 0; j < plot_all_cloud->size(); j++) 61 | { 62 | //std::cout << plot_all_cloud->points[j].x << " " << plot_only_cloud->points[i].x << std::endl; 63 | //if(true) 64 | if(plot_all_cloud->points[j].x == plot_only_cloud->points[i].x && 65 | plot_all_cloud->points[j].y == plot_only_cloud->points[i].y && 66 | plot_all_cloud->points[j].z == plot_only_cloud->points[i].z) 67 | { 68 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); 69 | inliers->indices.push_back(j); 70 | extract.setInputCloud(plot_all_cloud); 71 | extract.setIndices(inliers); 72 | extract.setNegative(true); 73 | extract.filter(*plot_all_cloud); 74 | break; 75 | } 76 | 77 | } 78 | 79 | } 80 | 81 | pcl::io::savePCDFileASCII("/home/naik/Data/PointCloud/perimeter.pcd", *plot_all_cloud); 82 | 83 | 84 | std::ofstream height_file; 85 | height_file.open("/home/naik/Data/turfgrass_heights.csv"); 86 | 87 | Vector3 sum; 88 | sum.x = 0; 89 | sum.y = 0; 90 | sum.z = 0; 91 | for(size_t i = 0; i < plot_all_cloud->points.size(); i++) 92 | { 93 | sum.x += plot_all_cloud->points[i].x; 94 | sum.y += plot_all_cloud->points[i].y; 95 | sum.z += plot_all_cloud->points[i].z; 96 | } 97 | 98 | Vector3 centroid; 99 | centroid.x = sum.x / plot_all_cloud->size(); 100 | centroid.y = sum.y / plot_all_cloud->size(); 101 | centroid.z = sum.z / plot_all_cloud->size(); 102 | 103 | double xx = 0.0; 104 | double xy = 0.0; 105 | double xz = 0.0; 106 | double yy = 0.0; 107 | double yz = 0.0; 108 | double zz = 0.0; 109 | 110 | for(size_t i = 0; i < plot_all_cloud->points.size(); i++) 111 | { 112 | Vector3 r; 113 | r.x = plot_all_cloud->points[i].x - centroid.x; 114 | r.y = plot_all_cloud->points[i].y - centroid.y; 115 | r.z = plot_all_cloud->points[i].z - centroid.z; 116 | xx += r.x * r.x; 117 | xy += r.x * r.y; 118 | xz += r.x * r.z; 119 | yy += r.y * r.y; 120 | yz += r.y * r.z; 121 | zz += r.z * r.z; 122 | } 123 | 124 | double det_x = yy*zz - yz*yz; 125 | double det_y = xx*zz - xz*xz; 126 | double det_z = xx*yy - xy*xy; 127 | 128 | double max = det_x; 129 | int max_letter = 1; 130 | if(det_y > max) 131 | { 132 | max = det_y; 133 | max_letter = 2; 134 | } 135 | if(det_z > max) 136 | { 137 | max = det_z; 138 | max_letter = 3; 139 | } 140 | 141 | Vector3 dir; 142 | if(max_letter == 1) 143 | { 144 | dir.x = det_x; 145 | dir.y = xz*yz - xy*zz; 146 | dir.z = xy*yz - xz*yy; 147 | } 148 | else if(max_letter == 2) 149 | { 150 | dir.x = xz*yz - xy*zz; 151 | dir.y = det_y; 152 | dir.z = xy*xz - yz*xx; 153 | } 154 | else if(max_letter == 3) 155 | { 156 | dir.x = xy*yz - xz*yy; 157 | dir.y = xy*xz - yz*xx; 158 | dir.z = det_z; 159 | } 160 | 161 | double length = (dir.x * dir.x) + (dir.y * dir.y) + (dir.z * dir.z); 162 | length = sqrt(length); 163 | 164 | dir.x = dir.x / length; 165 | dir.y = dir.y / length; 166 | dir.z = dir.z / length; 167 | 168 | double distance = 0; 169 | Vector3 subtract; 170 | Vector3 point_max; 171 | int index = -1; 172 | for(size_t i = 0; i < plot_only_cloud->points.size(); i++) 173 | { 174 | double temp_distance = 0; 175 | subtract.x = plot_only_cloud->points[i].x - centroid.x; 176 | subtract.y = plot_only_cloud->points[i].y - centroid.y; 177 | subtract.z = plot_only_cloud->points[i].z - centroid.z; 178 | temp_distance = (dir.x * subtract.x) + (dir.y * subtract.y) + (dir.z * subtract.z); 179 | 180 | double mag_dist = std::abs(temp_distance); 181 | 182 | height_file << mag_dist << std::endl; 183 | 184 | if(mag_dist > distance) { 185 | distance = mag_dist; 186 | point_max.x = plot_only_cloud->points[i].x; 187 | point_max.y = plot_only_cloud->points[i].y; 188 | point_max.z = plot_only_cloud->points[i].z; 189 | index = i; 190 | } 191 | } 192 | 193 | std::cerr << "Max distance in frame(meters): " << distance << " meters." << std::endl; 194 | //std::cerr << "Max distance in frame(inches): " << (distance * 39.3701) << " inches." << std::endl; 195 | 196 | 197 | return 0; 198 | } 199 | -------------------------------------------------------------------------------- /point_cloud_processing/src/velodyne_tf_broadcast.cpp: -------------------------------------------------------------------------------- 1 | #define _USE_MATH_DEFINES 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv){ 10 | ros::init(argc, argv, "velodyne_tf_broadcaster"); 11 | 12 | ros::NodeHandle nh; 13 | 14 | tf::TransformBroadcaster br; 15 | tf::Transform transform; 16 | 17 | geometry_msgs::Quaternion orientation; 18 | orientation.x = 0; 19 | orientation.y = 0; 20 | orientation.z = 0; 21 | orientation.w = 1; 22 | 23 | tf2::Quaternion q_orig, q_rot, q_new; 24 | 25 | tf2::convert(orientation , q_orig); 26 | double r = 0, p = M_PI_2, y = M_PI_2; 27 | 28 | q_rot.setRPY(r, p, y); 29 | 30 | q_new = q_rot * q_orig; 31 | q_new.normalize(); 32 | 33 | 34 | ros::Rate rate(10.0); 35 | while (nh.ok()){ 36 | transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) ); 37 | transform.setRotation( tf::Quaternion(q_new.x(), q_new.y(), q_new.z(), q_new.w()) ); 38 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "dji", "velodyne")); 39 | rate.sleep(); 40 | } 41 | 42 | ros::spin(); 43 | return 0; 44 | }; -------------------------------------------------------------------------------- /point_cloud_processing/src/view_colored_plot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | struct RGB_VAL{ 22 | double r; // a fraction between 0 and 1 23 | double g; // a fraction between 0 and 1 24 | double b; // a fraction between 0 and 1 25 | }; 26 | 27 | RGB_VAL hsv2rgb(double h, double sl, double l); 28 | 29 | RGB_VAL hsv2rgb(double h, double sl, double l) { 30 | 31 | double v; 32 | double r,g,b; 33 | 34 | r = l; // default to gray 35 | g = l; 36 | b = l; 37 | 38 | v = (l <= 0.5) ? (l * (1.0 + sl)) : (l + sl - l * sl); 39 | 40 | if (v > 0) { 41 | 42 | double m; 43 | double sv; 44 | int sextant; 45 | double fract, vsf, mid1, mid2; 46 | 47 | m = l + l - v; 48 | sv = (v - m ) / v; 49 | h *= 6.0; 50 | 51 | sextant = (int)h; 52 | fract = h - sextant; 53 | vsf = v * sv * fract; 54 | mid1 = m + vsf; 55 | mid2 = v - vsf; 56 | 57 | switch (sextant) { 58 | 59 | case 0: 60 | r = v; 61 | g = mid1; 62 | b = m; 63 | break; 64 | 65 | case 1: 66 | r = mid2; 67 | g = v; 68 | b = m; 69 | break; 70 | 71 | case 2: 72 | r = m; 73 | g = v; 74 | b = mid1; 75 | break; 76 | 77 | case 3: 78 | r = m; 79 | g = mid2; 80 | b = v; 81 | break; 82 | 83 | case 4: 84 | r = mid1; 85 | g = m; 86 | b = v; 87 | break; 88 | 89 | case 5: 90 | r = v; 91 | g = m; 92 | b = mid2; 93 | break; 94 | 95 | } 96 | 97 | } 98 | 99 | RGB_VAL rgb; 100 | 101 | rgb.r = r * 255; 102 | rgb.g = g * 255; 103 | rgb.b = b * 255; 104 | return rgb; 105 | 106 | } 107 | 108 | int main(int argc, char** argv) 109 | { 110 | 111 | if(argc != 4) 112 | { 113 | std::cerr << "crop_param_file plane_param_file point_cloud_file" << std::endl; 114 | return -1; 115 | } 116 | 117 | double distance_threshold = 0; 118 | 119 | std::ifstream infile(argv[2]); 120 | 121 | int count = 0; 122 | float param = 0; 123 | while(infile >> param) { 124 | switch(count) { 125 | case 0: 126 | distance_threshold = param; 127 | break; 128 | default: 129 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 130 | break; 131 | } 132 | count++; 133 | } 134 | 135 | std::ostringstream oss; 136 | oss << argv[3]; 137 | 138 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 139 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 140 | { 141 | PCL_ERROR("Couldn't read file "); 142 | PCL_ERROR(oss.str().c_str()); 143 | PCL_ERROR("\n"); 144 | return (-1); 145 | } 146 | 147 | double min_z = 100; 148 | double max_z = -100; 149 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 150 | { 151 | if(temp_cloud->points[i].z > max_z) { 152 | max_z = temp_cloud->points[i].z; 153 | } 154 | 155 | if(temp_cloud->points[i].z < min_z) { 156 | min_z = temp_cloud->points[i].z; 157 | } 158 | } 159 | 160 | float negative_x_filter = 0; 161 | float positive_x_filter = 0; 162 | float negative_y_filter = 0; 163 | float positive_y_filter = 0; 164 | float negative_z_filter = 0; 165 | float positive_z_filter = 0; 166 | float rotation_x = 0; 167 | float rotation_y = 0; 168 | float rotation_z = 0; 169 | 170 | std::ifstream infile2(argv[1]); 171 | 172 | count = 0; 173 | param = 0; 174 | while(infile2 >> param) { 175 | switch(count) { 176 | case 0: 177 | negative_x_filter = param; 178 | break; 179 | case 1: 180 | positive_x_filter = param; 181 | break; 182 | case 2: 183 | negative_y_filter = param; 184 | break; 185 | case 3: 186 | positive_y_filter = param; 187 | break; 188 | case 4: 189 | negative_z_filter = param; 190 | break; 191 | case 5: 192 | positive_z_filter = param; 193 | break; 194 | case 6: 195 | rotation_x = param; 196 | break; 197 | case 7: 198 | rotation_y = param; 199 | break; 200 | case 8: 201 | rotation_z = param; 202 | break; 203 | default: 204 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 205 | break; 206 | } 207 | count++; 208 | } 209 | 210 | 211 | pcl::PointCloud::Ptr bodyFiltered(new pcl::PointCloud); 212 | pcl::CropBox boxFilter; 213 | //boxFilter.setTranslation(Eigen::Vector3f(4, 1.5, 0)); 214 | boxFilter.setMin(Eigen::Vector4f(negative_x_filter, negative_y_filter, negative_z_filter, 1.0)); 215 | boxFilter.setMax(Eigen::Vector4f(positive_x_filter, positive_y_filter, positive_z_filter, 1.0)); 216 | //boxFilter.setMin(Eigen::Vector4f(-1, -3, -100, 1.0)); 217 | //boxFilter.setMax(Eigen::Vector4f(1, 3, 100, 1.0)); 218 | boxFilter.setRotation(Eigen::Vector3f(rotation_x, rotation_y, rotation_z)); 219 | boxFilter.setInputCloud(temp_cloud); 220 | boxFilter.filter(*bodyFiltered); 221 | 222 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 223 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 224 | // Create segmentation object 225 | pcl::SACSegmentation seg; 226 | //Optional 227 | seg.setOptimizeCoefficients(true); 228 | seg.setAxis(Eigen::Vector3f(0,0,1)); 229 | // Mandatory 230 | seg.setModelType(pcl::SACMODEL_PLANE); 231 | seg.setMethodType(pcl::SAC_RANSAC); 232 | seg.setDistanceThreshold(distance_threshold); 233 | 234 | seg.setInputCloud(bodyFiltered); 235 | seg.segment(*inliers, *coefficients); 236 | 237 | if(inliers->indices.size() == 0) 238 | { 239 | PCL_ERROR("Could not estimate a planar model for the given dataset."); 240 | return -1; 241 | } 242 | 243 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 244 | cloud_rgb->points.resize(bodyFiltered->size()); 245 | double s = 0.5; 246 | double v = 0.5; 247 | double h = 0; 248 | struct RGB_VAL rgb_out; 249 | for(size_t i = 0; i < bodyFiltered->points.size(); i++) 250 | { 251 | 252 | cloud_rgb->points[i].x = bodyFiltered->points[i].x; 253 | cloud_rgb->points[i].y = bodyFiltered->points[i].y; 254 | cloud_rgb->points[i].z = bodyFiltered->points[i].z; 255 | cloud_rgb->points[i].r = 0; 256 | cloud_rgb->points[i].g = 0; 257 | cloud_rgb->points[i].b = 0; 258 | } 259 | 260 | for(size_t i = 0; i < inliers->indices.size(); ++i) 261 | { 262 | h = (cloud_rgb->points[inliers->indices[i]].z - min_z) / (max_z - min_z); 263 | rgb_out = hsv2rgb(h, s, v); 264 | cloud_rgb->points[inliers->indices[i]].r = rgb_out.r; 265 | cloud_rgb->points[inliers->indices[i]].g = rgb_out.g; 266 | cloud_rgb->points[inliers->indices[i]].b = rgb_out.b; 267 | } 268 | 269 | pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 270 | viewer->setBackgroundColor (1, 1, 1); 271 | pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud_rgb); 272 | viewer->addPointCloud (cloud_rgb, rgb, "sample cloud"); 273 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); 274 | viewer->addCoordinateSystem (1.0); 275 | viewer->initCameraParameters (); 276 | 277 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 278 | //viewer.showCloud(temp_cloud); 279 | //while(!viewer.wasStopped ()) 280 | //{ 281 | //} 282 | 283 | while (!viewer->wasStopped ()) 284 | { 285 | viewer->spinOnce (100); 286 | } 287 | return 0; 288 | } 289 | -------------------------------------------------------------------------------- /point_cloud_processing/src/view_grid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | #include 10 | #include 11 | 12 | struct RGB_VAL{ 13 | double r; // a fraction between 0 and 1 14 | double g; // a fraction between 0 and 1 15 | double b; // a fraction between 0 and 1 16 | }; 17 | 18 | RGB_VAL hsv2rgb(double h, double sl, double l); 19 | 20 | RGB_VAL hsv2rgb(double h, double sl, double l) { 21 | 22 | double v; 23 | double r,g,b; 24 | 25 | r = l; // default to gray 26 | g = l; 27 | b = l; 28 | 29 | v = (l <= 0.5) ? (l * (1.0 + sl)) : (l + sl - l * sl); 30 | 31 | if (v > 0) { 32 | 33 | double m; 34 | double sv; 35 | int sextant; 36 | double fract, vsf, mid1, mid2; 37 | 38 | m = l + l - v; 39 | sv = (v - m ) / v; 40 | h *= 6.0; 41 | 42 | sextant = (int)h; 43 | fract = h - sextant; 44 | vsf = v * sv * fract; 45 | mid1 = m + vsf; 46 | mid2 = v - vsf; 47 | 48 | switch (sextant) { 49 | 50 | case 0: 51 | r = v; 52 | g = mid1; 53 | b = m; 54 | break; 55 | 56 | case 1: 57 | r = mid2; 58 | g = v; 59 | b = m; 60 | break; 61 | 62 | case 2: 63 | r = m; 64 | g = v; 65 | b = mid1; 66 | break; 67 | 68 | case 3: 69 | r = m; 70 | g = mid2; 71 | b = v; 72 | break; 73 | 74 | case 4: 75 | r = mid1; 76 | g = m; 77 | b = v; 78 | break; 79 | 80 | case 5: 81 | r = v; 82 | g = m; 83 | b = mid2; 84 | break; 85 | 86 | } 87 | 88 | } 89 | 90 | RGB_VAL rgb; 91 | 92 | rgb.r = r * 255; 93 | rgb.g = g * 255; 94 | rgb.b = b * 255; 95 | return rgb; 96 | 97 | } 98 | 99 | int main(int argc, char** argv) 100 | { 101 | if(argc != 3) 102 | { 103 | std::cerr << "Expecting input argument: point_cloud_file clusters_data_file" << std::endl; 104 | return -1; 105 | } 106 | 107 | std::ostringstream oss; 108 | oss << argv[1]; 109 | 110 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 111 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 112 | { 113 | PCL_ERROR("Couldn't read file "); 114 | PCL_ERROR(oss.str().c_str()); 115 | PCL_ERROR("\n"); 116 | return (-1); 117 | } 118 | 119 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 120 | cloud_rgb->points.resize(temp_cloud->size()); 121 | double min_z = 100; 122 | double max_z = -100; 123 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 124 | { 125 | if(temp_cloud->points[i].z > max_z) { 126 | max_z = temp_cloud->points[i].z; 127 | } 128 | 129 | if(temp_cloud->points[i].z < min_z) { 130 | min_z = temp_cloud->points[i].z; 131 | } 132 | } 133 | 134 | double s = 0.5; 135 | double v = 0.5; 136 | double h = 0; 137 | struct RGB_VAL rgb_out; 138 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 139 | { 140 | 141 | cloud_rgb->points[i].x = temp_cloud->points[i].x; 142 | cloud_rgb->points[i].y = temp_cloud->points[i].y; 143 | cloud_rgb->points[i].z = temp_cloud->points[i].z; 144 | h = (temp_cloud->points[i].z - min_z) / (max_z - min_z); 145 | rgb_out = hsv2rgb(h, s, v); 146 | cloud_rgb->points[i].r = rgb_out.r; 147 | cloud_rgb->points[i].g = rgb_out.g; 148 | cloud_rgb->points[i].b = rgb_out.b; 149 | } 150 | 151 | pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 152 | viewer->setBackgroundColor (1, 1, 1); 153 | pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud_rgb); 154 | viewer->addPointCloud (cloud_rgb, rgb, "sample cloud"); 155 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); 156 | 157 | std::ifstream infile(argv[2]); 158 | std::string line; 159 | int linecount = 0; 160 | int obb_count = 0; 161 | while(std::getline(infile, line)) 162 | { 163 | if(linecount == 0) 164 | { 165 | linecount++; 166 | continue; 167 | } 168 | std::istringstream s(line); 169 | std::string field; 170 | int fieldcount = 0; 171 | int clustercount = -1; 172 | double rotation = -1; 173 | double height = -1; 174 | double width = -1; 175 | double box_x = -1; 176 | double box_y = -1; 177 | while(getline(s, field, ',')) 178 | { 179 | if(fieldcount == 0) 180 | clustercount = std::atoi(field.c_str()); 181 | else if(fieldcount == 1) 182 | rotation = std::atof(field.c_str()); 183 | else if(fieldcount == 2) 184 | height = std::atof(field.c_str()); 185 | else if(fieldcount == 3) 186 | width = std::atof(field.c_str()); 187 | else if(fieldcount == 4) 188 | box_x = std::atof(field.c_str()); 189 | else if(fieldcount == 5) 190 | box_y = std::atof(field.c_str()); 191 | /* std::cout << clustercount << std::endl; 192 | std::cout << rotation << std::endl; 193 | std::cout << width << std::endl; 194 | std::cout << height << std::endl; 195 | std::cout << box_x << std::endl; 196 | std::cout << box_y << std::endl; 197 | */ 198 | fieldcount++; 199 | } 200 | std::ostringstream obb_number; 201 | obb_number << "obb " << obb_count; 202 | obb_count++; 203 | 204 | viewer->addCube(box_x - (width/2), box_x + (width/2), box_y - (height/2), box_y + (height/2), 1, 1, 0, 0, 0, obb_number.str()); 205 | 206 | } 207 | //viewer->addCube(9, 11, -1.5, 4.5, 0, 0, 0, 0, 0, "grid"); 208 | viewer->addCoordinateSystem (1.0); 209 | viewer->initCameraParameters (); 210 | 211 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 212 | //viewer.showCloud(temp_cloud); 213 | //while(!viewer.wasStopped ()) 214 | //{ 215 | //} 216 | 217 | while (!viewer->wasStopped ()) 218 | { 219 | viewer->spinOnce (100); 220 | } 221 | return 0; 222 | } 223 | -------------------------------------------------------------------------------- /point_cloud_processing/src/view_oriented_grid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | #include 10 | #include 11 | 12 | Eigen::Quaternionf euler2Quaternion(const double roll, const double pitch, const double yaw); 13 | 14 | Eigen::Quaternionf euler2Quaternion(const double roll, const double pitch, const double yaw) 15 | { 16 | Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitZ()); 17 | Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitY()); 18 | Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitX()); 19 | 20 | Eigen::Quaternionf q = rollAngle * yawAngle * pitchAngle; 21 | return q; 22 | } 23 | 24 | int main(int argc, char** argv) 25 | { 26 | if(argc != 3) 27 | { 28 | std::cerr << "Expecting input argument: point_cloud_file clusters_data_file" << std::endl; 29 | return -1; 30 | } 31 | 32 | std::ostringstream oss; 33 | oss << argv[1]; 34 | 35 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 36 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 37 | { 38 | PCL_ERROR("Couldn't read file "); 39 | PCL_ERROR(oss.str().c_str()); 40 | PCL_ERROR("\n"); 41 | return (-1); 42 | } 43 | 44 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 45 | cloud_rgb->points.resize(temp_cloud->size()); 46 | double min_z = 100; 47 | double max_z = -100; 48 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 49 | { 50 | if(temp_cloud->points[i].z > max_z) { 51 | max_z = temp_cloud->points[i].z; 52 | } 53 | 54 | if(temp_cloud->points[i].z < min_z) { 55 | min_z = temp_cloud->points[i].z; 56 | } 57 | } 58 | 59 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 60 | { 61 | 62 | cloud_rgb->points[i].x = temp_cloud->points[i].x; 63 | cloud_rgb->points[i].y = temp_cloud->points[i].y; 64 | cloud_rgb->points[i].z = temp_cloud->points[i].z; 65 | cloud_rgb->points[i].r = 255; 66 | cloud_rgb->points[i].g = 255; 67 | cloud_rgb->points[i].b = 255; 68 | } 69 | 70 | pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 71 | viewer->setBackgroundColor (0, 0, 0); 72 | pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud_rgb); 73 | viewer->addPointCloud (cloud_rgb, rgb, "sample cloud"); 74 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); 75 | 76 | std::ifstream infile(argv[2]); 77 | std::string line; 78 | int linecount = 0; 79 | int obb_count = 0; 80 | while(std::getline(infile, line)) 81 | { 82 | if(linecount == 0) 83 | { 84 | linecount++; 85 | continue; 86 | } 87 | std::istringstream s(line); 88 | std::string field; 89 | int fieldcount = 0; 90 | int clustercount = -1; 91 | double rotation = -1; 92 | double height = -1; 93 | double width = -1; 94 | double box_x = -1; 95 | double box_y = -1; 96 | while(getline(s, field, ',')) 97 | { 98 | if(fieldcount == 0) 99 | clustercount = std::atoi(field.c_str()); 100 | else if(fieldcount == 1) 101 | rotation = std::atof(field.c_str()); 102 | else if(fieldcount == 2) 103 | height = std::atof(field.c_str()); 104 | else if(fieldcount == 3) 105 | width = std::atof(field.c_str()); 106 | else if(fieldcount == 4) 107 | box_x = std::atof(field.c_str()); 108 | else if(fieldcount == 5) 109 | box_y = std::atof(field.c_str()); 110 | /* std::cout << clustercount << std::endl; 111 | std::cout << rotation << std::endl; 112 | std::cout << width << std::endl; 113 | std::cout << height << std::endl; 114 | std::cout << box_x << std::endl; 115 | std::cout << box_y << std::endl; 116 | */ 117 | fieldcount++; 118 | } 119 | std::ostringstream obb_number; 120 | obb_number << "obb " << obb_count; 121 | obb_count++; 122 | 123 | Eigen::Quaternionf q = euler2Quaternion(rotation, 0, 0); 124 | 125 | viewer->addCube(Eigen::Vector3f(box_x, box_y, 0), q, width, height, 0, obb_number.str()); 126 | //viewer->addCube(box_x - (width/2), box_x + (width/2), box_y - (height/2), box_y + (height/2), 0, 0, 0, 0, 0, obb_number.str()); 127 | 128 | } 129 | //viewer->addCube(9, 11, -1.5, 4.5, 0, 0, 0, 0, 0, "grid"); 130 | viewer->addCoordinateSystem (1.0); 131 | viewer->initCameraParameters (); 132 | 133 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 134 | //viewer.showCloud(temp_cloud); 135 | //while(!viewer.wasStopped ()) 136 | //{ 137 | //} 138 | 139 | while (!viewer->wasStopped ()) 140 | { 141 | viewer->spinOnce (100); 142 | } 143 | return 0; 144 | } 145 | -------------------------------------------------------------------------------- /point_cloud_processing/src/view_single_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | 10 | struct RGB_VAL{ 11 | double r; // a fraction between 0 and 1 12 | double g; // a fraction between 0 and 1 13 | double b; // a fraction between 0 and 1 14 | }; 15 | 16 | RGB_VAL hsv2rgb(double h, double sl, double l); 17 | 18 | RGB_VAL hsv2rgb(double h, double sl, double l) { 19 | 20 | double v; 21 | double r,g,b; 22 | 23 | r = l; // default to gray 24 | g = l; 25 | b = l; 26 | 27 | v = (l <= 0.5) ? (l * (1.0 + sl)) : (l + sl - l * sl); 28 | 29 | if (v > 0) { 30 | 31 | double m; 32 | double sv; 33 | int sextant; 34 | double fract, vsf, mid1, mid2; 35 | 36 | m = l + l - v; 37 | sv = (v - m ) / v; 38 | h *= 6.0; 39 | 40 | sextant = (int)h; 41 | fract = h - sextant; 42 | vsf = v * sv * fract; 43 | mid1 = m + vsf; 44 | mid2 = v - vsf; 45 | 46 | switch (sextant) { 47 | 48 | case 0: 49 | r = v; 50 | g = mid1; 51 | b = m; 52 | break; 53 | 54 | case 1: 55 | r = mid2; 56 | g = v; 57 | b = m; 58 | break; 59 | 60 | case 2: 61 | r = m; 62 | g = v; 63 | b = mid1; 64 | break; 65 | 66 | case 3: 67 | r = m; 68 | g = mid2; 69 | b = v; 70 | break; 71 | 72 | case 4: 73 | r = mid1; 74 | g = m; 75 | b = v; 76 | break; 77 | 78 | case 5: 79 | r = v; 80 | g = m; 81 | b = mid2; 82 | break; 83 | 84 | } 85 | 86 | } 87 | 88 | RGB_VAL rgb; 89 | 90 | rgb.r = r * 255; 91 | rgb.g = g * 255; 92 | rgb.b = b * 255; 93 | return rgb; 94 | 95 | } 96 | 97 | int main(int argc, char** argv) 98 | { 99 | if(argc != 2) 100 | { 101 | std::cerr << "Expecting input argument: file" << std::endl; 102 | return -1; 103 | } 104 | 105 | std::ostringstream oss; 106 | oss << argv[1]; 107 | 108 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 109 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 110 | { 111 | PCL_ERROR("Couldn't read file "); 112 | PCL_ERROR(oss.str().c_str()); 113 | PCL_ERROR("\n"); 114 | return (-1); 115 | } 116 | 117 | pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud); 118 | cloud_rgb->points.resize(temp_cloud->size()); 119 | double min_z = 100; 120 | double max_z = -100; 121 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 122 | { 123 | if(temp_cloud->points[i].z > max_z) { 124 | max_z = temp_cloud->points[i].z; 125 | } 126 | 127 | if(temp_cloud->points[i].z < min_z) { 128 | min_z = temp_cloud->points[i].z; 129 | } 130 | } 131 | 132 | std::cout << "Min Z Value: " << min_z << std::endl; 133 | std::cout << "Max Z Value: " << max_z << std::endl; 134 | 135 | double s = 0.5; 136 | double v = 0.5; 137 | double h = 0; 138 | struct RGB_VAL rgb_out; 139 | for(size_t i = 0; i < temp_cloud->points.size(); i++) 140 | { 141 | 142 | cloud_rgb->points[i].x = temp_cloud->points[i].x; 143 | cloud_rgb->points[i].y = temp_cloud->points[i].y; 144 | cloud_rgb->points[i].z = temp_cloud->points[i].z; 145 | h = (temp_cloud->points[i].z - min_z) / (max_z - min_z); 146 | rgb_out = hsv2rgb(h, s, v); 147 | cloud_rgb->points[i].r = rgb_out.r; 148 | cloud_rgb->points[i].g = rgb_out.g; 149 | cloud_rgb->points[i].b = rgb_out.b; 150 | } 151 | 152 | pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 153 | viewer->setBackgroundColor (1, 1, 1); 154 | pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud_rgb); 155 | viewer->addPointCloud (cloud_rgb, rgb, "sample cloud"); 156 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); 157 | viewer->addCoordinateSystem (1.0); 158 | viewer->initCameraParameters (); 159 | 160 | //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 161 | //viewer.showCloud(temp_cloud); 162 | //while(!viewer.wasStopped ()) 163 | //{ 164 | //} 165 | 166 | while (!viewer->wasStopped ()) 167 | { 168 | viewer->spinOnce (100); 169 | } 170 | return 0; 171 | } 172 | -------------------------------------------------------------------------------- /point_cloud_processing/src/voting_scheme.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | int main(int argc, char** argv) 17 | { 18 | std::list > cluster_list; 19 | if(argc != 3) 20 | { 21 | std::cerr << "Expecting input argument: folder #_of_.pcd_files " << std::endl; 22 | return -1; 23 | } 24 | 25 | std::ostringstream oss; 26 | oss << argv[1] << "cluster_"; 27 | 28 | std::istringstream ss(argv[2]); 29 | int x; 30 | if(!(ss >> x)) { 31 | std::cerr << "Invalid number: " << argv[1] << '\n'; 32 | } 33 | else if (!ss.eof()) { 34 | std::cerr << "Trailing characters after number: " << argv[1] << '\n'; 35 | } 36 | 37 | for(int i = 0; i < x; i++) { 38 | std::cout << "Count: " << i << std::endl; 39 | std::ostringstream oss2; 40 | oss2 << oss.str() << i << ".pcd"; 41 | pcl::PointCloud cloud; 42 | if(pcl::io::loadPCDFile (oss2.str(), cloud) == -1) 43 | { 44 | PCL_ERROR("Couldn't read file "); 45 | PCL_ERROR(oss.str().c_str()); 46 | PCL_ERROR("\n"); 47 | return (-1); 48 | } 49 | cluster_list.push_back(cloud); 50 | } 51 | 52 | std::ofstream myfile; 53 | myfile.open ("/home/naik/Data/PointCloud/2-22-2020_Simulated/clusters_data.csv"); 54 | int count = 0; 55 | std::list >::iterator it; 56 | for(it = cluster_list.begin(); it != cluster_list.end(); ++it) { 57 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 58 | pcl::copyPointCloud(*it, *cloud); 59 | 60 | std::ostringstream cluster_number; 61 | cluster_number << "cluster " << count; 62 | std::ostringstream obb_number; 63 | obb_number << "obb " << count; 64 | 65 | double average = 0; 66 | for(size_t i = 0; i < cloud->points.size(); i++) 67 | { 68 | average += cloud->points[i].z; 69 | } 70 | average /= cloud->points.size(); 71 | 72 | for(size_t i = 0; i < cloud->points.size(); i++) 73 | { 74 | cloud->points[i].z = average; 75 | } 76 | 77 | pcl::MomentOfInertiaEstimation feature_extractor; 78 | feature_extractor.setInputCloud (cloud); 79 | feature_extractor.compute (); 80 | 81 | std::vector moment_of_inertia; 82 | std::vector eccentricity; 83 | pcl::PointXYZ min_point_OBB; 84 | pcl::PointXYZ max_point_OBB; 85 | pcl::PointXYZ position_OBB; 86 | Eigen::Matrix3f rotational_matrix_OBB; 87 | float major_value, middle_value, minor_value; 88 | Eigen::Vector3f major_vector, middle_vector, minor_vector; 89 | Eigen::Vector3f mass_center; 90 | 91 | feature_extractor.getMomentOfInertia (moment_of_inertia); 92 | feature_extractor.getEccentricity (eccentricity); 93 | feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); 94 | feature_extractor.getEigenValues (major_value, middle_value, minor_value); 95 | feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); 96 | feature_extractor.getMassCenter (mass_center); 97 | 98 | Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); 99 | Eigen::Quaternionf quat (rotational_matrix_OBB); 100 | 101 | Eigen::Vector3f euler = quat.toRotationMatrix().eulerAngles(2, 1, 0); 102 | double width = max_point_OBB.x - min_point_OBB.x; 103 | double height = max_point_OBB.y - min_point_OBB.y; 104 | 105 | myfile << count << "," << euler[0] << "," << width << "," << height << "," 106 | << position_OBB.x << "," << position_OBB.y << "," << position_OBB.z 107 | << std::endl; 108 | std::cout << "Yaw: " << euler[0] << std::endl; 109 | std::cout << "Width: " << width << std::endl; 110 | std::cout << "Height: " << height << std::endl; 111 | 112 | count++; 113 | } 114 | 115 | myfile.close(); 116 | return (0); 117 | } 118 | -------------------------------------------------------------------------------- /point_cloud_processing/src/voxel_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | int main (int argc, char** argv) 9 | { 10 | 11 | if(argc != 3) 12 | { 13 | std::cerr << "param_file point_cloud_file" << std::endl; 14 | return -1; 15 | } 16 | 17 | double x_leaf = 0; 18 | double y_leaf = 0; 19 | double z_leaf = 0; 20 | 21 | std::ifstream infile(argv[1]); 22 | 23 | int count = 0; 24 | float param = 0; 25 | while(infile >> param) { 26 | switch(count) { 27 | case 0: 28 | x_leaf = param; 29 | break; 30 | case 1: 31 | y_leaf = param; 32 | break; 33 | case 2: 34 | z_leaf = param; 35 | break; 36 | default: 37 | std::cout << "Shouldn't get to this. Param file setup incorrectly." << std::endl; 38 | break; 39 | } 40 | count++; 41 | } 42 | 43 | std::ostringstream oss; 44 | oss << argv[2]; 45 | 46 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 47 | if(pcl::io::loadPCDFile (oss.str(), *temp_cloud) == -1) 48 | { 49 | PCL_ERROR("Couldn't read file "); 50 | PCL_ERROR(oss.str().c_str()); 51 | PCL_ERROR("\n"); 52 | return (-1); 53 | } 54 | 55 | pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ()); 56 | 57 | pcl::toPCLPointCloud2(*temp_cloud, *cloud); 58 | pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); 59 | 60 | // Create the filtering object 61 | pcl::VoxelGrid sor; 62 | sor.setInputCloud (cloud); 63 | sor.setLeafSize (x_leaf, y_leaf, z_leaf); 64 | sor.filter (*cloud_filtered); 65 | 66 | 67 | pcl::PointCloud::Ptr pc_filtered(new pcl::PointCloud); 68 | pcl::fromPCLPointCloud2( *cloud_filtered, *pc_filtered); 69 | 70 | std::string new_name = oss.str().substr(0, oss.str().size()-4); 71 | std::ostringstream oss2; 72 | oss2 << new_name << "_voxel_filtered.pcd"; 73 | pcl::io::savePCDFileASCII(oss2.str(), *pc_filtered); 74 | return (0); 75 | } -------------------------------------------------------------------------------- /point_cloud_processing/src/zed_cloud_view.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | std::ostringstream oss; 17 | std::ostringstream oss2; 18 | pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); 19 | 20 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input) 21 | { 22 | 23 | pcl::PCLPointCloud2 pcl_pc2; 24 | pcl_conversions::toPCL(*input, pcl_pc2); 25 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 26 | pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud); 27 | //std::cerr << "Point cloud data: " << temp_cloud->points.size () << std::endl; 28 | 29 | viewer.showCloud(temp_cloud); 30 | 31 | } 32 | 33 | int main(int argc, char** argv) 34 | { 35 | 36 | ros::init(argc, argv, "point_cloud_processing"); 37 | ros::NodeHandle nh; 38 | 39 | ros::Subscriber sub = nh.subscribe("/zed/point_cloud/cloud_registered", 1, cloud_cb); 40 | 41 | ros::spin(); 42 | } 43 | --------------------------------------------------------------------------------