def forward(self):
self._pi, self.pc = [], []
self.err = np.zeros((len(self.pw) * 2, ))
for k, p in enumerate(self.pw):
self.pc.append(
Quarternion.rotate_with_quaternion(self.quat, p.p) + self.t)
uv = np.matmul(self.K, self.pc[k])
uv /= uv[2]
self._pi.append(uv[0:2]) # reprojected coordinate
self.err[k * 2:k * 2 + 2] = self._pi[k] - self.pi[k]
self.residual = np.matmul(self.err, self.err) / len(self.pw)
return self.residual
def __setattr__(self, key, value):
if key == 'fx':
self.__dict__['K'][0, 0] = value
elif key == 'fy':
self.__dict__['K'][1, 1] = value
elif key == 'img_w':
self.__dict__['K'][0, 2] = value / 2
elif key == 'img_h':
self.__dict__['K'][1, 2] = value / 2
elif key == 'K':
self.__dict__['fx'] = value[0, 0]
self.__dict__['fy'] = value[1, 1]
self.__dict__['img_w'] = value[0, 2] * 2
self.__dict__['img_h'] = value[1, 2] * 2
elif key == 'q':
self.__dict__['q'] = Quarternion(value)
self.__dict__['R'] = Quarternion.quaternion_to_mat(value)
elif key == 'R':
self.__dict__['R'] = value
self.__dict__['q'] = Quarternion.mat_to_quaternion(value)
elif key not in ['f', 't']:
raise AttributeError
self.__dict__[key] = value
self.__dict__['K_'] = np.linalg.pinv(self.K)
def set_variables(self, var):
k = 0
for frm_idx in self.window:
frm = self.global_map.frames[frm_idx]
assert frm.status is True, "Frame %d pose is unknown!" % frm.frm_idx
frm.cam.q = Quarternion(var[k:k + 4])
frm.cam.t = var[k + 4:k + 7]
# frm.cam.fx = var[k + 7]
# frm.cam.fy = var[k + 8]
k += 7
for pt_idx in self.pw_index:
p = self.global_map.pw[pt_idx]
assert p is not None, "Point3D %d is unknown" % pt_idx
p.p = var[k:k + 3]
k += 3
def calc_jacobian_mat(self):
landmark_idx = 0
self.indptr = []
self.jc_data, self.indices_c = [], []
self.jp_data, self.indices_p = [], []
for i, p_idx in enumerate(self.local_map.pw_index):
pt = self.global_map.pw[p_idx]
for frm_idx in self.global_map.viewed_frames[p_idx]:
j = self.window_index.get(frm_idx, None)
if j is None:
continue
frm = self.global_map.frames[frm_idx]
q = Quarternion.mat_to_quaternion(frm.cam.R)
fu, fv, t = frm.cam.fx, frm.cam.fy, frm.cam.t
jpose = derr_over_dcam(q, t, fu, fv, pt)
# jfuv = derr_over_df(q, t, pt)
# self.jc_data.append(np.column_stack((jpose, jfuv)))
self.jc_data.append(jpose)
self.indices_c.append(j)
jpt = derr_over_dpw(q, t, fu, fv, pt)
self.jp_data.append(jpt)
self.indices_p.append(i)
self.indptr.append(landmark_idx)
landmark_idx += 1
self.indptr.append(landmark_idx)
M = landmark_idx * 2
Nc = self.fixed_frm_num * self.cam_block_size[1]
Np = self.fixed_pt_num * self.point_block_size[1]
self.jc = sparse.bsr_matrix(
(np.asarray(self.jc_data), np.asarray(
self.indices_c), np.asarray(self.indptr)),
shape=(M, Nc),
blocksize=self.cam_block_size)
self.jp = sparse.bsr_matrix(
(np.asarray(self.jp_data), np.asarray(
self.indices_p), np.asarray(self.indptr)),
shape=(M, Np),
blocksize=self.point_block_size)
self.j_sparse = sparse.hstack((self.jc, self.jp)).tolil()
return self.j_sparse
def __init__(self, **kwargs):
# intrinsic params
f, fx, fy, img_w, img_h = [1.0, 500, 500, 1920, 1080]
if 'R' in kwargs:
R = kwargs['R']
assert R.shape == (3, 3), "rotation mat should be 3x3 matrix"
else:
R = np.eye(3)
if 't' in kwargs:
t = kwargs['t']
else:
t = np.zeros((3, ))
if 'f' in kwargs:
f = kwargs['f']
if 'fx' in kwargs:
fx = kwargs['fx']
if 'fy' in kwargs:
fy = kwargs['fy']
if 'img_w' in kwargs:
img_w = kwargs['img_w']
if 'img_h' in kwargs:
img_h = kwargs['img_h']
if 'K' in kwargs:
K = kwargs['K']
fx = K[0, 0]
fy = K[1, 1]
img_w = K[0, 2] * 2
img_h = K[1, 2] * 2
self.__dict__['R'] = R
self.__dict__['t'] = t
self.__dict__['q'] = Quarternion.mat_to_quaternion(R)
self.__dict__['f'] = f
self.__dict__['fx'] = fx
self.__dict__['fy'] = fy
self.__dict__['img_w'] = img_w
self.__dict__['img_h'] = img_h
self.__dict__['K'] = np.array([[fx, 0.0, img_w / 2],
[0.0, fy, img_h / 2], [0.0, 0.0, 1.0]])
self.__dict__['K_'] = np.linalg.pinv(self.K)
def derr_over_dpc(q, t, fu, fv, pw):
pc = Quarternion.rotate_with_quaternion(q, pw.p) + t
return np.array([[fu, 0, -fu * pc[0] / pc[2]],
[0, fv, -fv * pc[1] / pc[2]]]) / pc[2]
def derr_over_dpw(q, t, fu, fv, pw):
dedpc = derr_over_dpc(q, t, fu, fv, pw)
dpcdpw = Quarternion.quaternion_to_mat(q)
return np.matmul(dedpc, dpcdpw)
def derr_over_df(q, t, pw):
pc = Quarternion.rotate_with_quaternion(q, pw.p) + t
return np.diag([pc[0] / pc[2], pc[1] / pc[2]])