1package main
2
3import (
4 "fmt"
5 "math"
6 "math/rand/v2"
7 "os"
8
9 "github.com/gofiber/fiber/v2/log"
10 "gonum.org/v1/gonum/optimize"
11)
12
13type Detector struct {
14 Name string
15 End float64 // samples
16 Position [3]float64 // m
17 EventEpochs []float64 // samples
18 ClockSpeedMult float64
19}
20
21func main() {
22 samplerate := 44100. // Hz
23 start := 2106421. // samples
24 sos := 347.99 // m/s
25 // Detector 1 is reference clock
26 data := []Detector{{
27 Name: "Computer",
28 End: 7134747,
29 Position: [3]float64{-0.817, -0.701, 1.343},
30 EventEpochs: []float64{
31 5226608,
32 5408665,
33 5599824,
34 5759440,
35 5879540,
36 },
37 }, {
38 Name: "Xiaomi Phone",
39 End: 7134723,
40 Position: [3]float64{-0.964, 0.394, 0.031},
41 EventEpochs: []float64{
42 5226652,
43 5408683,
44 5599858,
45 5759346,
46 5879681,
47 },
48 }, {
49 Name: "Google Pixel Phone",
50 End: 7134793,
51 Position: [3]float64{1.113, -0.722, 1.001},
52 EventEpochs: []float64{
53 5226677,
54 5408768,
55 5599835,
56 5759503,
57 5879506,
58 },
59 }, {
60 Name: "Camera",
61 End: 7134678,
62 Position: [3]float64{1.157, 0.548, 0.037},
63 EventEpochs: []float64{
64 5226654,
65 5408705,
66 5599821,
67 5759352,
68 5879593,
69 },
70 },
71 }
72
73 // Compute clock speed multiplier
74 for detector_index := range data {
75 data[detector_index].ClockSpeedMult = (data[detector_index].End - start) / (data[0].End - start) // How much to divide the time since start by to get the same sample count as the reference clock
76 }
77
78 realEventPositions := [][3]float64{{-0.229, -0.2942, 1.1319}, {-0.249, 0.4158, 1.0519}, {0.178, -0.4332, 0.8139}, {0.087, 0.5478, 0.3449}, {0.601, -0.8742, 1.9019}}
79
80 for event_index := range data[0].EventEpochs {
81 problem := optimize.Problem{
82 Func: func(x []float64) float64 {
83 guessedX := x[0]
84 guessedY := x[1]
85 guessedZ := x[2]
86 guessedEpoch := x[3] // samples
87 errorSum := 0.
88 for detector_index := range data {
89 distance := math.Sqrt((guessedX-data[detector_index].Position[0])*(guessedX-data[detector_index].Position[0]) + (guessedY-data[detector_index].Position[1])*(guessedY-data[detector_index].Position[1]) + (guessedZ-data[detector_index].Position[2])*(guessedZ-data[detector_index].Position[2]))
90 expectedEpoch := guessedEpoch + (distance/sos)*samplerate
91 offset := ((data[detector_index].EventEpochs[event_index]-start)/data[detector_index].ClockSpeedMult + start - expectedEpoch)
92 errorSum += offset * offset
93 }
94 return errorSum
95 },
96 }
97 result, err := optimize.Minimize(problem, []float64{0, 0, 0, data[0].EventEpochs[event_index]}, nil, &optimize.NelderMead{})
98 if err != nil {
99 log.Error(err)
100 }
101 fmt.Printf("[Event %d]: (%0.4f m, %0.4f m, %0.4f m, %0.4f s) | Error: %0.4fm\n", event_index, result.X[0], result.X[1], result.X[2], result.X[3]/samplerate, math.Sqrt((realEventPositions[event_index][0]-result.X[0])*(realEventPositions[event_index][0]-result.X[0])+(realEventPositions[event_index][1]-result.X[1])*(realEventPositions[event_index][1]-result.X[1])+(realEventPositions[event_index][2]-result.X[2])*(realEventPositions[event_index][2]-result.X[2])))
102 }
103
104 expected_error_samples_sd_per_detector := 2. // to be multiplied by *sqrt(3) when doing the uniform distrib
105 montecarlosims := 1000
106 stepsPerDirection := 10
107 max_error_exposure := 1. // m (for point cloud vis)
108 boundsX := []float64{-1, 1}
109 boundsY := []float64{-1, 1}
110 boundsZ := []float64{0, 2}
111 resStr := fmt.Sprintf(`ply
112format ascii 1.0
113element vertex %d
114property float x
115property float y
116property float z
117property uchar red
118property uchar green
119property uchar blue
120end_header
121`, stepsPerDirection*stepsPerDirection*stepsPerDirection)
122
123 for i := range stepsPerDirection {
124 for j := range stepsPerDirection {
125 fmt.Printf("%.1f %%\n", 100*float64(i)/float64(stepsPerDirection)+100*float64(j)/float64(stepsPerDirection)/float64(stepsPerDirection))
126 for k := range stepsPerDirection {
127 S := 0.
128 realPos := []float64{boundsX[0] + float64(i)/float64(stepsPerDirection-1)*(boundsX[1]-boundsX[0]), boundsY[0] + float64(j)/float64(stepsPerDirection-1)*(boundsY[1]-boundsY[0]), boundsZ[0] + float64(k)/float64(stepsPerDirection-1)*(boundsZ[1]-boundsZ[0])}
129 epochs := []float64{}
130
131 for detector_index := range data {
132 distance := math.Sqrt((realPos[0]-data[detector_index].Position[0])*(realPos[0]-data[detector_index].Position[0]) + (realPos[1]-data[detector_index].Position[1])*(realPos[1]-data[detector_index].Position[1]) + (realPos[2]-data[detector_index].Position[2])*(realPos[2]-data[detector_index].Position[2]))
133 expectedEpoch := (distance/sos)*samplerate + 2*(rand.Float64()-0.5)*expected_error_samples_sd_per_detector*math.Sqrt(3)
134 epochs = append(epochs, expectedEpoch)
135 }
136
137 for range montecarlosims {
138 problem := optimize.Problem{
139 Func: func(x []float64) float64 {
140 guessedX := x[0]
141 guessedY := x[1]
142 guessedZ := x[2]
143 guessedEpoch := x[3] // samples
144 errorSum := 0.
145 for detector_index := range data {
146 distance := math.Sqrt((guessedX-data[detector_index].Position[0])*(guessedX-data[detector_index].Position[0]) + (guessedY-data[detector_index].Position[1])*(guessedY-data[detector_index].Position[1]) + (guessedZ-data[detector_index].Position[2])*(guessedZ-data[detector_index].Position[2]))
147 expectedEpoch := guessedEpoch + (distance/sos)*samplerate
148 offset := (epochs[detector_index] - expectedEpoch)
149 errorSum += offset * offset
150 }
151 return errorSum
152 },
153 }
154 result, err := optimize.Minimize(problem, []float64{realPos[0], realPos[1], realPos[2], 0}, nil, &optimize.NelderMead{})
155 if err != nil {
156 log.Error(err)
157 }
158 S += math.Sqrt((realPos[0]-result.X[0])*(realPos[0]-result.X[0]) + (realPos[1]-result.X[1])*(realPos[1]-result.X[1]) + (realPos[2]-result.X[2])*(realPos[2]-result.X[2]))
159 }
160 avg_error := S / float64(montecarlosims)
161 color := int(min(avg_error/max_error_exposure*255., 255))
162 resStr += fmt.Sprintf("%.6f %.6f %.6f %d %d %d\n", realPos[0], realPos[1], realPos[2], color, color, color)
163 }
164 }
165 }
166 file, err := os.Create("error.ply") // Usage: https://blender.stackexchange.com/questions/310858/how-to-visualize-point-cloud-colors-in-blender-4-0-after-ply-data-import
167 if err != nil {
168 log.Fatal(err)
169 }
170 defer file.Close()
171
172 file.WriteString(resStr)
173}