def make_cam_from_params(self, params):
if 1:
t = params[:3]
rod = params[3:]
rmat = rodrigues2matrix( rod )
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = rmat
cam_model = CameraModel.from_dict(d)
return cam_model
C = params[:3]
quat = params[3:]
qmag = np.sqrt(np.sum(quat**2))
quat = quat/qmag
R,rquat=get_rotation_matrix_and_quaternion(quat)
t = -np.dot(R, C)
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = R
cam_model = CameraModel.from_dict(d)
return cam_model
def make_cam_from_params(self, params):
if 1:
t = params[:3]
rod = params[3:]
rmat = rodrigues2matrix(rod)
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = rmat
cam_model = CameraModel.from_dict(d)
return cam_model
C = params[:3]
quat = params[3:]
qmag = np.sqrt(np.sum(quat**2))
quat = quat / qmag
R, rquat = get_rotation_matrix_and_quaternion(quat)
t = -np.dot(R, C)
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = R
cam_model = CameraModel.from_dict(d)
return cam_model
def test_rodrigues_cv():
for x in np.linspace(-np.pi,np.pi,5):
for y in np.linspace(-np.pi,np.pi,5):
for z in np.linspace(-np.pi,np.pi,5):
rvec = np.array((x,y,z))
rmat = cvnumpy.rodrigues2matrix(rvec)
rmat_cv = cvnumpy.rodrigues2matrix_cv(rvec)
assert np.allclose(rmat, rmat_cv)
def fit_extrinsics(base_cam,X3d,x2d,geom=None):
assert x2d.ndim==2
assert x2d.shape[1]==2
assert X3d.ndim==2
assert X3d.shape[1]==3
if 0:
fname = 'x2d_'+base_cam.name + '.png'
fname = fname.replace('/','-')
save_point_image(fname, (base_cam.width, base_cam.height), x2d )
#print 'saved pt debug image to',fname
ipts = np.array(x2d,dtype=np.float64)
opts = np.array(X3d,dtype=np.float64)
K = np.array(base_cam.get_K(), dtype=np.float64)
dist_coeffs = np.array( base_cam.get_D(), dtype=np.float64)
retval, rvec, tvec = cv2.solvePnP( opts, ipts,
K,
dist_coeffs)
assert retval
# we get two possible cameras back, figure out which one has objects in front
rmata = rodrigues2matrix( rvec )
intrinsics = base_cam.to_dict()
del intrinsics['Q']
del intrinsics['translation']
del intrinsics['name']
d = intrinsics.copy()
d['translation'] = tvec
d['Q'] = rmata
d['name'] = base_cam.name
cam_model_a = CameraModel.from_dict(d)
mza = np.mean(cam_model_a.project_3d_to_camera_frame(X3d)[:,2])
# don't bother with second - it does not have a valid rotation matrix
if 1:
founda = cam_model_a.project_3d_to_pixel(X3d)
erra = np.mean(np.sqrt(np.sum((founda-x2d)**2, axis=1)))
cam_model = cam_model_a
if 1:
found = cam_model.project_3d_to_pixel(X3d)
orig = x2d
reproj_error = np.sqrt(np.sum((found-orig)**2, axis=1))
cum = np.mean(reproj_error)
mean_cam_z = np.mean(cam_model.project_3d_to_camera_frame(X3d)[:,2])
if (mean_cam_z < 0 or cum > 20) and 0:
# hmm, we have a flipped view of the camera.
print '-'*80,'HACK ON'
center, lookat, up = cam_model.get_view()
#cam2 = cam_model.get_view_camera( -center, lookat, -up )
cam2 = cam_model.get_view_camera( center, lookat, up )
cam2.name=base_cam.name
return fit_extrinsics_iterative(cam2,X3d,x2d, geom=geom)
result = dict(cam=cam_model,
mean_err=cum,
mean_cam_z = mean_cam_z,
)
return result
def fit_extrinsics(base_cam, X3d, x2d, geom=None):
assert x2d.ndim == 2
assert x2d.shape[1] == 2
assert X3d.ndim == 2
assert X3d.shape[1] == 3
if 0:
fname = 'x2d_' + base_cam.name + '.png'
fname = fname.replace('/', '-')
save_point_image(fname, (base_cam.width, base_cam.height), x2d)
#print 'saved pt debug image to',fname
ipts = np.array(x2d, dtype=np.float64)
opts = np.array(X3d, dtype=np.float64)
K = np.array(base_cam.get_K(), dtype=np.float64)
dist_coeffs = np.array(base_cam.get_D(), dtype=np.float64)
retval, rvec, tvec = cv2.solvePnP(opts, ipts, K, dist_coeffs)
assert retval
# we get two possible cameras back, figure out which one has objects in front
rmata = rodrigues2matrix(rvec)
intrinsics = base_cam.to_dict()
del intrinsics['Q']
del intrinsics['translation']
del intrinsics['name']
d = intrinsics.copy()
d['translation'] = tvec
d['Q'] = rmata
d['name'] = base_cam.name
cam_model_a = CameraModel.from_dict(d)
mza = np.mean(cam_model_a.project_3d_to_camera_frame(X3d)[:, 2])
# don't bother with second - it does not have a valid rotation matrix
if 1:
founda = cam_model_a.project_3d_to_pixel(X3d)
erra = np.mean(np.sqrt(np.sum((founda - x2d)**2, axis=1)))
cam_model = cam_model_a
if 1:
found = cam_model.project_3d_to_pixel(X3d)
orig = x2d
reproj_error = np.sqrt(np.sum((found - orig)**2, axis=1))
cum = np.mean(reproj_error)
mean_cam_z = np.mean(cam_model.project_3d_to_camera_frame(X3d)[:, 2])
if (mean_cam_z < 0 or cum > 20) and 0:
# hmm, we have a flipped view of the camera.
print '-' * 80, 'HACK ON'
center, lookat, up = cam_model.get_view()
#cam2 = cam_model.get_view_camera( -center, lookat, -up )
cam2 = cam_model.get_view_camera(center, lookat, up)
cam2.name = base_cam.name
return fit_extrinsics_iterative(cam2, X3d, x2d, geom=geom)
result = dict(
cam=cam_model,
mean_err=cum,
mean_cam_z=mean_cam_z,
)
return result
