// File: icemiliang[P]vot/src/vot/../../include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
// Function: void Eigen::internal::least_square_conjugate_gradient<Eigen::Ref<Eigen::SparseMatrix<double, 0, int> const, 0, Eigen::OuterStride<-1>>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, -1, 1, true>, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, true>, Eigen::LeastSquareDiagonalPreconditioner<double>>(Eigen::Ref<Eigen::SparseMatrix<double, 0, int> const, 0, Eigen::OuterStride<-1>> const&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, -1, 1, true> const&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, true>&, Eigen::LeastSquareDiagonalPreconditioner<double> const&, long&, Eigen::Block<Eigen::Matrix<double, -1, 1, 0, -1, 1>, -1, 1, true>::RealScalar&)
EIGEN_DONT_INLINE
void least_square_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
                                     const Preconditioner& precond, Index& iters,
                                     typename Dest::RealScalar& tol_error)
{
  using std::sqrt;
  using std::abs;
  typedef typename Dest::RealScalar RealScalar;
  typedef typename Dest::Scalar Scalar;
  typedef Matrix<Scalar,Dynamic,1> VectorType;
  
  RealScalar tol = tol_error;
  Index maxIters = iters;
  
  Index m = mat.rows(), n = mat.cols();

  VectorType residual        = rhs - mat * x;
  VectorType normal_residual = mat.adjoint() * residual;

  RealScalar rhsNorm2 = (mat.adjoint()*rhs).squaredNorm();
  if(rhsNorm2 == 0) 
  {
    x.setZero();
    iters = 0;
    tol_error = 0;
    return;
  }
  RealScalar threshold = tol*tol*rhsNorm2;
  RealScalar residualNorm2 = normal_residual.squaredNorm();
  if (residualNorm2 < threshold)
  {
    iters = 0;
    tol_error = sqrt(residualNorm2 / rhsNorm2);
    return;
  }
  
  VectorType p(n);
  p = precond.solve(normal_residual);                         // initial search direction

  VectorType z(n), tmp(m);
  RealScalar absNew = numext::real(normal_residual.dot(p));  // the square of the absolute value of r scaled by invM
  Index i = 0;
  while(i < maxIters)
  {
    tmp.noalias() = mat * p;

    Scalar alpha = absNew / tmp.squaredNorm();      // the amount we travel on dir
    x += alpha * p;                                 // update solution
    residual -= alpha * tmp;                        // update residual
    normal_residual = mat.adjoint() * residual;     // update residual of the normal equation
    
    residualNorm2 = normal_residual.squaredNorm();
    if(residualNorm2 < threshold)
      break;
    
    z = precond.solve(normal_residual);             // approximately solve for "A'A z = normal_residual"

    RealScalar absOld = absNew;
    absNew = numext::real(normal_residual.dot(z));  // update the absolute value of r
    RealScalar beta = absNew / absOld;              // calculate the Gram-Schmidt value used to create the new search direction
    p = z + beta * p;                               // update search direction
    i++;
  }
  tol_error = sqrt(residualNorm2 / rhsNorm2);
  iters = i;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x440, %rsp            # imm = 0x440
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq %rcx, -0x20(%rbp)
movq %r8, -0x28(%rbp)
movq %r9, -0x30(%rbp)
movq -0x30(%rbp), %rax
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movq -0x28(%rbp), %rax
movq (%rax), %rax
movq %rax, -0x40(%rbp)
movq -0x8(%rbp), %rdi
callq 0x35c70
movq %rax, -0x48(%rbp)
movq -0x8(%rbp), %rdi
callq 0x35500
movq %rax, -0x50(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x3a0(%rbp)
movq -0x8(%rbp), %rsi
movq -0x18(%rbp), %rdx
leaq -0x120(%rbp), %rdi
movq %rdi, -0x398(%rbp)
callq 0x37680
movq -0x3a0(%rbp), %rsi
movq -0x398(%rbp), %rdx
leaq -0xe0(%rbp), %rdi
movq %rdi, -0x390(%rbp)
callq 0x37610
movq -0x390(%rbp), %rsi
leaq -0x60(%rbp), %rdi
callq 0x376d0
movq -0x8(%rbp), %rdi
callq 0x37710
movq %rdx, -0x388(%rbp)
movb %al, -0x379(%rbp)
jmp 0x36c44
movq -0x388(%rbp), %rax
movb -0x379(%rbp), %cl
movb %cl, -0x158(%rbp)
movq %rax, -0x150(%rbp)
leaq -0x148(%rbp), %rdi
leaq -0x158(%rbp), %rsi
leaq -0x60(%rbp), %rdx
callq 0x37740
jmp 0x36c77
leaq -0x130(%rbp), %rdi
leaq -0x148(%rbp), %rsi
callq 0x37790
jmp 0x36c8c
movq -0x8(%rbp), %rdi
callq 0x37710
movq %rdx, -0x3b0(%rbp)
movb %al, -0x3a1(%rbp)
jmp 0x36ca4
movq -0x3b0(%rbp), %rax
movb -0x3a1(%rbp), %cl
movb %cl, -0x1c8(%rbp)
movq %rax, -0x1c0(%rbp)
movq -0x10(%rbp), %rdx
leaq -0x1b8(%rbp), %rdi
leaq -0x1c8(%rbp), %rsi
callq 0x377d0
jmp 0x36cd7
leaq -0x1b8(%rbp), %rdi
callq 0x37820
movsd %xmm0, -0x3b8(%rbp)
jmp 0x36ced
movsd -0x3b8(%rbp), %xmm0
movsd %xmm0, -0x170(%rbp)
movsd -0x170(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x36d6e
jp 0x36d6e
movq -0x18(%rbp), %rdi
callq 0x37860
jmp 0x36d1b
movq -0x28(%rbp), %rax
movq $0x0, (%rax)
movq -0x30(%rbp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movl $0x1, -0x1cc(%rbp)
jmp 0x37285
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x160(%rbp)
movl %eax, -0x164(%rbp)
jmp 0x372c7
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x160(%rbp)
movl %eax, -0x164(%rbp)
jmp 0x372bb
movsd -0x38(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd -0x170(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x1d8(%rbp)
leaq -0x130(%rbp), %rdi
callq 0x33cc0
movsd %xmm0, -0x3c0(%rbp)
jmp 0x36da1
movsd -0x3c0(%rbp), %xmm0
movsd %xmm0, -0x1e0(%rbp)
movsd -0x1e0(%rbp), %xmm1
movsd -0x1d8(%rbp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x36dfe
movq -0x28(%rbp), %rax
movq $0x0, (%rax)
movsd -0x1e0(%rbp), %xmm0
divsd -0x170(%rbp), %xmm0
callq 0xa3d0
movq -0x30(%rbp), %rax
movsd %xmm0, (%rax)
movl $0x1, -0x1cc(%rbp)
jmp 0x37285
leaq -0x1f0(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x37890
jmp 0x36e10
movq -0x20(%rbp), %rdi
leaq -0x130(%rbp), %rsi
callq 0x378f0
movq %rdx, -0x3d0(%rbp)
movq %rax, -0x3c8(%rbp)
jmp 0x36e30
movq -0x3d0(%rbp), %rax
movq -0x3c8(%rbp), %rcx
movq %rcx, -0x200(%rbp)
movq %rax, -0x1f8(%rbp)
leaq -0x1f0(%rbp), %rdi
leaq -0x200(%rbp), %rsi
callq 0x379d0
jmp 0x36e61
leaq -0x210(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x37890
jmp 0x36e73
leaq -0x220(%rbp), %rdi
leaq -0x48(%rbp), %rsi
callq 0x37890
jmp 0x36e85
leaq -0x130(%rbp), %rdi
leaq -0x1f0(%rbp), %rsi
callq 0x37a00
movsd %xmm0, -0x3d8(%rbp)
jmp 0x36ea2
movsd -0x3d8(%rbp), %xmm0
movsd %xmm0, -0x230(%rbp)
leaq -0x230(%rbp), %rdi
callq 0x33d20
movsd %xmm0, -0x3e0(%rbp)
jmp 0x36ec8
movsd -0x3e0(%rbp), %xmm0
movsd %xmm0, -0x228(%rbp)
movq $0x0, -0x238(%rbp)
movq -0x238(%rbp), %rax
cmpq -0x40(%rbp), %rax
jge 0x3722c
movq -0x8(%rbp), %rdi
leaq -0x1f0(%rbp), %rsi
callq 0x37a70
movq %rdx, -0x3f0(%rbp)
movq %rax, -0x3e8(%rbp)
jmp 0x36f14
movq -0x3f0(%rbp), %rax
movq -0x3e8(%rbp), %rcx
movq %rcx, -0x248(%rbp)
movq %rax, -0x240(%rbp)
leaq -0x220(%rbp), %rdi
callq 0x37ac0
movq %rax, -0x3f8(%rbp)
jmp 0x36f45
movq -0x3f8(%rbp), %rax
movq %rax, -0x250(%rbp)
leaq -0x250(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x37af0
jmp 0x36f68
movsd -0x228(%rbp), %xmm0
movsd %xmm0, -0x408(%rbp)
leaq -0x220(%rbp), %rdi
callq 0x33cc0
movsd %xmm0, -0x400(%rbp)
jmp 0x36f8e
movsd -0x400(%rbp), %xmm1
movsd -0x408(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x258(%rbp)
leaq -0x288(%rbp), %rdi
leaq -0x258(%rbp), %rsi
leaq -0x1f0(%rbp), %rdx
callq 0x37b50
jmp 0x36fc6
movq -0x18(%rbp), %rdi
leaq -0x288(%rbp), %rsi
callq 0x37bf0
jmp 0x36fd8
leaq -0x2b8(%rbp), %rdi
leaq -0x258(%rbp), %rsi
leaq -0x220(%rbp), %rdx
callq 0x37b50
jmp 0x36ff4
leaq -0x60(%rbp), %rdi
leaq -0x2b8(%rbp), %rsi
callq 0x37c50
jmp 0x37006
movq -0x8(%rbp), %rdi
callq 0x37710
movq %rdx, -0x418(%rbp)
movb %al, -0x409(%rbp)
jmp 0x3701e
movq -0x418(%rbp), %rax
movb -0x409(%rbp), %cl
movb %cl, -0x2e0(%rbp)
movq %rax, -0x2d8(%rbp)
leaq -0x2d0(%rbp), %rdi
leaq -0x2e0(%rbp), %rsi
leaq -0x60(%rbp), %rdx
callq 0x37740
jmp 0x37051
leaq -0x130(%rbp), %rdi
leaq -0x2d0(%rbp), %rsi
callq 0x37cb0
jmp 0x37066
leaq -0x130(%rbp), %rdi
callq 0x33cc0
movsd %xmm0, -0x420(%rbp)
jmp 0x3707c
movsd -0x420(%rbp), %xmm0
movsd %xmm0, -0x1e0(%rbp)
movsd -0x1e0(%rbp), %xmm1
movsd -0x1d8(%rbp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x370f8
jmp 0x3722c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x160(%rbp)
movl %eax, -0x164(%rbp)
jmp 0x372af
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x160(%rbp)
movl %eax, -0x164(%rbp)
jmp 0x372a3
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x160(%rbp)
movl %eax, -0x164(%rbp)
leaq -0x220(%rbp), %rdi
callq 0x21580
jmp 0x372a3
movq -0x20(%rbp), %rdi
leaq -0x130(%rbp), %rsi
callq 0x378f0
movq %rdx, -0x430(%rbp)
movq %rax, -0x428(%rbp)
jmp 0x37118
movq -0x430(%rbp), %rax
movq -0x428(%rbp), %rcx
movq %rcx, -0x2f0(%rbp)
movq %rax, -0x2e8(%rbp)
leaq -0x210(%rbp), %rdi
leaq -0x2f0(%rbp), %rsi
callq 0x379d0
jmp 0x37149
movsd -0x228(%rbp), %xmm0
movsd %xmm0, -0x2f8(%rbp)
leaq -0x130(%rbp), %rdi
leaq -0x210(%rbp), %rsi
callq 0x37a00
movsd %xmm0, -0x438(%rbp)
jmp 0x37176
movsd -0x438(%rbp), %xmm0
movsd %xmm0, -0x300(%rbp)
leaq -0x300(%rbp), %rdi
callq 0x33d20
movsd %xmm0, -0x440(%rbp)
jmp 0x3719c
movsd -0x440(%rbp), %xmm0
movsd %xmm0, -0x228(%rbp)
movsd -0x228(%rbp), %xmm0
movsd -0x2f8(%rbp), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x308(%rbp)
leaq -0x378(%rbp), %rdi
leaq -0x308(%rbp), %rsi
leaq -0x1f0(%rbp), %rdx
callq 0x37b50
jmp 0x371e4
leaq -0x348(%rbp), %rdi
leaq -0x210(%rbp), %rsi
leaq -0x378(%rbp), %rdx
callq 0x37ce0
jmp 0x37200
leaq -0x1f0(%rbp), %rdi
leaq -0x348(%rbp), %rsi
callq 0x37d50
jmp 0x37215
movq -0x238(%rbp), %rax
addq $0x1, %rax
movq %rax, -0x238(%rbp)
jmp 0x36ee3
movsd -0x1e0(%rbp), %xmm0
divsd -0x170(%rbp), %xmm0
callq 0xa3d0
movq -0x30(%rbp), %rax
movsd %xmm0, (%rax)
movq -0x238(%rbp), %rcx
movq -0x28(%rbp), %rax
movq %rcx, (%rax)
leaq -0x220(%rbp), %rdi
callq 0x21580
leaq -0x210(%rbp), %rdi
callq 0x21580
leaq -0x1f0(%rbp), %rdi
callq 0x21580
movl $0x0, -0x1cc(%rbp)
leaq -0x130(%rbp), %rdi
callq 0x21580
leaq -0x60(%rbp), %rdi
callq 0x21580
addq $0x440, %rsp            # imm = 0x440
popq %rbp
retq
leaq -0x210(%rbp), %rdi
callq 0x21580
leaq -0x1f0(%rbp), %rdi
callq 0x21580
leaq -0x130(%rbp), %rdi
callq 0x21580
leaq -0x60(%rbp), %rdi
callq 0x21580
movq -0x160(%rbp), %rdi
callq 0xa950
nopl (%rax)