ACCIS/cross_cal.cpp at main · SIOSlab/ACCIS · 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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#include <cross_cal.hpp>

#include <angles.hpp>
#include <sat_state.hpp>

#include <opencv2/features2d.hpp>

#include <cmath>
#include <iostream>

filter::dist cross_cal::run(const transmission& query, filter::base& filt) {

    using namespace cv;

    using namespace sifter;

    filter::dist dist_x = query.dist_x;

    meas h;

    h.cam = cam;

    for (const transmission& tr : train) {

        double dt = query.t - tr.t;

        sat_state sq, st;
        sq.X = query.dist_x.mean;
        st.X = tr.dist_x.mean;

        if (dt > 0 && dt < dt_max && query.sat_id != tr.sat_id) {

            matches smatch = match(query.sift, tr.sift, cam, max_dist);

            if (smatch.num_pts > 0) {

                std::cout << "Cross-calibrating Sat " << query.sat_id << " vs. "
                          << tr.sat_id << " with " << smatch.num_pts
                          << " points" << std::endl;

                filter::dist dist_xx = filt.join(dist_x, tr.dist_x);

                h.tr = tr.t;

                for (int i = 0; i < smatch.num_pts; i++) {

                    h.zr = smatch.train[i];

                    vec<4> z = smatch.query[i];

                    dist_xx = filt.update(query.t, z, dist_xx, dist_w_kp, h);

                }

                dist_x = filt.marginal(dist_xx, 0, sat_state::N);

            }

        }

    }

    return dist_x;

}

vec<> cross_cal::meas::h(double t, cvec<> x, cvec<> w) {

    sat_state xr, xc;
    xc.X = x.head<sat_state::N>();
    xr.X = x.tail<sat_state::N>();

    return cross_cal_meas(t, tr, xc, xr, cam, zr) + w;

}

vec<4> cross_cal_meas(double tc, double tr, const sat_state& xc,
        const sat_state& xr, sat_cam& cam, cvec<4> zr) {

    vec<2> pixr1, pixr2;
    pixr1 = zr.head<2>();
    pixr2 = zr.tail<2>();

    vec<2> ll1, ll2;
    ll1 = cam.pix2latlon(tr, xr, cam.undistort(xr, pixr1));
    ll2 = cam.pix2latlon(tr, xr, cam.undistort(xr, pixr2));

    vec<2> pixc1, pixc2;
    pixc1 = cam.distort(xc, cam.latlon2pix(tc, xc, ll1));
    pixc2 = cam.distort(xc, cam.latlon2pix(tc, xc, ll2));

    vec<4> zc;
    zc.head<2>() = pixc1;
    zc.tail<2>() = pixc2;

    return zc;

}