// File: /workspace/github_cpp_makefile_star2_O2/chrislarson1[P]MVEE/src/mvee.cpp
// Function: mvee::Mvee::khachiyan(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, double, double)
void Mvee::khachiyan(MatrixXd& data, double eps, double lmcoeff)
{
    // Khachiyan's algorithm
    auto t0 = chrono::system_clock::now();    
    iters = 0; 
    long double err = INFINITY; 
    double alpha;
    int n = data.rows();
    int d = data.cols();
    MatrixXd X = data.transpose();
    MatrixXd Q(X.rows() + 1, X.cols());
    Q.row(0) = X.row(0);
    Q.row(1) = X.row(1);
    Q.row(2).setOnes();
    VectorXd u = (1 / (double) n) * VectorXd::Ones(n);
    VectorXd uhat = u;
    MatrixXd G(d + 1, d + 1);
    MatrixXd noiseye = MatrixXd::Identity(G.rows(), G.cols()) * lmcoeff;
    VectorXd g(n);
    double m; int i;
    while (err > eps)
    {
        G = Q * u.asDiagonal() * Q.transpose() + noiseye;
        g = (Q.transpose() * G.inverse() * Q).diagonal();
        m = g.maxCoeff();
        i = findIdx(g, m);
        alpha = (m - d - 1) / ((d + 1) * (m - 1));
        uhat = (1 - alpha) * u;
        uhat(i) += alpha;
        err = (uhat - u).norm();
        u = uhat;
        iters++;
    }
    time = chrono::duration<double>(
        chrono::system_clock::now() - t0
    ).count();
    
    // Decompose US^2U^T
    MatrixXd E = (1 / (double) d) * (
        X * u.asDiagonal() * X.transpose()
        - (X * u) * (X * u).transpose()
    ).inverse();
    VectorXd x = X * u;
    VectorXd s(d);
    MatrixXd V(d, d);
    MatrixXd U(d, d);
    decompose(E, s, U, V);

    // Cache result
    _centroid = toStdVec(x, [](double z) { return z; });
    _radii    = toStdVec(s, [](double z) { return 1 / sqrt(z); });
    _pose     = toStdMat(V, [](double z) { return z; });
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x248, %rsp            # imm = 0x248
movsd %xmm0, (%rsp)
movq %rsi, %rbx
movq %rdi, %r13
movsd %xmm1, 0x190(%rsp)
callq 0x55230
andq $0x0, 0x50(%r13)
movq %rax, %rbp
movl 0x8(%rbx), %eax
movl %eax, 0xc(%rsp)
movl 0x10(%rbx), %r15d
movl %r15d, 0x8(%rsp)
leaq 0x198(%rsp), %r14
movq %rbx, (%r14)
leaq 0x48(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x59420
movq 0x8(%rbx), %rax
movq 0x10(%rbx), %rcx
incq %rax
movq %rax, (%r14)
leaq 0xc8(%rsp), %rdx
movq %rcx, (%rdx)
leaq 0x30(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x56bf0
leaq 0x198(%rsp), %rdi
leaq 0x48(%rsp), %rsi
xorl %edx, %edx
callq 0x5a680
leaq 0xc8(%rsp), %rdi
leaq 0x30(%rsp), %rsi
xorl %edx, %edx
callq 0x5a680
leaq 0xc8(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x55640
leaq 0x198(%rsp), %rdi
leaq 0x48(%rsp), %rsi
pushq $0x1
popq %rdx
callq 0x5a680
leaq 0xc8(%rsp), %rdi
leaq 0x30(%rsp), %rsi
pushq $0x1
popq %rdx
callq 0x5a680
leaq 0xc8(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x55640
leaq 0x198(%rsp), %rdi
leaq 0x30(%rsp), %rsi
pushq $0x2
popq %rdx
callq 0x5a680
leaq 0x198(%rsp), %rdi
callq 0x56670
movslq 0xc(%rsp), %rsi
cvtsi2sd %esi, %xmm0
movsd 0x2414f(%rip), %xmm1    # 0x7f000
divsd %xmm0, %xmm1
movsd %xmm1, 0x128(%rsp)
leaq 0xc8(%rsp), %rdi
callq 0x55a40
leaq 0x198(%rsp), %rdi
leaq 0x128(%rsp), %rsi
leaq 0xc8(%rsp), %rdx
callq 0x58970
leaq 0x10(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x58930
leaq 0x180(%rsp), %rdi
leaq 0x10(%rsp), %r12
movq %r12, %rsi
callq 0x598f0
leal 0x1(%r15), %ebx
leaq 0x198(%rsp), %rsi
movl %ebx, (%rsi)
leaq 0xc8(%rsp), %rdx
movl %ebx, (%rdx)
leaq 0x90(%rsp), %rdi
callq 0x597c0
movq 0x98(%rsp), %rsi
movq 0xa0(%rsp), %rdx
leaq 0xc8(%rsp), %rdi
callq 0x55560
leaq 0x198(%rsp), %rdi
leaq 0xc8(%rsp), %rsi
leaq 0x190(%rsp), %rdx
callq 0x59540
leaq 0x210(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x55c90
movq %rbp, 0x108(%rsp)
movq %r13, 0x20(%rsp)
leaq 0x170(%rsp), %rdi
leaq 0xc(%rsp), %rsi
callq 0x58250
movsd (%rsp), %xmm0
movsd %xmm0, 0x120(%rsp)
cvtsi2sd %r15d, %xmm0
movsd %xmm0, 0x28(%rsp)
cvtsi2sd %ebx, %xmm0
movsd %xmm0, 0x110(%rsp)
fldl 0x120(%rsp)
fstpt 0x204(%rsp)
flds 0x24064(%rip)           # 0x7f040
leaq 0xc8(%rsp), %rbp
leaq 0x198(%rsp), %r13
leaq 0x170(%rsp), %rbx
leaq 0x180(%rsp), %r14
fldt 0x204(%rsp)
fxch %st(1)
fucompi %st(1), %st
fstp %st(0)
jbe 0x5b1bd
movq %r12, 0xa8(%rsp)
leaq 0x30(%rsp), %r15
movq %r15, %rdi
leaq 0xa8(%rsp), %rsi
callq 0x559e0
movq %rax, 0x128(%rsp)
movq %rdx, 0x130(%rsp)
movq %r15, 0x60(%rsp)
movq %rbp, %rdi
leaq 0x128(%rsp), %rsi
leaq 0x60(%rsp), %rdx
callq 0x5a700
movq %r13, %rdi
movq %rbp, %rsi
leaq 0x210(%rsp), %rdx
callq 0x55690
leaq 0x90(%rsp), %rdi
movq %r13, %rsi
callq 0x56d90
leaq 0x30(%rsp), %rax
movq %rax, 0xa8(%rsp)
leaq 0x90(%rsp), %rdi
callq 0x57ab0
movq %rax, 0x60(%rsp)
leaq 0xa8(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0x59c20
movq %rax, 0x128(%rsp)
movq %rdx, 0x130(%rsp)
movq %rbp, %rdi
leaq 0x128(%rsp), %rsi
leaq 0x30(%rsp), %rdx
callq 0x56d80
movq %r13, %rdi
movq %rbp, %rsi
callq 0x5a740
movq %rbx, %rdi
movq %r13, %rsi
callq 0x57ed0
movq %rbx, %rdi
callq 0x572e0
movq %rbx, %rdi
movsd %xmm0, (%rsp)
callq 0x569f0
movsd (%rsp), %xmm2
movl %eax, %r15d
movapd %xmm2, %xmm1
subsd 0x28(%rsp), %xmm1
movsd 0x23efc(%rip), %xmm0    # 0x7f010
addsd %xmm0, %xmm1
addsd %xmm0, %xmm2
mulsd 0x110(%rsp), %xmm2
divsd %xmm2, %xmm1
movsd 0x23ecf(%rip), %xmm0    # 0x7f000
movsd %xmm1, (%rsp)
subsd %xmm1, %xmm0
movsd %xmm0, 0xc8(%rsp)
movq %r13, %rdi
movq %rbp, %rsi
movq %r12, %rdx
callq 0x56860
movq %r14, %rdi
movq %r13, %rsi
callq 0x56a10
movslq %r15d, %rsi
movq %r14, %rdi
callq 0x5a640
movsd (%rsp), %xmm0
addsd (%rax), %xmm0
movsd %xmm0, (%rax)
movq %r13, %rdi
movq %r14, %rsi
movq %r12, %rdx
callq 0x5a0d0
movq %r13, %rdi
callq 0x58aa0
movsd %xmm0, (%rsp)
movq %r12, %rdi
movq %r14, %rsi
callq 0x56530
movsd (%rsp), %xmm0
movsd %xmm0, 0x118(%rsp)
fldl 0x118(%rsp)
movq 0x20(%rsp), %rax
incq 0x50(%rax)
jmp 0x5affc
callq 0x55230
subq 0x108(%rsp), %rax
cvtsi2sd %rax, %xmm0
divsd 0x23e31(%rip), %xmm0    # 0x7f008
movq 0x20(%rsp), %r15
movsd %xmm0, 0x48(%r15)
movsd 0x23e16(%rip), %xmm0    # 0x7f000
divsd 0x28(%rsp), %xmm0
movsd %xmm0, 0x168(%rsp)
leaq 0x230(%rsp), %rsi
movq %r12, (%rsi)
leaq 0x48(%rsp), %rbx
movq %rbx, %rdi
callq 0x559e0
leaq 0x238(%rsp), %rsi
movq %rax, (%rsi)
movq %rdx, 0x8(%rsi)
leaq 0x228(%rsp), %rdx
movq %rbx, (%rdx)
leaq 0x78(%rsp), %rdi
callq 0x5a700
leaq 0x48(%rsp), %rdi
leaq 0x10(%rsp), %rsi
callq 0x581c0
movq %rax, %rbx
movq %rdx, %r14
leaq 0x48(%rsp), %rdi
leaq 0x10(%rsp), %rsi
callq 0x581c0
leaq 0xa8(%rsp), %rcx
movq %rbx, (%rcx)
movq %r14, 0x8(%rcx)
movq %rax, 0x10(%rcx)
movq %rdx, 0x18(%rcx)
leaq 0x128(%rsp), %rdi
leaq 0x78(%rsp), %rsi
movq %rcx, %rdx
callq 0x56710
leaq 0xc8(%rsp), %rdi
leaq 0x128(%rsp), %rsi
callq 0x57460
leaq 0x198(%rsp), %rdi
leaq 0x168(%rsp), %rsi
leaq 0xc8(%rsp), %rdx
callq 0x574b0
leaq 0x60(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x55360
leaq 0x48(%rsp), %rdi
leaq 0x10(%rsp), %rsi
callq 0x581c0
leaq 0x198(%rsp), %rsi
movq %rax, (%rsi)
movq %rdx, 0x8(%rsi)
leaq 0xa8(%rsp), %rdi
callq 0x57e40
leaq 0x78(%rsp), %rdi
leaq 0x8(%rsp), %rsi
callq 0x58250
leaq 0x198(%rsp), %rdi
leaq 0x8(%rsp), %rdx
movq %rdx, %rsi
callq 0x597c0
leaq 0xc8(%rsp), %rdi
leaq 0x8(%rsp), %rdx
movq %rdx, %rsi
callq 0x597c0
leaq 0x60(%rsp), %rsi
leaq 0x78(%rsp), %rdx
leaq 0xc8(%rsp), %rcx
leaq 0x198(%rsp), %r8
callq 0x57660
leaq 0x40b(%rip), %rdx       # 0x5b75e
leaq 0x128(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x5a060
leaq 0x128(%rsp), %rbx
movq %r15, %rdi
movq %rbx, %rsi
callq 0x554a0
movq %rbx, %rdi
callq 0x59aa0
leaq 0x3d6(%rip), %rdx       # 0x5b760
leaq 0x128(%rsp), %rdi
leaq 0x78(%rsp), %rsi
callq 0x5a060
leaq 0x18(%r15), %rdi
leaq 0x128(%rsp), %rbx
movq %rbx, %rsi
callq 0x554a0
movq %rbx, %rdi
callq 0x59aa0
leaq 0x3c9(%rip), %rdx       # 0x5b788
leaq 0x128(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x5a0c0
addq $0x30, %r15
leaq 0x128(%rsp), %rbx
movq %r15, %rdi
movq %rbx, %rsi
callq 0x55650
movq %rbx, %rdi
callq 0x55c80
movq 0xc8(%rsp), %rdi
callq 0x583e0
movq 0x198(%rsp), %rdi
callq 0x583e0
movq 0x78(%rsp), %rdi
callq 0x583e0
movq 0xa8(%rsp), %rdi
callq 0x583e0
movq 0x60(%rsp), %rdi
callq 0x583e0
movq 0x170(%rsp), %rdi
callq 0x583e0
movq 0x210(%rsp), %rdi
callq 0x583e0
movq 0x90(%rsp), %rdi
callq 0x583e0
movq 0x180(%rsp), %rdi
callq 0x583e0
movq 0x10(%rsp), %rdi
callq 0x583e0
movq 0x30(%rsp), %rdi
callq 0x583e0
movq 0x48(%rsp), %rdi
callq 0x583e0
addq $0x248, %rsp            # imm = 0x248
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x5b498
jmp 0x5b498
jmp 0x5b498
movq %rax, %rbx
movq 0xc8(%rsp), %rdi
callq 0x583e0
jmp 0x5b4ad
movq %rax, %rbx
movq 0x198(%rsp), %rdi
callq 0x583e0
movq 0x78(%rsp), %rdi
callq 0x583e0
movq 0xa8(%rsp), %rdi
callq 0x583e0
jmp 0x5b4fe
movq %rax, %rbx
jmp 0x5b4ba
movq %rax, %rbx
jmp 0x5b4c4
jmp 0x5b526
jmp 0x5b526
jmp 0x5b526
movq %rax, %rbx
jmp 0x5b536
movq %rax, %rbx
jmp 0x5b550
movq %rax, %rbx
jmp 0x5b55d
jmp 0x5b515
jmp 0x5b515
movq %rax, %rbx
jmp 0x5b571
movq %rax, %rbx
movq 0x60(%rsp), %rdi
callq 0x583e0
jmp 0x5b529
jmp 0x5b515
jmp 0x5b515
jmp 0x5b515
movq %rax, %rbx
jmp 0x5b543
movq %rax, %rbx
jmp 0x5b567
jmp 0x5b526
jmp 0x5b526
jmp 0x5b526
jmp 0x5b526
jmp 0x5b526
jmp 0x5b526
movq %rax, %rbx
movq 0x170(%rsp), %rdi
callq 0x583e0
movq 0x210(%rsp), %rdi
callq 0x583e0
movq 0x90(%rsp), %rdi
callq 0x583e0
movq 0x180(%rsp), %rdi
callq 0x583e0
movq 0x10(%rsp), %rdi
callq 0x583e0
movq 0x30(%rsp), %rdi
callq 0x583e0
movq 0x48(%rsp), %rdi
callq 0x583e0
movq %rbx, %rdi
callq 0x59d40
nop