essential_matrix_estimator_nister.py

import torch
import numpy as np
from math_utils import *
from utils import *
from cv_utils import *
try:
    from pymagsac import optimizeEssentialMatrix
    pymagsac_available = 1

    def numerical_optimization(matches, weights, K1, K2, inlier_indices, best_model, unnormalzied_threshold, best_score):
        # bundle adjustment
        estimated_models, _ = optimizeEssentialMatrix(
            matches.squeeze().cpu().detach().numpy(),
            K1,
            K2,
            inlier_indices,
            best_model,
            unnormalzied_threshold,
            float(best_score)
        )
        # normalize the models
        estimated_models = torch.from_numpy(estimated_models).to(matches.device, matches.dtype).unsqueeze(0)/torch.norm(torch.from_numpy(estimated_models).to(matches.device, matches.dtype).unsqueeze(0), dim=(1,2))

        return estimated_models

except ImportError:
    pymagsac_available = 0


class EssentialMatrixEstimatorNister(object):

    def __init__(self, device='cuda'):
        self.sample_size = 5
        self.device = device

    def estimate_model(
            self,
            matches,
            weights=None,
            K1=None,
            K2=None,
            inlier_indices=None,
            best_model=None,
            unnormalzied_threshold=None,
            best_score=0):

        # minimal solver
        if matches.shape[1] == self.sample_size:
            return self.estimate_minimal_model(matches, weights)

        # non-minial solver with numerical optimization in pymagsac c++
        elif matches.shape[1] > self.sample_size:
            if pymagsac_available:
                return numerical_optimization(
                    matches,
                    weights,
                    K1,
                    K2,
                    inlier_indices,
                    best_model,
                    unnormalzied_threshold,
                    best_score
                )
            else:
                return self.estimate_minimal_model(matches, weights)

        return None

    def estimate_minimal_model(self, pts, weights=None):  # x1 y1 x2 y2
        """Using Nister's 5 PC to estimate Essential matrix."""
        try:
            pts.shape[1] == self.sample_size
        except ValueError:
            print("This is not a minimal sample.")

        batch_size, num, _ = pts.shape
        pts1 = pts[:, :, 0:2]
        pts2 = pts[:, :, 2:4]

        # get the points
        x1, y1 = pts1[:, :, 0], pts1[:, :, 1]
        x2, y2 = pts2[:, :, 0], pts2[:, :, 1]

        # Step1: construct the A matrix, A F = 0.
        # 5 equations for 9 variables, A is 5x9 matrix containing epipolar constraints
        # Essential matrix is a linear combination of the 4 vectors spanning the null space of A
        a_59 = torch.ones_like(x1)#.shape)
        if weights is not None:
            A_s = weights.unsqueeze(-1) * torch.stack(
                (
                    torch.mul(x1, x2),
                    torch.mul(x1, y2),
                    x1,
                    torch.mul(y1, x2),
                    torch.mul(y1, y2),
                    y1,
                    x2,
                    y2,
                    a_59
                ),
                dim=-1)
        else:
            A_s = torch.stack(
                (
                    torch.mul(x1, x2),
                    torch.mul(x1, y2),
                    x1,
                    torch.mul(y1, x2),
                    torch.mul(y1, y2),
                    y1,
                    x2,
                    y2,
                    a_59
                ), dim=-1
            )

        _, _, v = torch.linalg.svd(A_s.transpose(-1, -2)@A_s)#.transpose(-1, -2)@A_s)# eigenvalues in increasing order
        null_ = v[:, -4:, :].transpose(-1, -2) # the last four rows
        nullSpace = v[:, -4:, :]

        coeffs = torch.zeros(batch_size, 10, 20, device=null_.device, dtype=null_.dtype)
        d = torch.zeros(batch_size, 60, device=null_.device, dtype=null_.dtype)
        fun = lambda i, j : null_[:, 3 * j + i]

        # Determinant constraint
        coeffs[:, 9] = self.o2(self.o1(fun(0, 1), fun(1, 2)) - self.o1(fun(0, 2), fun(1, 1)), fun(2, 0)) +\
            self.o2(self.o1(fun(0, 2), fun(1, 0)) - self.o1(fun(0, 0), fun(1, 2)), fun(2, 1)) +\
            self.o2(self.o1(fun(0, 0), fun(1, 1)) - self.o1(fun(0, 1), fun(1, 0)), fun(2, 2))

        indices = torch.tensor([[0, 10, 20], [10, 40, 30], [20, 30, 50]])

        # Compute EE^T (equation 20 in paper)
        for i in range(3):
            for j in range(3):
                d[:, indices[i, j]: indices[i, j] + 10] = self.o1(fun(i, 0), fun(j, 0)) + \
                                                       self.o1(fun(i, 1), fun(j, 1)) + \
                                                       self.o1(fun(i, 2), fun(j, 2))

        for i in range(10):
            t = 0.5 * (d[:, indices[0, 0] + i] + d[:, indices[1, 1] + i] + d[:, indices[2, 2] + i])
            d[:, indices[0, 0] + i] -= t
            d[:, indices[1, 1] + i] -= t
            d[:, indices[2, 2] + i] -= t

        cnt = 0
        for i in range(3):
            for j in range(3):
                row = self.o2(d[:, indices[i, 0]: indices[i, 0] + 10], fun(0, j)) + \
                      self.o2(d[:, indices[i, 1]: indices[i, 1] + 10], fun(1, j)) + \
                      self.o2(d[:, indices[i, 2]: indices[i, 2] + 10], fun(2, j))
                coeffs[:, cnt] = row
                cnt += 1

        b = coeffs[:, :, 10:]
        singular_filter = torch.linalg.matrix_rank(coeffs[:, :, :10]) >= torch.max(
            torch.linalg.matrix_rank(coeffs),
            torch.ones_like(torch.linalg.matrix_rank(coeffs[:, :, :10]))*10)
        try:
            eliminated_mat = torch.linalg.solve(coeffs[singular_filter, :, :10], b[singular_filter])
        except Exception as e:
            print(e)

        coeffs_ = torch.concat((coeffs[singular_filter, :, :10], eliminated_mat), dim=-1)

        A = torch.zeros(coeffs_.shape[0], 3, 13, device=coeffs_.device, dtype=coeffs_.dtype)

        for i in range(3):
            A[:, i, 0] = 0.0
            A[:, i:i + 1, 1:4] = coeffs_[:, 4 + 2 * i: 5 + 2 * i, 10:13]
            A[:, i:i + 1, 0:3] -= coeffs_[:, 5 + 2 * i: 6 + 2 * i, 10:13]
            A[:, i, 4] = 0.0
            A[:, i:i + 1, 5:8] = coeffs_[:, 4 + 2 * i: 5 + 2 * i, 13:16]
            A[:, i:i + 1, 4:7] -= coeffs_[:, 5 + 2 * i: 6 + 2 * i, 13:16]
            A[:, i, 8] = 0.0
            A[:, i:i + 1, 9:13] = coeffs_[:, 4 + 2 * i: 5 + 2 * i, 16:20]
            A[:, i:i + 1, 8:12] -= coeffs_[:, 5 + 2 * i: 6 + 2 * i, 16:20]

        cs = torch.zeros(A.shape[0], 11, device=A.device, dtype=A.dtype)
        cs[:, 0] = A[:, 0, 12] * A[:, 1, 3] * A[:, 2, 7] - A[:, 0, 12] * A[:, 1, 7] * A[:, 2, 3] - A[:, 0, 3] * A[:, 2, 7] * A[:, 1, 12] + \
               A[:, 0, 7] * A[:, 2, 3] * A[:, 1, 12] + A[:, 0, 3] * A[:, 1, 7] * A[:, 2, 12] - A[:, 0, 7] * A[:, 1, 3] * A[:, 2, 12]

        cs[:, 1] = A[:, 0, 11] * A[:, 1, 3] * A[:, 2, 7] - A[:, 0, 11] * A[:, 1, 7] * A[:, 2, 3] + A[:, 0, 12] * A[:, 1, 2] * A[:, 2, 7] + \
               A[:, 0, 12] * A[:, 1, 3] * A[:, 2, 6] - A[:, 0, 12] * A[:, 1, 6] * A[:, 2, 3] - A[:, 0, 12] * A[:, 1, 7] * A[:, 2, 2] - \
               A[:, 0, 2] * A[:, 2, 7] * A[:, 1, 12] - A[:, 0, 3] * A[:, 2, 6] * A[:, 1, 12] - A[:, 0, 3] * A[:, 2, 7] * A[:, 1, 11] + \
               A[:, 0, 6] * A[:, 2, 3] * A[:, 1, 12] + A[:, 0, 7] * A[:, 2, 2] * A[:, 1, 12] + A[:, 0, 7] * A[:, 2, 3] * A[:, 1, 11] + \
               A[:, 0, 2] * A[:, 1, 7] * A[:, 2, 12] + A[:, 0, 3] * A[:, 1, 6] * A[:, 2, 12] + A[:, 0, 3] * A[:, 1, 7] * A[:, 2, 11] - \
               A[:, 0, 6] * A[:, 1, 3] * A[:, 2, 12] - A[:, 0, 7] * A[:, 1, 2] * A[:, 2, 12] - A[:, 0, 7] * A[:, 1, 3] * A[:, 2, 11]

        cs[:, 2] = A[:, 0, 10] * A[:, 1, 3] * A[:, 2, 7] - A[:, 0, 10] * A[:, 1, 7] * A[:, 2, 3] + A[:, 0, 11] * A[:, 1, 2] * A[:, 2, 7] + \
               A[:, 0, 11] * A[:, 1, 3] * A[:, 2, 6] - A[:, 0, 11] * A[:, 1, 6] * A[:, 2, 3] - A[:, 0, 11] * A[:, 1, 7] * A[:, 2, 2] + \
               A[:, 1, 1] * A[:, 0, 12] * A[:, 2, 7] + A[:, 0, 12] * A[:, 1, 2] * A[:, 2, 6] + A[:, 0, 12] * A[:, 1, 3] * A[:, 2, 5] - \
               A[:, 0, 12] * A[:, 1, 5] * A[:, 2, 3] - A[:, 0, 12] * A[:, 1, 6] * A[:, 2, 2] - A[:, 0, 12] * A[:, 1, 7] * A[:, 2, 1] - \
               A[:, 0, 1] * A[:, 2, 7] * A[:, 1, 12] - A[:, 0, 2] * A[:, 2, 6] * A[:, 1, 12] - A[:, 0, 2] * A[:, 2, 7] * A[:, 1, 11] - \
               A[:, 0, 3] * A[:, 2, 5] * A[:, 1, 12] - A[:, 0, 3] * A[:, 2, 6] * A[:, 1, 11] - A[:, 0, 3] * A[:, 2, 7] * A[:, 1, 10] + \
               A[:, 0, 5] * A[:, 2, 3] * A[:, 1, 12] + A[:, 0, 6] * A[:, 2, 2] * A[:, 1, 12] + A[:, 0, 6] * A[:, 2, 3] * A[:, 1, 11] + \
               A[:, 0, 7] * A[:, 2, 1] * A[:, 1, 12] + A[:, 0, 7] * A[:, 2, 2] * A[:, 1, 11] + A[:, 0, 7] * A[:, 2, 3] * A[:, 1, 10] + \
               A[:, 0, 1] * A[:, 1, 7] * A[:, 2, 12] + A[:, 0, 2] * A[:, 1, 6] * A[:, 2, 12] + A[:, 0, 2] * A[:, 1, 7] * A[:, 2, 11] + \
               A[:, 0, 3] * A[:, 1, 5] * A[:, 2, 12] + A[:, 0, 3] * A[:, 1, 6] * A[:, 2, 11] + A[:, 0, 3] * A[:, 1, 7] * A[:, 2, 10] - \
               A[:, 0, 5] * A[:, 1, 3] * A[:, 2, 12] - A[:, 0, 6] * A[:, 1, 2] * A[:, 2, 12] - A[:, 0, 6] * A[:, 1, 3] * A[:, 2, 11] - \
               A[:, 0, 7] * A[:, 1, 1] * A[:, 2, 12] - A[:, 0, 7] * A[:, 1, 2] * A[:, 2, 11] - A[:, 0, 7] * A[:, 1, 3] * A[:, 2, 10]

        cs[:, 3] = A[:, 0, 3] * A[:, 1, 7] * A[:, 2, 9] - A[:, 0, 3] * A[:, 1, 9] * A[:, 2, 7] - A[:, 0, 7] * A[:, 1, 3] * A[:, 2, 9] + \
               A[:, 0, 7] * A[:, 1, 9] * A[:, 2, 3] + A[:, 0, 9] * A[:, 1, 3] * A[:, 2, 7] - A[:, 0, 9] * A[:, 1, 7] * A[:, 2, 3] + \
               A[:, 0, 10] * A[:, 1, 2] * A[:, 2, 7] + A[:, 0, 10] * A[:, 1, 3] * A[:, 2, 6] - A[:, 0, 10] * A[:, 1, 6] * A[:, 2, 3] - \
               A[:, 0, 10] * A[:, 1, 7] * A[:, 2, 2] + A[:, 1, 0] * A[:, 0, 12] * A[:, 2, 7] + A[:, 0, 11] * A[:, 1, 1] * A[:, 2, 7] + \
               A[:, 0, 11] * A[:, 1, 2] * A[:, 2, 6] + A[:, 0, 11] * A[:, 1, 3] * A[:, 2, 5] - A[:, 0, 11] * A[:, 1, 5] * A[:, 2, 3] - \
               A[:, 0, 11] * A[:, 1, 6] * A[:, 2, 2] - A[:, 0, 11] * A[:, 1, 7] * A[:, 2, 1] + A[:, 1, 1] * A[:, 0, 12] * A[:, 2, 6] + \
               A[:, 0, 12] * A[:, 1, 2] * A[:, 2, 5] + A[:, 0, 12] * A[:, 1, 3] * A[:, 2, 4] - A[:, 0, 12] * A[:, 1, 4] * A[:, 2, 3] - \
               A[:, 0, 12] * A[:, 1, 5] * A[:, 2, 2] - A[:, 0, 12] * A[:, 1, 6] * A[:, 2, 1] - A[:, 0, 12] * A[:, 1, 7] * A[:, 2, 0] - \
               A[:, 0, 0] * A[:, 2, 7] * A[:, 1, 12] - A[:, 0, 1] * A[:, 2, 6] * A[:, 1, 12] - A[:, 0, 1] * A[:, 2, 7] * A[:, 1, 11] - \
               A[:, 0, 2] * A[:, 2, 5] * A[:, 1, 12] - A[:, 0, 2] * A[:, 2, 6] * A[:, 1, 11] - A[:, 0, 2] * A[:, 2, 7] * A[:, 1, 10] - \
               A[:, 0, 3] * A[:, 2, 4] * A[:, 1, 12] - A[:, 0, 3] * A[:, 2, 5] * A[:, 1, 11] - A[:, 0, 3] * A[:, 2, 6] * A[:, 1, 10] + \
               A[:, 0, 4] * A[:, 2, 3] * A[:, 1, 12] + A[:, 0, 5] * A[:, 2, 2] * A[:, 1, 12] + A[:, 0, 5] * A[:, 2, 3] * A[:, 1, 11] + \
               A[:, 0, 6] * A[:, 2, 1] * A[:, 1, 12] + A[:, 0, 6] * A[:, 2, 2] * A[:, 1, 11] + A[:, 0, 6] * A[:, 2, 3] * A[:, 1, 10] + \
               A[:, 0, 7] * A[:, 2, 0] * A[:, 1, 12] + A[:, 0, 7] * A[:, 2, 1] * A[:, 1, 11] + A[:, 0, 7] * A[:, 2, 2] * A[:, 1, 10] + \
               A[:, 0, 0] * A[:, 1, 7] * A[:, 2, 12] + A[:, 0, 1] * A[:, 1, 6] * A[:, 2, 12] + A[:, 0, 1] * A[:, 1, 7] * A[:, 2, 11] + \
               A[:, 0, 2] * A[:, 1, 5] * A[:, 2, 12] + A[:, 0, 2] * A[:, 1, 6] * A[:, 2, 11] + A[:, 0, 2] * A[:, 1, 7] * A[:, 2, 10] + \
               A[:, 0, 3] * A[:, 1, 4] * A[:, 2, 12] + A[:, 0, 3] * A[:, 1, 5] * A[:, 2, 11] + A[:, 0, 3] * A[:, 1, 6] * A[:, 2, 10] - \
               A[:, 0, 4] * A[:, 1, 3] * A[:, 2, 12] - A[:, 0, 5] * A[:, 1, 2] * A[:, 2, 12] - A[:, 0, 5] * A[:, 1, 3] * A[:, 2, 11] - \
               A[:, 0, 6] * A[:, 1, 1] * A[:, 2, 12] - A[:, 0, 6] * A[:, 1, 2] * A[:, 2, 11] - A[:, 0, 6] * A[:, 1, 3] * A[:, 2, 10] - \
               A[:, 0, 7] * A[:, 1, 0] * A[:, 2, 12] - A[:, 0, 7] * A[:, 1, 1] * A[:, 2, 11] - A[:, 0, 7] * A[:, 1, 2] * A[:, 2, 10]

        cs[:, 4] = A[:, 0, 2] * A[:, 1, 7] * A[:, 2, 9] - A[:, 0, 2] * A[:, 1, 9] * A[:, 2, 7] + A[:, 0, 3] * A[:, 1, 6] * A[:, 2, 9] + \
               A[:, 0, 3] * A[:, 1, 7] * A[:, 2, 8] - A[:, 0, 3] * A[:, 1, 8] * A[:, 2, 7] - A[:, 0, 3] * A[:, 1, 9] * A[:, 2, 6] - \
               A[:, 0, 6] * A[:, 1, 3] * A[:, 2, 9] + A[:, 0, 6] * A[:, 1, 9] * A[:, 2, 3] - A[:, 0, 7] * A[:, 1, 2] * A[:, 2, 9] - \
               A[:, 0, 7] * A[:, 1, 3] * A[:, 2, 8] + A[:, 0, 7] * A[:, 1, 8] * A[:, 2, 3] + A[:, 0, 7] * A[:, 1, 9] * A[:, 2, 2] + \
               A[:, 0, 8] * A[:, 1, 3] * A[:, 2, 7] - A[:, 0, 8] * A[:, 1, 7] * A[:, 2, 3] + A[:, 0, 9] * A[:, 1, 2] * A[:, 2, 7] + \
               A[:, 0, 9] * A[:, 1, 3] * A[:, 2, 6] - A[:, 0, 9] * A[:, 1, 6] * A[:, 2, 3] - A[:, 0, 9] * A[:, 1, 7] * A[:, 2, 2] + \
               A[:, 0, 10] * A[:, 1, 1] * A[:, 2, 7] + A[:, 0, 10] * A[:, 1, 2] * A[:, 2, 6] + A[:, 0, 10] * A[:, 1, 3] * A[:, 2, 5] - \
               A[:, 0, 10] * A[:, 1, 5] * A[:, 2, 3] - A[:, 0, 10] * A[:, 1, 6] * A[:, 2, 2] - A[:, 0, 10] * A[:, 1, 7] * A[:, 2, 1] + \
               A[:, 1, 0] * A[:, 0, 11] * A[:, 2, 7] + A[:, 1, 0] * A[:, 0, 12] * A[:, 2, 6] + A[:, 0, 11] * A[:, 1, 1] * A[:, 2, 6] + \
               A[:, 0, 11] * A[:, 1, 2] * A[:, 2, 5] + A[:, 0, 11] * A[:, 1, 3] * A[:, 2, 4] - A[:, 0, 11] * A[:, 1, 4] * A[:, 2, 3] - \
               A[:, 0, 11] * A[:, 1, 5] * A[:, 2, 2] - A[:, 0, 11] * A[:, 1, 6] * A[:, 2, 1] - A[:, 0, 11] * A[:, 1, 7] * A[:, 2, 0] + \
               A[:, 1, 1] * A[:, 0, 12] * A[:, 2, 5] + A[:, 0, 12] * A[:, 1, 2] * A[:, 2, 4] - A[:, 0, 12] * A[:, 1, 4] * A[:, 2, 2] - \
               A[:, 0, 12] * A[:, 1, 5] * A[:, 2, 1] - A[:, 0, 12] * A[:, 1, 6] * A[:, 2, 0] - A[:, 0, 0] * A[:, 2, 6] * A[:, 1, 12] - \
               A[:, 0, 0] * A[:, 2, 7] * A[:, 1, 11] - A[:, 0, 1] * A[:, 2, 5] * A[:, 1, 12] - A[:, 0, 1] * A[:, 2, 6] * A[:, 1, 11] - \
               A[:, 0, 1] * A[:, 2, 7] * A[:, 1, 10] - A[:, 0, 2] * A[:, 2, 4] * A[:, 1, 12] - A[:, 0, 2] * A[:, 2, 5] * A[:, 1, 11] - \
               A[:, 0, 2] * A[:, 2, 6] * A[:, 1, 10] - A[:, 0, 3] * A[:, 2, 4] * A[:, 1, 11] - A[:, 0, 3] * A[:, 2, 5] * A[:, 1, 10] + \
               A[:, 0, 4] * A[:, 2, 2] * A[:, 1, 12] + A[:, 0, 4] * A[:, 2, 3] * A[:, 1, 11] + A[:, 0, 5] * A[:, 2, 1] * A[:, 1, 12] + \
               A[:, 0, 5] * A[:, 2, 2] * A[:, 1, 11] + A[:, 0, 5] * A[:, 2, 3] * A[:, 1, 10] + A[:, 0, 6] * A[:, 2, 0] * A[:, 1, 12] + \
               A[:, 0, 6] * A[:, 2, 1] * A[:, 1, 11] + A[:, 0, 6] * A[:, 2, 2] * A[:, 1, 10] + A[:, 0, 7] * A[:, 2, 0] * A[:, 1, 11] + \
               A[:, 0, 7] * A[:, 2, 1] * A[:, 1, 10] + A[:, 0, 0] * A[:, 1, 6] * A[:, 2, 12] + A[:, 0, 0] * A[:, 1, 7] * A[:, 2, 11] + \
               A[:, 0, 1] * A[:, 1, 5] * A[:, 2, 12] + A[:, 0, 1] * A[:, 1, 6] * A[:, 2, 11] + A[:, 0, 1] * A[:, 1, 7] * A[:, 2, 10] + \
               A[:, 0, 2] * A[:, 1, 4] * A[:, 2, 12] + A[:, 0, 2] * A[:, 1, 5] * A[:, 2, 11] + A[:, 0, 2] * A[:, 1, 6] * A[:, 2, 10] + \
               A[:, 0, 3] * A[:, 1, 4] * A[:, 2, 11] + A[:, 0, 3] * A[:, 1, 5] * A[:, 2, 10] - A[:, 0, 4] * A[:, 1, 2] * A[:, 2, 12] - \
               A[:, 0, 4] * A[:, 1, 3] * A[:, 2, 11] - A[:, 0, 5] * A[:, 1, 1] * A[:, 2, 12] - A[:, 0, 5] * A[:, 1, 2] * A[:, 2, 11] - \
               A[:, 0, 5] * A[:, 1, 3] * A[:, 2, 10] - A[:, 0, 6] * A[:, 1, 0] * A[:, 2, 12] - A[:, 0, 6] * A[:, 1, 1] * A[:, 2, 11] - \
               A[:, 0, 6] * A[:, 1, 2] * A[:, 2, 10] - A[:, 0, 7] * A[:, 1, 0] * A[:, 2, 11] - A[:, 0, 7] * A[:, 1, 1] * A[:, 2, 10]

        cs[:, 5] = A[:, 0, 1] * A[:, 1, 7] * A[:, 2, 9] - A[:, 0, 1] * A[:, 1, 9] * A[:, 2, 7] + A[:, 0, 2] * A[:, 1, 6] * A[:, 2, 9] + \
               A[:, 0, 2] * A[:, 1, 7] * A[:, 2, 8] - A[:, 0, 2] * A[:, 1, 8] * A[:, 2, 7] - A[:, 0, 2] * A[:, 1, 9] * A[:, 2, 6] + \
               A[:, 0, 3] * A[:, 1, 5] * A[:, 2, 9] + A[:, 0, 3] * A[:, 1, 6] * A[:, 2, 8] - A[:, 0, 3] * A[:, 1, 8] * A[:, 2, 6] - \
               A[:, 0, 3] * A[:, 1, 9] * A[:, 2, 5] - A[:, 0, 5] * A[:, 1, 3] * A[:, 2, 9] + A[:, 0, 5] * A[:, 1, 9] * A[:, 2, 3] - \
               A[:, 0, 6] * A[:, 1, 2] * A[:, 2, 9] - A[:, 0, 6] * A[:, 1, 3] * A[:, 2, 8] + A[:, 0, 6] * A[:, 1, 8] * A[:, 2, 3] + \
               A[:, 0, 6] * A[:, 1, 9] * A[:, 2, 2] - A[:, 0, 7] * A[:, 1, 1] * A[:, 2, 9] - A[:, 0, 7] * A[:, 1, 2] * A[:, 2, 8] + \
               A[:, 0, 7] * A[:, 1, 8] * A[:, 2, 2] + A[:, 0, 7] * A[:, 1, 9] * A[:, 2, 1] + A[:, 0, 8] * A[:, 1, 2] * A[:, 2, 7] + \
               A[:, 0, 8] * A[:, 1, 3] * A[:, 2, 6] - A[:, 0, 8] * A[:, 1, 6] * A[:, 2, 3] - A[:, 0, 8] * A[:, 1, 7] * A[:, 2, 2] + \
               A[:, 0, 9] * A[:, 1, 1] * A[:, 2, 7] + A[:, 0, 9] * A[:, 1, 2] * A[:, 2, 6] + A[:, 0, 9] * A[:, 1, 3] * A[:, 2, 5] - \
               A[:, 0, 9] * A[:, 1, 5] * A[:, 2, 3] - A[:, 0, 9] * A[:, 1, 6] * A[:, 2, 2] - A[:, 0, 9] * A[:, 1, 7] * A[:, 2, 1] + \
               A[:, 0, 10] * A[:, 1, 0] * A[:, 2, 7] + A[:, 0, 10] * A[:, 1, 1] * A[:, 2, 6] + A[:, 0, 10] * A[:, 1, 2] * A[:, 2, 5] + \
               A[:, 0, 10] * A[:, 1, 3] * A[:, 2, 4] - A[:, 0, 10] * A[:, 1, 4] * A[:, 2, 3] - A[:, 0, 10] * A[:, 1, 5] * A[:, 2, 2] - \
               A[:, 0, 10] * A[:, 1, 6] * A[:, 2, 1] - A[:, 0, 10] * A[:, 1, 7] * A[:, 2, 0] + A[:, 1, 0] * A[:, 0, 11] * A[:, 2, 6] + \
               A[:, 1, 0] * A[:, 0, 12] * A[:, 2, 5] + A[:, 0, 11] * A[:, 1, 1] * A[:, 2, 5] + A[:, 0, 11] * A[:, 1, 2] * A[:, 2, 4] - \
               A[:, 0, 11] * A[:, 1, 4] * A[:, 2, 2] - A[:, 0, 11] * A[:, 1, 5] * A[:, 2, 1] - A[:, 0, 11] * A[:, 1, 6] * A[:, 2, 0] + \
               A[:, 1, 1] * A[:, 0, 12] * A[:, 2, 4] - A[:, 0, 12] * A[:, 1, 4] * A[:, 2, 1] - A[:, 0, 12] * A[:, 1, 5] * A[:, 2, 0] - \
               A[:, 0, 0] * A[:, 2, 5] * A[:, 1, 12] - A[:, 0, 0] * A[:, 2, 6] * A[:, 1, 11] - A[:, 0, 0] * A[:, 2, 7] * A[:, 1, 10] - \
               A[:, 0, 1] * A[:, 2, 4] * A[:, 1, 12] - A[:, 0, 1] * A[:, 2, 5] * A[:, 1, 11] - A[:, 0, 1] * A[:, 2, 6] * A[:, 1, 10] - \
               A[:, 0, 2] * A[:, 2, 4] * A[:, 1, 11] - A[:, 0, 2] * A[:, 2, 5] * A[:, 1, 10] - A[:, 0, 3] * A[:, 2, 4] * A[:, 1, 10] + \
               A[:, 0, 4] * A[:, 2, 1] * A[:, 1, 12] + A[:, 0, 4] * A[:, 2, 2] * A[:, 1, 11] + A[:, 0, 4] * A[:, 2, 3] * A[:, 1, 10] + \
               A[:, 0, 5] * A[:, 2, 0] * A[:, 1, 12] + A[:, 0, 5] * A[:, 2, 1] * A[:, 1, 11] + A[:, 0, 5] * A[:, 2, 2] * A[:, 1, 10] + \
               A[:, 0, 6] * A[:, 2, 0] * A[:, 1, 11] + A[:, 0, 6] * A[:, 2, 1] * A[:, 1, 10] + A[:, 0, 7] * A[:, 2, 0] * A[:, 1, 10] + \
               A[:, 0, 0] * A[:, 1, 5] * A[:, 2, 12] + A[:, 0, 0] * A[:, 1, 6] * A[:, 2, 11] + A[:, 0, 0] * A[:, 1, 7] * A[:, 2, 10] + \
               A[:, 0, 1] * A[:, 1, 4] * A[:, 2, 12] + A[:, 0, 1] * A[:, 1, 5] * A[:, 2, 11] + A[:, 0, 1] * A[:, 1, 6] * A[:, 2, 10] + \
               A[:, 0, 2] * A[:, 1, 4] * A[:, 2, 11] + A[:, 0, 2] * A[:, 1, 5] * A[:, 2, 10] + A[:, 0, 3] * A[:, 1, 4] * A[:, 2, 10] - \
               A[:, 0, 4] * A[:, 1, 1] * A[:, 2, 12] - A[:, 0, 4] * A[:, 1, 2] * A[:, 2, 11] - A[:, 0, 4] * A[:, 1, 3] * A[:, 2, 10] - \
               A[:, 0, 5] * A[:, 1, 0] * A[:, 2, 12] - A[:, 0, 5] * A[:, 1, 1] * A[:, 2, 11] - A[:, 0, 5] * A[:, 1, 2] * A[:, 2, 10] - \
               A[:, 0, 6] * A[:, 1, 0] * A[:, 2, 11] - A[:, 0, 6] * A[:, 1, 1] * A[:, 2, 10] - A[:, 0, 7] * A[:, 1, 0] * A[:, 2, 10]

        cs[:, 6] = A[:, 0, 0] * A[:, 1, 7] * A[:, 2, 9] - A[:, 0, 0] * A[:, 1, 9] * A[:, 2, 7] + A[:, 0, 1] * A[:, 1, 6] * A[:, 2, 9] + \
               A[:, 0, 1] * A[:, 1, 7] * A[:, 2, 8] - A[:, 0, 1] * A[:, 1, 8] * A[:, 2, 7] - A[:, 0, 1] * A[:, 1, 9] * A[:, 2, 6] + \
               A[:, 0, 2] * A[:, 1, 5] * A[:, 2, 9] + A[:, 0, 2] * A[:, 1, 6] * A[:, 2, 8] - A[:, 0, 2] * A[:, 1, 8] * A[:, 2, 6] - \
               A[:, 0, 2] * A[:, 1, 9] * A[:, 2, 5] + A[:, 0, 3] * A[:, 1, 4] * A[:, 2, 9] + A[:, 0, 3] * A[:, 1, 5] * A[:, 2, 8] - \
               A[:, 0, 3] * A[:, 1, 8] * A[:, 2, 5] - A[:, 0, 3] * A[:, 1, 9] * A[:, 2, 4] - A[:, 0, 4] * A[:, 1, 3] * A[:, 2, 9] + \
               A[:, 0, 4] * A[:, 1, 9] * A[:, 2, 3] - A[:, 0, 5] * A[:, 1, 2] * A[:, 2, 9] - A[:, 0, 5] * A[:, 1, 3] * A[:, 2, 8] + \
               A[:, 0, 5] * A[:, 1, 8] * A[:, 2, 3] + A[:, 0, 5] * A[:, 1, 9] * A[:, 2, 2] - A[:, 0, 6] * A[:, 1, 1] * A[:, 2, 9] - \
               A[:, 0, 6] * A[:, 1, 2] * A[:, 2, 8] + A[:, 0, 6] * A[:, 1, 8] * A[:, 2, 2] + A[:, 0, 6] * A[:, 1, 9] * A[:, 2, 1] - \
               A[:, 0, 7] * A[:, 1, 0] * A[:, 2, 9] - A[:, 0, 7] * A[:, 1, 1] * A[:, 2, 8] + A[:, 0, 7] * A[:, 1, 8] * A[:, 2, 1] + \
               A[:, 0, 7] * A[:, 1, 9] * A[:, 2, 0] + A[:, 0, 8] * A[:, 1, 1] * A[:, 2, 7] + A[:, 0, 8] * A[:, 1, 2] * A[:, 2, 6] + \
               A[:, 0, 8] * A[:, 1, 3] * A[:, 2, 5] - A[:, 0, 8] * A[:, 1, 5] * A[:, 2, 3] - A[:, 0, 8] * A[:, 1, 6] * A[:, 2, 2] - \
               A[:, 0, 8] * A[:, 1, 7] * A[:, 2, 1] + A[:, 0, 9] * A[:, 1, 0] * A[:, 2, 7] + A[:, 0, 9] * A[:, 1, 1] * A[:, 2, 6] + \
               A[:, 0, 9] * A[:, 1, 2] * A[:, 2, 5] + A[:, 0, 9] * A[:, 1, 3] * A[:, 2, 4] - A[:, 0, 9] * A[:, 1, 4] * A[:, 2, 3] - \
               A[:, 0, 9] * A[:, 1, 5] * A[:, 2, 2] - A[:, 0, 9] * A[:, 1, 6] * A[:, 2, 1] - A[:, 0, 9] * A[:, 1, 7] * A[:, 2, 0] + \
               A[:, 0, 10] * A[:, 1, 0] * A[:, 2, 6] + A[:, 0, 10] * A[:, 1, 1] * A[:, 2, 5] + A[:, 0, 10] * A[:, 1, 2] * A[:, 2, 4] - \
               A[:, 0, 10] * A[:, 1, 4] * A[:, 2, 2] - A[:, 0, 10] * A[:, 1, 5] * A[:, 2, 1] - A[:, 0, 10] * A[:, 1, 6] * A[:, 2, 0] + \
               A[:, 1, 0] * A[:, 0, 11] * A[:, 2, 5] + A[:, 1, 0] * A[:, 0, 12] * A[:, 2, 4] + A[:, 0, 11] * A[:, 1, 1] * A[:, 2, 4] - \
               A[:, 0, 11] * A[:, 1, 4] * A[:, 2, 1] - A[:, 0, 11] * A[:, 1, 5] * A[:, 2, 0] - A[:, 0, 12] * A[:, 1, 4] * A[:, 2, 0] - \
               A[:, 0, 0] * A[:, 2, 4] * A[:, 1, 12] - A[:, 0, 0] * A[:, 2, 5] * A[:, 1, 11] - A[:, 0, 0] * A[:, 2, 6] * A[:, 1, 10] - \
               A[:, 0, 1] * A[:, 2, 4] * A[:, 1, 11] - A[:, 0, 1] * A[:, 2, 5] * A[:, 1, 10] - A[:, 0, 2] * A[:, 2, 4] * A[:, 1, 10] + \
               A[:, 0, 4] * A[:, 2, 0] * A[:, 1, 12] + A[:, 0, 4] * A[:, 2, 1] * A[:, 1, 11] + A[:, 0, 4] * A[:, 2, 2] * A[:, 1, 10] + \
               A[:, 0, 5] * A[:, 2, 0] * A[:, 1, 11] + A[:, 0, 5] * A[:, 2, 1] * A[:, 1, 10] + A[:, 0, 6] * A[:, 2, 0] * A[:, 1, 10] + \
               A[:, 0, 0] * A[:, 1, 4] * A[:, 2, 12] + A[:, 0, 0] * A[:, 1, 5] * A[:, 2, 11] + A[:, 0, 0] * A[:, 1, 6] * A[:, 2, 10] + \
               A[:, 0, 1] * A[:, 1, 4] * A[:, 2, 11] + A[:, 0, 1] * A[:, 1, 5] * A[:, 2, 10] + A[:, 0, 2] * A[:, 1, 4] * A[:, 2, 10] - \
               A[:, 0, 4] * A[:, 1, 0] * A[:, 2, 12] - A[:, 0, 4] * A[:, 1, 1] * A[:, 2, 11] - A[:, 0, 4] * A[:, 1, 2] * A[:, 2, 10] - \
               A[:, 0, 5] * A[:, 1, 0] * A[:, 2, 11] - A[:, 0, 5] * A[:, 1, 1] * A[:, 2, 10] - A[:, 0, 6] * A[:, 1, 0] * A[:, 2, 10]

        cs[:, 7] = A[:, 0, 0] * A[:, 1, 6] * A[:, 2, 9] + A[:, 0, 0] * A[:, 1, 7] * A[:, 2, 8] - A[:, 0, 0] * A[:, 1, 8] * A[:, 2, 7] - \
               A[:, 0, 0] * A[:, 1, 9] * A[:, 2, 6] + A[:, 0, 1] * A[:, 1, 5] * A[:, 2, 9] + A[:, 0, 1] * A[:, 1, 6] * A[:, 2, 8] - \
               A[:, 0, 1] * A[:, 1, 8] * A[:, 2, 6] - A[:, 0, 1] * A[:, 1, 9] * A[:, 2, 5] + A[:, 0, 2] * A[:, 1, 4] * A[:, 2, 9] + \
               A[:, 0, 2] * A[:, 1, 5] * A[:, 2, 8] - A[:, 0, 2] * A[:, 1, 8] * A[:, 2, 5] - A[:, 0, 2] * A[:, 1, 9] * A[:, 2, 4] + \
               A[:, 0, 3] * A[:, 1, 4] * A[:, 2, 8] - A[:, 0, 3] * A[:, 1, 8] * A[:, 2, 4] - A[:, 0, 4] * A[:, 1, 2] * A[:, 2, 9] - \
               A[:, 0, 4] * A[:, 1, 3] * A[:, 2, 8] + A[:, 0, 4] * A[:, 1, 8] * A[:, 2, 3] + A[:, 0, 4] * A[:, 1, 9] * A[:, 2, 2] - \
               A[:, 0, 5] * A[:, 1, 1] * A[:, 2, 9] - A[:, 0, 5] * A[:, 1, 2] * A[:, 2, 8] + A[:, 0, 5] * A[:, 1, 8] * A[:, 2, 2] + \
               A[:, 0, 5] * A[:, 1, 9] * A[:, 2, 1] - A[:, 0, 6] * A[:, 1, 0] * A[:, 2, 9] - A[:, 0, 6] * A[:, 1, 1] * A[:, 2, 8] + \
               A[:, 0, 6] * A[:, 1, 8] * A[:, 2, 1] + A[:, 0, 6] * A[:, 1, 9] * A[:, 2, 0] - A[:, 0, 7] * A[:, 1, 0] * A[:, 2, 8] + \
               A[:, 0, 7] * A[:, 1, 8] * A[:, 2, 0] + A[:, 0, 8] * A[:, 1, 0] * A[:, 2, 7] + A[:, 0, 8] * A[:, 1, 1] * A[:, 2, 6] + \
               A[:, 0, 8] * A[:, 1, 2] * A[:, 2, 5] + A[:, 0, 8] * A[:, 1, 3] * A[:, 2, 4] - A[:, 0, 8] * A[:, 1, 4] * A[:, 2, 3] - \
               A[:, 0, 8] * A[:, 1, 5] * A[:, 2, 2] - A[:, 0, 8] * A[:, 1, 6] * A[:, 2, 1] - A[:, 0, 8] * A[:, 1, 7] * A[:, 2, 0] + \
               A[:, 0, 9] * A[:, 1, 0] * A[:, 2, 6] + A[:, 0, 9] * A[:, 1, 1] * A[:, 2, 5] + A[:, 0, 9] * A[:, 1, 2] * A[:, 2, 4] - \
               A[:, 0, 9] * A[:, 1, 4] * A[:, 2, 2] - A[:, 0, 9] * A[:, 1, 5] * A[:, 2, 1] - A[:, 0, 9] * A[:, 1, 6] * A[:, 2, 0] + \
               A[:, 0, 10] * A[:, 1, 0] * A[:, 2, 5] + A[:, 0, 10] * A[:, 1, 1] * A[:, 2, 4] - A[:, 0, 10] * A[:, 1, 4] * A[:, 2, 1] - \
               A[:, 0, 10] * A[:, 1, 5] * A[:, 2, 0] + A[:, 1, 0] * A[:, 0, 11] * A[:, 2, 4] - A[:, 0, 11] * A[:, 1, 4] * A[:, 2, 0] - \
               A[:, 0, 0] * A[:, 2, 4] * A[:, 1, 11] - A[:, 0, 0] * A[:, 2, 5] * A[:, 1, 10] - A[:, 0, 1] * A[:, 2, 4] * A[:, 1, 10] + \
               A[:, 0, 4] * A[:, 2, 0] * A[:, 1, 11] + A[:, 0, 4] * A[:, 2, 1] * A[:, 1, 10] + A[:, 0, 5] * A[:, 2, 0] * A[:, 1, 10] + \
               A[:, 0, 0] * A[:, 1, 4] * A[:, 2, 11] + A[:, 0, 0] * A[:, 1, 5] * A[:, 2, 10] + A[:, 0, 1] * A[:, 1, 4] * A[:, 2, 10] - \
               A[:, 0, 4] * A[:, 1, 0] * A[:, 2, 11] - A[:, 0, 4] * A[:, 1, 1] * A[:, 2, 10] - A[:, 0, 5] * A[:, 1, 0] * A[:, 2, 10]

        cs[:, 8] = A[:, 0, 0] * A[:, 1, 5] * A[:, 2, 9] + A[:, 0, 0] * A[:, 1, 6] * A[:, 2, 8] - A[:, 0, 0] * A[:, 1, 8] * A[:, 2, 6] - \
               A[:, 0, 0] * A[:, 1, 9] * A[:, 2, 5] + A[:, 0, 1] * A[:, 1, 4] * A[:, 2, 9] + A[:, 0, 1] * A[:, 1, 5] * A[:, 2, 8] - \
               A[:, 0, 1] * A[:, 1, 8] * A[:, 2, 5] - A[:, 0, 1] * A[:, 1, 9] * A[:, 2, 4] + A[:, 0, 2] * A[:, 1, 4] * A[:, 2, 8] - \
               A[:, 0, 2] * A[:, 1, 8] * A[:, 2, 4] - A[:, 0, 4] * A[:, 1, 1] * A[:, 2, 9] - A[:, 0, 4] * A[:, 1, 2] * A[:, 2, 8] + \
               A[:, 0, 4] * A[:, 1, 8] * A[:, 2, 2] + A[:, 0, 4] * A[:, 1, 9] * A[:, 2, 1] - A[:, 0, 5] * A[:, 1, 0] * A[:, 2, 9] - \
               A[:, 0, 5] * A[:, 1, 1] * A[:, 2, 8] + A[:, 0, 5] * A[:, 1, 8] * A[:, 2, 1] + A[:, 0, 5] * A[:, 1, 9] * A[:, 2, 0] - \
               A[:, 0, 6] * A[:, 1, 0] * A[:, 2, 8] + A[:, 0, 6] * A[:, 1, 8] * A[:, 2, 0] + A[:, 0, 8] * A[:, 1, 0] * A[:, 2, 6] + \
               A[:, 0, 8] * A[:, 1, 1] * A[:, 2, 5] + A[:, 0, 8] * A[:, 1, 2] * A[:, 2, 4] - A[:, 0, 8] * A[:, 1, 4] * A[:, 2, 2] - \
               A[:, 0, 8] * A[:, 1, 5] * A[:, 2, 1] - A[:, 0, 8] * A[:, 1, 6] * A[:, 2, 0] + A[:, 0, 9] * A[:, 1, 0] * A[:, 2, 5] + \
               A[:, 0, 9] * A[:, 1, 1] * A[:, 2, 4] - A[:, 0, 9] * A[:, 1, 4] * A[:, 2, 1] - A[:, 0, 9] * A[:, 1, 5] * A[:, 2, 0] + \
               A[:, 0, 10] * A[:, 1, 0] * A[:, 2, 4] - A[:, 0, 10] * A[:, 1, 4] * A[:, 2, 0] - A[:, 0, 0] * A[:, 2, 4] * A[:, 1, 10] + \
               A[:, 0, 4] * A[:, 2, 0] * A[:, 1, 10] + A[:, 0, 0] * A[:, 1, 4] * A[:, 2, 10] - A[:, 0, 4] * A[:, 1, 0] * A[:, 2, 10]

        cs[:, 9] = A[:, 0, 0] * A[:, 1, 4] * A[:, 2, 9] + A[:, 0, 0] * A[:, 1, 5] * A[:, 2, 8] - A[:, 0, 0] * A[:, 1, 8] * A[:, 2, 5] - \
               A[:, 0, 0] * A[:, 1, 9] * A[:, 2, 4] + A[:, 0, 1] * A[:, 1, 4] * A[:, 2, 8] - A[:, 0, 1] * A[:, 1, 8] * A[:, 2, 4] - \
               A[:, 0, 4] * A[:, 1, 0] * A[:, 2, 9] - A[:, 0, 4] * A[:, 1, 1] * A[:, 2, 8] + A[:, 0, 4] * A[:, 1, 8] * A[:, 2, 1] + \
               A[:, 0, 4] * A[:, 1, 9] * A[:, 2, 0] - A[:, 0, 5] * A[:, 1, 0] * A[:, 2, 8] + A[:, 0, 5] * A[:, 1, 8] * A[:, 2, 0] + \
               A[:, 0, 8] * A[:, 1, 0] * A[:, 2, 5] + A[:, 0, 8] * A[:, 1, 1] * A[:, 2, 4] - A[:, 0, 8] * A[:, 1, 4] * A[:, 2, 1] - \
               A[:, 0, 8] * A[:, 1, 5] * A[:, 2, 0] + A[:, 0, 9] * A[:, 1, 0] * A[:, 2, 4] - A[:, 0, 9] * A[:, 1, 4] * A[:, 2, 0]

        cs[:, 10] = A[:, 0, 0] * A[:, 1, 4] * A[:, 2, 8] - A[:, 0, 0] * A[:, 1, 8] * A[:, 2, 4] - A[:, 0, 4] * A[:, 1, 0] * A[:, 2, 8] + \
                A[:, 0, 4] * A[:, 1, 8] * A[:, 2, 0] + A[:, 0, 8] * A[:, 1, 0] * A[:, 2, 4] - A[:, 0, 8] * A[:, 1, 4] * A[:, 2, 0]

        E_models = []
        #s = StrumPolynomialSolver(10)
        #n_solss, rootss = s.bisect_sturm(cs)

        # for loop because of different numbers of solutions
        for bi in range(A.shape[0]):
            A_i = A[bi]
            null_i = nullSpace[bi]

            # companion matrix solver
            # try:
            C = torch.zeros((10, 10), device=cs.device, dtype=cs.dtype)
            C[0:-1, 1:] = torch.eye(C[0:-1, 0:-1].shape[0], device=cs.device, dtype=cs.dtype)
            C[-1, :] = -cs[bi][:-1]/cs[bi][-1]
            # check if the companion matrix contains nans or infs
            if torch.isnan(C).any() or torch.isinf(C).any():
                continue
                #n_sols, roots = s.bisect_sturm(cs[bi])
                #print("nan in C")
            else:
                roots = torch.real(torch.linalg.eigvals(C))
            # except ValueError:
            #n_sols, roots = s.bisect_sturm(cs[bi])

            if roots is None:
                continue
            n_sols = roots.size()
            if n_sols == 0:
                continue
            Bs = torch.stack((A_i[:3, :1] * (roots ** 3) + A_i[:3, 1:2] * roots.square() + A_i[0:3, 2:3] * (roots) + A_i[0:3, 3:4],
                            A_i[0:3, 4:5] * (roots ** 3) + A_i[0:3, 5:6] * roots.square() + A_i[0:3, 6:7] * (roots) + A_i[0:3, 7:8]), dim=0).transpose(0 ,-1)

            bs = (A_i[0:3, 8:9] * (roots ** 4) + A_i[0:3, 9:10] * (roots ** 3) + A_i[0:3, 10:11] * roots.square() + A_i[0:3, 11:12] * roots + A_i[0:3, 12:13]).T.unsqueeze(-1)

            # We try to solve using top two rows, if fails, will use matrix decomposition to solve Ax=b.
            try:
                xzs = Bs[:, 0:2, 0:2].inverse() @ (bs[:, 0:2])
            except:
                continue
            mask = (abs(Bs[:, 2].unsqueeze(1) @ xzs - bs[:, 2].unsqueeze(1)) > 1e-3).flatten()
            if torch.sum(mask) != 0:
                q, r = torch.linalg.qr(Bs[mask].clone())#
                xzs[mask] = torch.linalg.solve(r, q.transpose(-1, -2) @ bs[mask])#[mask]

            # models
            Es = null_i[0] * (-xzs[:, 0]) + null_i[1] * (-xzs[:, 1]) + null_i[2] * roots.unsqueeze(-1) + null_i[3]

            # Since the rows of N are orthogonal unit vectors, we can normalize the coefficients instead
            inv = 1.0 / torch.sqrt((-xzs[:, 0]) ** 2 + (-xzs[:, 1]) ** 2 + roots.unsqueeze(-1) ** 2 + 1.0)
            Es *= inv
            if Es.shape[0] < 10:
                Es = torch.concat((Es.clone(), torch.eye(3, device=Es.device, dtype=Es.dtype).repeat(10-Es.shape[0], 1).reshape(-1, 9)))
            E_models.append(Es)

        if not E_models:
            return torch.eye(3, device=cs.device, dtype=cs.dtype).unsqueeze(0)
        else:
            return torch.concat(E_models).view(-1,  3,  3).transpose(-1, -2)
            # be careful of the differences between c++ and python, transpose

    def o1(self, a, b):
        """A, b are first order polys [x,y,z,1] c is degree 2 poly with order [ x^2, x*y, x*z, x, y^2, y*z, y, z^2,
        z, 1]"""
        # print(a[0] * b[2] + a[2] * b[0])
        return torch.stack([a[:, 0] * b[:, 0], a[:, 0] * b[:, 1] + a[:, 1] * b[:, 0], a[:, 0] * b[:, 2] + a[:, 2] * b[:, 0],
                            a[:, 0] * b[:, 3] + a[:, 3] * b[:, 0], a[:, 1] * b[:, 1], a[:, 1] * b[:, 2] + a[:, 2] * b[:, 1],
                            a[:, 1] * b[:, 3] + a[:, 3] * b[:, 1], a[:, 2] * b[:, 2], a[:, 2] * b[:, 3] + a[:, 3] * b[:, 2],
                            a[:, 3] * b[:, 3]], dim=-1)

    def o2(self, a, b):  # 10 4 20
        """A is second degree poly with order [ x^2, x*y, x*z, x, y^2, y*z, y, z^2, z, 1] b is first degree with
        order [x y z 1] c is third degree with order (same as nister's paper) [ x^3, y^3, x^2*y, x*y^2, x^2*z, x^2,
        y^2*z, y^2, x*y*z, x*y, x*z^2, x*z, x, y*z^2, y*z, y, z^3, z^2, z, 1]"""
        return torch.stack(
            [a[:, 0] * b[:, 0], a[:, 4] * b[:, 1], a[:, 0] * b[:, 1] + a[:, 1] * b[:, 0], a[:, 1] * b[:, 1] + a[:, 4] * b[:, 0], a[:, 0] * b[:, 2] + a[:, 2] * b[:, 0],
             a[:, 0] * b[:, 3] + a[:, 3] * b[:, 0], a[:, 4] * b[:, 2] + a[:, 5] * b[:, 1], a[:, 4] * b[:, 3] + a[:, 6] * b[:, 1],
             a[:, 1] * b[:, 2] + a[:, 2] * b[:, 1] + a[:, 5] * b[:, 0], a[:, 1] * b[:, 3] + a[:, 3] * b[:, 1] + a[:, 6] * b[:, 0],
             a[:, 2] * b[:, 2] + a[:, 7] * b[:, 0], a[:, 2] * b[:, 3] + a[:, 3] * b[:, 2] + a[:, 8] * b[:, 0], a[:, 3] * b[:, 3] + a[:, 9] * b[:, 0],
             a[:, 5] * b[:, 2] + a[:, 7] * b[:, 1], a[:, 5] * b[:, 3] + a[:, 6] * b[:, 2] + a[:, 8] * b[:, 1],
             a[:, 6] * b[:, 3] + a[:, 9] * b[:, 1], a[:, 7] * b[:, 2], a[:, 7] * b[:, 3] + a[:, 8] * b[:, 2],
             a[:, 8] * b[:, 3] + a[:, 9] * b[:, 2], a[:, 9] * b[:, 3]], dim=-1)