def evaluate(self, params, compute_jacobians=None):
T_cam_w = params[0]
pt_w = params[1]
pt_cam = T_cam_w.dot(pt_w)
if compute_jacobians:
jacobians = [None for _ in enumerate(params)]
predicted_obs, cam_jacobian = self.camera.project(
pt_cam, compute_jacobians=True)
residual = np.dot(self.stiffness, predicted_obs - self.obs)
if compute_jacobians[0]:
jacobians[0] = np.dot(self.stiffness,
cam_jacobian.dot(SE3.odot(pt_cam)))
if compute_jacobians[1]:
jacobians[1] = np.dot(
self.stiffness, cam_jacobian.dot(T_cam_w.rot.as_matrix()))
return residual, jacobians
residual = np.dot(self.stiffness,
self.camera.project(pt_cam) - self.obs)
return residual
def evaluate(self, params, compute_jacobians=None):
""" This is my docstring. """
T_2_1 = params[0]
pt_2 = T_2_1.dot(self.pt_1)
if compute_jacobians:
jacobians = [None for _ in enumerate(params)]
obs_2_pred, cam_jacobian = self.camera.project(
pt_2, compute_jacobians=True)
residual = np.dot(self.stiffness, obs_2_pred - self.obs_2)
if compute_jacobians[0]:
jacobians[0] = np.dot(self.stiffness,
cam_jacobian.dot(SE3.odot(pt_2)))
return residual, jacobians
residual = np.dot(self.stiffness,
self.camera.project(pt_2) - self.obs_2)
return residual
def evaluate(self, params, compute_jacobians=None):
""" This is my docstring. """
T_cam2_cam1 = params[0]
pt_cam1 = self.camera.triangulate(self.obs_1)
pt_cam2 = T_cam2_cam1.dot(pt_cam1)
if compute_jacobians:
jacobians = [None for _ in enumerate(params)]
predicted_obs, cam_jacobian = self.camera.project(
pt_cam2, compute_jacobians=True)
residual = np.dot(self.stiffness, predicted_obs - self.obs_2)
if compute_jacobians[0]:
jacobians[0] = np.dot(self.stiffness,
cam_jacobian.dot(SE3.odot(pt_cam2)))
return residual, jacobians
residual = np.dot(self.stiffness,
self.camera.project(pt_cam2) - self.obs_2)
return residual
