// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelPointGraph.cpp
// Function: gaps::R3SurfelPointGraph::RemoveOutlierEdges(double)
void R3SurfelPointGraph::
RemoveOutlierEdges(RNScalar max_zscore)
{
  // Allocate temporary memory for edge length statistics
  RNScalar *means = new RNScalar [ NPoints() ];
  RNScalar *stddevs = new RNScalar [ NPoints() ];

  // Compute edge length statistics
  for (int i = 0; i < NPoints(); i++) {
    // Compute sum
    RNLength sum = 0;
    R3SurfelPoint *point0 = Point(i);
    R3Point position0 = point0->Position();
    for (int j = 0; j < NNeighbors(i); j++) {
      R3SurfelPoint *point1 = Neighbor(i,j);
      R3Point position1 = point1->Position();
      RNLength edge_length = R3Distance(position0, position1);
      sum += edge_length;
    }
    
    // Compute mean
    means[i] = (NNeighbors(i)) ? sum / NNeighbors(i) : 0;

    // Compute sum of squared residuals
    RNLength ssd = 0;
    for (int j = 0; j < NNeighbors(i); j++) {
      R3SurfelPoint *point1 = Neighbor(i,j);
      R3Point position1 = point1->Position();
      RNLength edge_length = R3Distance(position0, position1);
      RNLength delta = edge_length - means[i];
      ssd += delta * delta;
    }

    // Compute standard deviation
    RNScalar variance = (NNeighbors(i)) ? ssd / NNeighbors(i) : 0; 
    stddevs[i] = sqrt(variance);
  }

  // Remove outlier edges
  for (int i = 0; i < NPoints(); i++) {
    if (stddevs[i] == 0) continue;
    R3SurfelPoint *point0 = Point(i);
    R3Point position0 = point0->Position();
    for (int j = 0; j < neighbors[i].NEntries(); j++) {
      R3SurfelPoint *point1 = neighbors[i].Kth(j);
      R3Point position1 = point1->Position();
      RNLength edge_length = R3Distance(position0, position1);
      RNScalar zscore = (edge_length - means[i]) / stddevs[i];
      if (zscore > max_zscore) {
        // Remove edge
        RNArrayEntry *entry = neighbors[i].KthEntry(j);
        neighbors[i].EntryContents(entry) = neighbors[i].Tail();
        neighbors[i].RemoveTail();
        j--;
      }
    }
  }    

  // Delete temporary memory for edge length statistics
  delete [] means;
  delete [] stddevs;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x58, %rsp
movsd %xmm0, 0x18(%rsp)
movq %rdi, %rbx
movslq 0x18(%rdi), %r14
leaq (,%r14,8), %rax
testq %r14, %r14
movq $-0x1, %r15
cmovnsq %rax, %r15
movq %r15, %rdi
callq 0x13030
movq %rax, 0x8(%rsp)
movq %r15, %rdi
callq 0x13030
movq %rax, 0x10(%rsp)
testq %r14, %r14
jle 0x37139
xorl %r15d, %r15d
leaq 0x28(%rsp), %r12
leaq 0x40(%rsp), %r13
movq 0x10(%rbx), %rax
movq %r15, %rbp
shlq $0x4, %rbp
movq (%rax,%rbp), %rcx
movq 0x8(%rax,%rbp), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rcx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rcx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rcx), %xmm2
movq %r12, %rdi
callq 0x83dc2
movq 0x68(%rbx), %rcx
movl 0xc(%rcx,%rbp), %eax
testl %eax, %eax
jle 0x36f92
xorpd %xmm0, %xmm0
xorl %r14d, %r14d
movsd %xmm0, (%rsp)
movq (%rcx,%rbp), %rax
movq (%rax,%r14,8), %rax
movq (%rax), %rcx
movq 0x8(%rax), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rcx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rcx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rcx), %xmm2
movq %r13, %rdi
callq 0x83dc2
movq %r12, %rdi
movq %r13, %rsi
callq 0x4ff14
movsd (%rsp), %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, (%rsp)
movsd (%rsp), %xmm0
incq %r14
movq 0x68(%rbx), %rcx
movslq 0xc(%rcx,%rbp), %rax
cmpq %rax, %r14
jl 0x36e6e
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
xorpd %xmm3, %xmm3
xorpd %xmm2, %xmm2
testl %eax, %eax
je 0x36efa
divsd %xmm1, %xmm0
movapd %xmm0, %xmm2
movq 0x8(%rsp), %rdx
movsd %xmm2, (%rdx,%r15,8)
jle 0x36fb2
xorl %r14d, %r14d
movsd %xmm2, 0x20(%rsp)
movsd %xmm3, (%rsp)
movq (%rcx,%rbp), %rax
movq (%rax,%r14,8), %rax
movq (%rax), %rcx
movq 0x8(%rax), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rcx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rcx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rcx), %xmm2
movq %r13, %rdi
callq 0x83dc2
movq %r12, %rdi
movq %r13, %rsi
callq 0x4ff14
movsd 0x20(%rsp), %xmm2
movsd (%rsp), %xmm3
subsd %xmm2, %xmm0
mulsd %xmm0, %xmm0
addsd %xmm0, %xmm3
incq %r14
movq 0x68(%rbx), %rcx
movslq 0xc(%rcx,%rbp), %rax
cmpq %rax, %r14
jl 0x36f14
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
jmp 0x36fb2
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
xorpd %xmm3, %xmm3
xorpd %xmm0, %xmm0
je 0x36fa7
divsd %xmm1, %xmm0
movq 0x8(%rsp), %rcx
movsd %xmm0, (%rcx,%r15,8)
xorpd %xmm2, %xmm2
xorpd %xmm0, %xmm0
testl %eax, %eax
je 0x36fc6
divsd %xmm1, %xmm3
movapd %xmm3, %xmm0
ucomisd %xmm2, %xmm0
jb 0x36fd2
sqrtsd %xmm0, %xmm0
jmp 0x36fd7
callq 0x133c0
movq 0x10(%rsp), %rax
movsd %xmm0, (%rax,%r15,8)
incq %r15
movslq 0x18(%rbx), %rax
cmpq %rax, %r15
jl 0x36e15
testl %eax, %eax
jle 0x37139
xorl %r15d, %r15d
xorpd %xmm0, %xmm0
leaq 0x40(%rsp), %r13
movq 0x10(%rsp), %rcx
movsd (%rcx,%r15,8), %xmm1
ucomisd %xmm0, %xmm1
jne 0x3701d
jnp 0x3712a
movsd %xmm1, (%rsp)
movq 0x10(%rbx), %rax
movq %r15, %rbp
shlq $0x4, %rbp
movq (%rax,%rbp), %rcx
movq 0x8(%rax,%rbp), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rcx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rcx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rcx), %xmm2
leaq 0x28(%rsp), %rdi
callq 0x83dc2
movq 0x68(%rbx), %rax
cmpl $0x0, 0xc(%rax,%rbp)
jle 0x37123
addq %rbp, %rax
xorl %r12d, %r12d
movq (%rax), %rax
movslq %r12d, %r14
movq (%rax,%r14,8), %rax
movq (%rax), %rcx
movq 0x8(%rax), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rcx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rcx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rcx), %xmm2
movq %r13, %rdi
callq 0x83dc2
leaq 0x28(%rsp), %rdi
movq %r13, %rsi
callq 0x4ff14
movq 0x8(%rsp), %rax
subsd (%rax,%r15,8), %xmm0
divsd (%rsp), %xmm0
ucomisd 0x18(%rsp), %xmm0
jbe 0x3710d
movq 0x68(%rbx), %rax
movq (%rax,%rbp), %rcx
movslq 0xc(%rax,%rbp), %rax
movq -0x8(%rcx,%rax,8), %rax
movq %rax, (%rcx,%r14,8)
movq 0x68(%rbx), %rdi
movl 0xc(%rdi,%rbp), %esi
addq %rbp, %rdi
decl %esi
callq 0xa9ec8
decl %r14d
movl %r14d, %r12d
incl %r12d
movq 0x68(%rbx), %rcx
leaq (%rcx,%rbp), %rax
cmpl 0xc(%rcx,%rbp), %r12d
jl 0x3707b
movl 0x18(%rbx), %eax
xorpd %xmm0, %xmm0
incq %r15
movslq %eax, %rcx
cmpq %rcx, %r15
jl 0x37006
movq 0x8(%rsp), %rdi
callq 0x13680
movq 0x10(%rsp), %rdi
callq 0x13680
addq $0x58, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq