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