GPRMapping/mComputing.py at main · EasternBoy/GPRMapping · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
import mRobot
import random
import torch
import gpytorch
import numpy as np

def update(robo, model, llh):
    N = len(robo)

    x_min = robo[0].pBnd.x_min
    x_max = robo[0].pBnd.x_max
    for i in range(N):
        model[i].eval()
        llh[i].eval()
        with torch.no_grad(), gpytorch.settings.fast_computations(log_prob=False, covar_root_decomposition=False):
            point =  torch.tensor([[random.uniform(x_min, x_max), random.uniform(x_min, x_max)]])
            llh[i](model[i](point))

    neighbors = mRobot.find_nears(robo)

    for i in range(N):
        neighbors[i].append(i)
        for j in neighbors[i]:
                npos = torch.tensor([robo[j].cpos], dtype=torch.double)
                nmea = robo[j].cmea
                model[j].get_fantasy_model(npos, nmea)

    return 0

def rand_position(robo):
    x_min = robo[0].pBnd.x_min
    x_max = robo[0].pBnd.x_max
    y_min = robo[0].pBnd.y_min
    y_max = robo[0].pBnd.y_max

    s_max = robo[0].pBnd.s_max

    N = len(robo)
    R = robo[0].R
    r = robo[0].r

    flag = True

    while flag:
        s = [[random.uniform(robo[i].cpos[0] - s_max, robo[i].cpos[0] + s_max), random.uniform(robo[i].cpos[1] - s_max, robo[i].cpos[1] + s_max)] for i in range(N)]

        flag = False

        for i in range(N):
            if s[i][0] < x_min or s[i][0] > x_max or s[i][1] < y_min or s[i][1] > y_max:
                flag = True

        # test = mRobot.check_nears(s, R, r, N)

        # if test:
        #     for i in range(N):
        #         robo[i].cpos = s[i]
        # else:
        #     flag = True


def proxADMM():

    return 0