def _log_map_impl()

in theseus/geometry/se2.py [0:0]

• 26 lines of code
• 3 McCabe index (conditional complexity)

    def _log_map_impl(self) -> torch.Tensor:
        rotation = self.rotation

        theta = rotation.log_map()
        half_theta = 0.5 * theta.view(-1)

        cosine, sine = rotation.to_cos_sin()
        cos_minus_one = cosine - 1
        halftheta_by_tan_of_halftheta = torch.zeros_like(cos_minus_one)

        # Compute halftheta_by_tan_of_halftheta when theta is not near zero
        idx_regular_vals = cos_minus_one.abs() > theseus.constants.EPS
        halftheta_by_tan_of_halftheta[idx_regular_vals] = (
            -(half_theta * sine)[idx_regular_vals] / cos_minus_one[idx_regular_vals]
        )
        # Same as above three lines but for small values
        idx_small_vals = cos_minus_one.abs() < theseus.constants.EPS
        if idx_small_vals.any():
            theta_sq_at_idx = theta[idx_small_vals] ** 2
            halftheta_by_tan_of_halftheta[idx_small_vals] = (
                -theta_sq_at_idx.view(-1) / 12 + 1
            )

        v_inv = torch.empty(self.shape[0], 2, 2).to(
            device=self.device, dtype=self.dtype
        )
        v_inv[:, 0, 0] = halftheta_by_tan_of_halftheta
        v_inv[:, 0, 1] = half_theta
        v_inv[:, 1, 0] = -half_theta
        v_inv[:, 1, 1] = halftheta_by_tan_of_halftheta
        tangent_translation = torch.matmul(v_inv, self[:, :2].unsqueeze(-1))

        return torch.cat([tangent_translation.view(-1, 2), theta], dim=1)