def plot_setup(geometry_filename, display_bagfiles):
geom = simple_geom.Geometry(geometry_filename)
displays = [
pymvg.CameraModel.load_camera_from_bagfile(dbf)
for dbf in display_bagfiles
]
print geometry_filename
print display_bagfiles
print[d.get_name() for d in displays]
display = displays[0]
tcs = np.zeros((display.height, display.width, 2)) - 1
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
verts = get_3d_verts(geom)
ax.plot(verts[:, 0], verts[:, 1], verts[:, 2], 'ko')
for display in displays:
plot_camera(ax, display)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
if 1:
fig = plt.figure()
n = len(displays)
for i in range(n):
display = displays[i]
ax = fig.add_subplot(n, 1, i + 1)
pts = display.project_3d_to_pixel(verts)
ax.plot(pts[:, 0], pts[:, 1], 'ko')
ax.set_xlim([0, display.width])
ax.set_ylim([0, display.height])
ax.set_title(display.name)
if 1:
plt.show()
def err(self, params):
camera = self.make_cam_from_params( params)
found = camera.project_3d_to_pixel(self.X3d)
each_err = np.sqrt(np.sum((found - self.x2d)**2,axis=1))
me = np.mean(each_err)
if 0:
print
print 'params', params
print 'found'
print np.hstack( (found, self.x2d, each_err[:,np.newaxis]) )
print 'mean reproj error: ',me
print
if PLOT and self.debug:
assert len(each_err)==len(self.x2d)
self.ax3d.cla()
verts = self.plot_verts
self.ax3d.plot( verts[:,0], verts[:,1], verts[:,2], 'ko' )
plot_camera( self.ax3d, camera )
self.ax2d.cla()
self.ax2d.plot( self.x2d[:,0], self.x2d[:,1], 'go', mfc='none')
self.ax2d.plot( found[:,0], found[:,1], 'rx', mfc='none')
for i in range( len(found)):
self.ax2d.plot( [found[i,0],self.x2d[i,0]],
[found[i,1],self.x2d[i,1]], 'k-' )
plt.draw()
if 0:
df = found[1:]-found[:-1]
#print 'found'
#print found
#print 'df'
#print df
bunching_penalty = 1.0/np.sum(df**2)
#print 'mean reproj error: % 20.1f bunching penalty: % 20.1f '%(me,bunching_penalty)
#return me + bunching_penalty
return me
def err(self, params):
camera = self.make_cam_from_params( params)
found = camera.project_3d_to_pixel(self.X3d)
each_err = np.sqrt(np.sum((found - self.x2d)**2,axis=1))
me = np.mean(each_err)
if 0:
print
print 'params', params
print 'found'
print np.hstack( (found, self.x2d, each_err[:,np.newaxis]) )
print 'mean reproj error: ',me
print
if PLOT and self.debug:
assert len(each_err)==len(self.x2d)
self.ax3d.cla()
verts = self.plot_verts
self.ax3d.plot( verts[:,0], verts[:,1], verts[:,2], 'ko' )
plot_camera( self.ax3d, camera )
self.ax2d.cla()
self.ax2d.plot( self.x2d[:,0], self.x2d[:,1], 'go', mfc='none')
self.ax2d.plot( found[:,0], found[:,1], 'rx', mfc='none')
for i in range( len(found)):
self.ax2d.plot( [found[i,0],self.x2d[i,0]],
[found[i,1],self.x2d[i,1]], 'k-' )
plt.draw()
if 0:
df = found[1:]-found[:-1]
#print 'found'
#print found
#print 'df'
#print df
bunching_penalty = 1.0/np.sum(df**2)
#print 'mean reproj error: % 20.1f bunching penalty: % 20.1f '%(me,bunching_penalty)
#return me + bunching_penalty
return me
print geometry_filename
print display_bagfiles
print [d.get_name() for d in displays]
display = displays[0]
tcs = np.zeros( (display.height,display.width,2))-1
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
verts = get_3d_verts(geom)
ax.plot( verts[:,0], verts[:,1], verts[:,2], 'ko' )
for display in displays:
plot_camera(ax, display)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
if 1:
fig = plt.figure()
n = len(displays)
for i in range(n):
display = displays[i]
ax = fig.add_subplot(n,1,i+1)
pts = display.project_3d_to_pixel(verts)
ax.plot(pts[:,0], pts[:,1], 'ko')
ax.set_xlim([0,display.width])
ax.set_ylim([0,display.height])
