def jacobians()

in theseus/embodied/measurements/moving_frame_between.py [0:0]

• 16 lines of code
• 1 McCabe index (conditional complexity)

    def jacobians(self) -> Tuple[List[torch.Tensor], torch.Tensor]:
        jacobians_b1: List[torch.Tensor] = []
        pose1__frame = between(self.frame1, self.pose1, jacobians=jacobians_b1)
        jacobians_b2: List[torch.Tensor] = []
        pose2__frame = between(self.frame2, self.pose2, jacobians=jacobians_b2)
        jacobians_b_out: List[torch.Tensor] = []
        var_diff = between(pose1__frame, pose2__frame, jacobians=jacobians_b_out)
        error = self.measurement.local(var_diff)

        JB1_f1, JB1_p1 = jacobians_b1
        JB2_f2, JB2_p2 = jacobians_b2
        J_Bout_B1, J_Bout_B2 = jacobians_b_out
        J_out_f1 = torch.matmul(J_Bout_B1, JB1_f1)
        J_out_p1 = torch.matmul(J_Bout_B1, JB1_p1)
        J_out_f2 = torch.matmul(J_Bout_B2, JB2_f2)
        J_out_p2 = torch.matmul(J_Bout_B2, JB2_p2)

        return [J_out_f1, J_out_f2, J_out_p1, J_out_p2], error