Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
if props.db_type == 'sqlite':
query = "PRAGMA table_info(%s)"%(props.image_table)
w_col = [_[1] for _ in db.execute(query) if _[1].find('Image_Width') >= 0][0]
h_col = [_[1] for _ in db.execute(query) if _[1].find('Image_Height') >= 0][0]
else:
query = "SELECT * FROM information_schema.COLUMNS WHERE TABLE_SCHEMA = '%s' AND TABLE_NAME = '%s' AND COLUMN_NAME REGEXP 'Image_Width' LIMIT 1"%(props.db_name, props.image_table)
w_col = db.execute(query)[0][0]
query = "SELECT * FROM information_schema.COLUMNS WHERE TABLE_SCHEMA = '%s' AND TABLE_NAME = '%s' AND COLUMN_NAME REGEXP 'Image_Height' LIMIT 1"%(props.db_name, props.image_table)
h_col = db.execute(query)[0][0]
query = "SELECT %s FROM %s LIMIT 1"%(w_col, props.image_table)
self.parent.image_x_dims = db.execute(query)[0][0]
query = "SELECT %s FROM %s LIMIT 1"%(h_col, props.image_table)
self.parent.image_y_dims = db.execute(query)[0][0]
ax = mlab.axes(self.trajectory_line_source,
xlabel='X', ylabel='Y',zlabel='T',
#extent = (1,self.parent.image_x_dims,1,self.parent.image_y_dims,self.parent.start_frame,self.parent.end_frame),
opacity = self.axes_opacity,
x_axis_visibility=True, y_axis_visibility=True, z_axis_visibility=True)
# Set axes to MATLAB's default 3d view
mlab.view(azimuth = 322.5,elevation = 30.0,
figure = self.trajectory_scene.mayavi_scene)
# Add object label text at end of trajectory
text_scale_factor = self.trajectory_node_scale_factor*5
end_nodes = {}
for (key,subgraph) in self.connected_nodes.items():
end_nodes[key] = [_[0] for _ in nx.get_node_attributes(subgraph,END_NODES).items() if _[1]][0]
self.trajectory_label_collection = dict(zip(self.connected_nodes.keys(),
[mlab.text3d(subgraph.node[end_nodes[key]]["x"],
mlab.plot3d(cam_axis_line[:,0], cam_axis_line[:,1], cam_axis_line[:,2], color=(1,1,1), tube_radius=0.0025)
#cam_axis_line = np.array([target_x0_closest, target_x0_closest + t_stp_stp_p])
#mlab.plot3d(cam_axis_line[:,0], cam_axis_line[:,1], cam_axis_line[:,2], color=(0,0,0), tube_radius=0.0025)
"""
t = 1e-2
pair = np.zeros([2,3])
for k in subsample_inds2.tolist():
pair[0,:] = source_object_points[k,:]
pair[1,:] = source_object_points[k,:] + t * source_object_normals[k,:]
mlab.plot3d(pair[:,0], pair[:,1], pair[:,2], color=(0,0,1), line_width=0.1, tube_radius=None)
"""
T_obj_world = mv.MayaviVisualizer.plot_stable_pose(object_mesh, neighbor_image.stable_pose, T_table_world, d=0.15)
mlab.axes()
mlab.show()
# point to plane ICP solver
ppis = reg.PointToPlaneICPSolver(sample_size=icp_sample_size, gamma=icp_relative_point_plane_cost, mu=icp_regularization_lambda)
ppfm = fm.PointToPlaneFeatureMatcher(dist_thresh=feature_matcher_dist_thresh, norm_thresh=feature_matcher_norm_thresh)
registration = ppis.register_2d(source_object_points, target_object_points, source_object_normals, target_object_normals, ppfm, num_iterations=num_registration_iters,
compute_total_cost=compute_total_cost, vis=debug)
registration_results.append(registration)
logging.info('Neighbor %d registration cost %f' %(i, registration.cost))
if registration.cost < min_cost:
min_cost = registration.cost
best_reg = registration
best_T_stp_camera = T_stp_stp_p.dot(T_stp_camera)
best_T_stp_obj = T_stp_obj
best_T_stp_stp_p = stf.SimilarityTransform3D(pose=tfx.pose(best_reg.R, best_reg.t), from_frame='stp', to_frame='stp')
X,Y,Z = mgrid[g.x_min:g.x_max:1j*g.nxsteps,\
g.y_min:g.y_max:1j*g.nysteps,\
g.z_min:g.z_max:1j*g.nzsteps]
data = mlab.pipeline.scalar_field(X,Y,Z,L1)
mlab.pipeline.iso_surface(data,contours=arange(mx-10,mx,1).tolist(),vmin=mx-10,vmax=mx)
# uncomment one of the following lines to see a different visualization of
# the data
#mlab.contour3d(X,Y,Z,L1,vmin=mx-5,vmax=mx,transparent=True)
#mlab.points3d(X,Y,Z,L1,vmin=mx-5,vmax=mx,transparent=True)
#===============================================================================
# adds some axes and enters the GUI main loop
#===============================================================================
mlab.axes()
mlab.show()
def write_mesh_stable_poses(self, mesh, filename, min_prob=0, vis=False):
prob_mapping, cv_hull = st.compute_stable_poses(mesh), mesh.convex_hull()
R_list = []
for face, p in prob_mapping.items():
if p >= min_prob:
R_list.append([p, st.compute_basis([cv_hull.vertices()[i] for i in face])])
if vis:
print 'P', R_list[0][0]
mv.figure()
mesh.visualize()
mv.axes()
mv.figure()
cv_hull_tf = cv_hull.transform(stf.SimilarityTransform3D(tfx.transform(R_list[0][1], np.zeros(3))))
cv_hull_tf.visualize()
mv.axes()
mv.show()
f = open(filename[:-4] + ".stp", "w")
f.write("#############################################################\n")
f.write("# STP file generated by UC Berkeley Automation Sciences Lab #\n")
f.write("# #\n")
f.write("# Num Poses: %d" %len(R_list))
for _ in range(46 - len(str(len(R_list)))):
f.write(" ")
f.write(" #\n")
f.write("# Min Probability: %s" %str(min_prob))
def show(self):
'''Show the 3D diagram in the screen.'''
from mayavi import mlab
self.triangleMesh= mlab.triangular_mesh(self.x, self.y, self.z, self.triangles, scalars= self.scalars)
mlab.colorbar(self.triangleMesh, orientation='vertical')
mlab.outline(self.triangleMesh)
mlab.axes(self.triangleMesh, xlabel= self.axialForceLabel, ylabel= self.bendingMomentYLabel, zlabel= self.bendingMomentZLabel)
#mlab.title(self.title)
mlab.show()
# Upper right corner
ra0 = self.extent[1]; dec0 = self.extent[3]
c = SkyCoord(ra=ra0*u.degree, dec=dec0*u.degree, frame='icrs')
RA_ll = str(int(c.ra.hms.h))+'h'+str(int(c.ra.hms.m))+'m'+str(round(c.ra.hms.s,1))+'s'
mlab.text3d(self.xrang,-10,self.zrang+5,RA_ll,scale=fontsize,orient_to_camera=True,color=tcolor)
DEC_ll = str(int(c.dec.dms.d))+'d'+str(int(abs(c.dec.dms.m)))+'m'+str(round(abs(c.dec.dms.s),1))+'s'
mlab.text3d(-40,self.yrang,self.zrang+5,DEC_ll,scale=fontsize,orient_to_camera=True,color=tcolor)
# V axis
if self.extent[5] > self.extent[4]:
v0 = self.extent[4]; v1 = self.extent[5]
else:
v0 = self.extent[5]; v1 = self.extent[4]
mlab.text3d(-10,-10,self.zrang,str(round(v0,1)),scale=fontsize,orient_to_camera=True,color=tcolor)
mlab.text3d(-10,-10,0,str(round(v1,1)),scale=fontsize,orient_to_camera=True,color=tcolor)
mlab.axes(self.field, ranges=self.extent, x_axis_visibility=False, y_axis_visibility=False, z_axis_visibility=False)
mlab.outline()
T_grasp_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_grasp_gripper), from_frame='gripper', to_frame='grasp')
T_mesh_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/baxter/T_mesh_gripper.stf')
T_grasp_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/baxter/T_grasp_gripper.stf')
gripper_params = {}
gripper_params['min_width'] = 0.026
gripper_params['max_width'] = 0.060
f = open('/home/jmahler/jeff_working/GPIS/data/grippers/baxter/params.json', 'w')
json.dump(gripper_params, f)
MayaviVisualizer.plot_pose(T_mesh_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005)
MayaviVisualizer.plot_pose(T_gripper_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005)
MayaviVisualizer.plot_mesh(gripper_mesh, T_mesh_world, style='surface', color=(1,1,1))
mv.axes()
mv.show()
if __name__ == '__main__':
def draw_3d(grid_x, grid_y, fval, title='pi'):
mlab.figure()
mlab.surf(grid_x, grid_y, fval)#, warp_scale="auto")
mlab.axes(xlabel='x', ylabel='z', zlabel=title)
mlab.orientation_axes(xlabel='x', ylabel='z', zlabel=title)
mlab.title(title)