points = np.loadtxt(r'../Data/pole2.txt', skiprows=1, delimiter=';')[:, :3]
#MyRANSAC.Model = CylinderModel((0,0), (0.05, 0.3)) #Theta and Phi, Min Max radius of the cylinders
#PlaneModel()
from Runner import Search
from geometry import *
Result = Search(1, points, CylinderModel(0.05, (0.02, 0.05), (0, 0)))
Result = Search(1, points, PlaneModel(0.05))
resultPoints = Result[1][0]
#resultPoints = np.vstack((Result[1][0], Result[1][1]))
# For visualization:
pcd = PointCloud()
pcd.points = Vector3dVector(Result[2])
pcd2 = PointCloud()
pcd2.points = Vector3dVector(resultPoints)
pcd2.paint_uniform_color([0.1, 0.1, 0.1])
estimate_normals(pcd2,
search_param=KDTreeSearchParamHybrid(radius=0.1, max_nn=20))
draw_geometries([pcd2])
#########
pcd = PointCloud()
pcd.points = Vector3dVector(points)
draw_geometries([pcd])
def open_3d_vis(args, output_dir):
"""
http://www.open3d.org/docs/tutorial/Basic/visualization.html
-- Mouse view control --
Left button + drag : Rotate.
Ctrl + left button + drag : Translate.
Wheel : Zoom in/out.
-- Keyboard view control --
[/] : Increase/decrease field of view.
R : Reset view point.
Ctrl/Cmd + C : Copy current view status into the clipboard. (A nice view has been saved as utilites/view.json
Ctrl/Cmd + V : Paste view status from clipboard.
-- General control --
Q, Esc : Exit window.
H : Print help message.
P, PrtScn : Take a screen capture.
D : Take a depth capture.
O : Take a capture of current rendering settings.
"""
json_dir = os.path.join(
output_dir, 'json_' + args.list_flag + '_trans') + '_iou' + str(1.0)
args.test_dir = json_dir
args.gt_dir = args.dataset_dir + 'car_poses'
args.res_file = os.path.join(output_dir,
'json_' + args.list_flag + '_res.txt')
args.simType = None
car_models = load_car_models(os.path.join(args.dataset_dir, 'car_models'))
det_3d_metric = Detect3DEval(args)
evalImgs = det_3d_metric.evaluate()
image_id = evalImgs['image_id']
print(image_id)
# We only draw the most loose constraint here
gtMatches = evalImgs['gtMatches'][0]
dtScores = evalImgs['dtScores']
mesh_car_all = []
# We also save road surface
xmin, xmax, ymin, ymax, zmin, zmax = np.inf, -np.inf, np.inf, -np.inf, np.inf, -np.inf
for car in det_3d_metric._gts[image_id]:
car_model = car_models[car['car_id']]
R = euler_angles_to_rotation_matrix(car['pose'][:3])
vertices = np.matmul(R, car_model['vertices'].T) + np.asarray(
car['pose'][3:])[:, None]
xmin, xmax, ymin, ymax, zmin, zmax = update_road_surface(
xmin, xmax, ymin, ymax, zmin, zmax, vertices)
mesh_car = TriangleMesh()
mesh_car.vertices = Vector3dVector(vertices.T)
mesh_car.triangles = Vector3iVector(car_model['faces'] - 1)
# Computing normal
mesh_car.compute_vertex_normals()
# we render the GT car in BLUE
mesh_car.paint_uniform_color([0, 0, 1])
mesh_car_all.append(mesh_car)
for i, car in enumerate(det_3d_metric._dts[image_id]):
if dtScores[i] > args.dtScores:
car_model = car_models[car['car_id']]
R = euler_angles_to_rotation_matrix(car['pose'][:3])
vertices = np.matmul(R, car_model['vertices'].T) + np.asarray(
car['pose'][3:])[:, None]
mesh_car = TriangleMesh()
mesh_car.vertices = Vector3dVector(vertices.T)
mesh_car.triangles = Vector3iVector(car_model['faces'] - 1)
# Computing normal
mesh_car.compute_vertex_normals()
if car['id'] in gtMatches:
# if it is a true positive, we render it as green
mesh_car.paint_uniform_color([0, 1, 0])
else:
# else we render it as red
mesh_car.paint_uniform_color([1, 0, 0])
mesh_car_all.append(mesh_car)
# Draw the road surface here
# x = np.linspace(xmin, xmax, 100)
# every 0.1 meter
xyz = get_road_surface_xyz(xmin, xmax, ymin, ymax, zmin, zmax)
# Pass xyz to Open3D.PointCloud and visualize
pcd_road = PointCloud()
pcd_road.points = Vector3dVector(xyz)
pcd_road.paint_uniform_color([0, 0, 1])
mesh_car_all.append(pcd_road)
# draw mesh frame
mesh_frame = create_mesh_coordinate_frame(size=3, origin=[0, 0, 0])
mesh_car_all.append(mesh_frame)
draw_geometries(mesh_car_all)
det_3d_metric.accumulate()
det_3d_metric.summarize()