9 #include <boost/optional/optional_io.hpp>
10 #include <boost/test/unit_test.hpp>
39 auto plane1 = Surface::makeShared<PlaneSurface>(Vector3D::UnitX() * 1,
41 auto plane2 = Surface::makeShared<PlaneSurface>(Vector3D::UnitX() * 2,
43 auto plane3 = Surface::makeShared<PlaneSurface>(Vector3D::UnitX() * 3,
47 cov << 0.04, 0, 0, 0.1;
50 plane1, {}, std::move(cov), -0.1, 0.45));
52 cov << 0.04, 0, 0, 0.1;
55 plane2, {}, std::move(cov), -0.2, 0.35));
57 cov << 0.04, 0, 0, 0.1;
60 plane3, {}, std::move(cov), -0.05, 0.25));
67 auto ts = traj.getTrackState(ts_idx);
68 ts.setReferenceSurface(plane1);
73 covTrk.diagonal() << 0.08, 0.3, 1, 1, 1, 1;
75 parValues << 0.3, 0.5, 0.5 *
M_PI, 0., 1 / 100., 0.;
77 ts.predicted() = parValues;
78 ts.predictedCovariance() = covTrk;
80 parValues << 0.301, 0.503, 0.5 *
M_PI, 0., 1 / 100., 0.;
82 ts.filtered() = parValues;
83 ts.filteredCovariance() = covTrk;
85 ts.jacobian().setIdentity();
88 ts = traj.getTrackState(ts_idx);
89 ts.setReferenceSurface(plane2);
91 parValues << 0.2, 0.5, 0.5 *
M_PI, 0., 1 / 100., 0.;
92 ts.predicted() = parValues;
93 ts.predictedCovariance() = covTrk;
95 parValues << 0.27, 0.53, 0.5 *
M_PI, 0., 1 / 100., 0.;
96 ts.filtered() = parValues;
97 ts.filteredCovariance() = covTrk;
99 ts.jacobian().setIdentity();
102 ts = traj.getTrackState(ts_idx);
103 ts.setReferenceSurface(plane3);
105 parValues << 0.35, 0.49, 0.5 *
M_PI, 0., 1 / 100., 0.;
106 ts.predicted() = parValues;
107 ts.predictedCovariance() = covTrk;
109 parValues << 0.33, 0.43, 0.5 *
M_PI, 0., 1 / 100., 0.;
110 ts.filtered() = parValues;
111 ts.filteredCovariance() = covTrk;
112 ts.pathLength() = 3.;
113 ts.jacobian().setIdentity();
118 BOOST_CHECK(gms(
tgContext, traj, ts_idx).ok());
123 auto ts1 = traj.getTrackState(0);
124 BOOST_CHECK(ts1.hasSmoothed());
125 BOOST_CHECK_NE(ts1.filtered(), ts1.smoothed());
130 expPars << 0.3510000, 0.4730000, 1.5707963, 0.0000000, 0.0100000, 0.0000000;
133 expCov.setIdentity();
134 expCov.diagonal() << 0.0800000, 0.3000000, 1.0000000, 1.0000000, 1.0000000,
138 auto ts2 = traj.getTrackState(1);
139 BOOST_CHECK(ts2.hasSmoothed());
140 BOOST_CHECK_NE(ts2.filtered(), ts2.smoothed());
142 expPars << 0.2500000, 0.4700000, 1.5707963, 0.0000000, 0.0100000, 0.0000000;
146 auto ts3 = traj.getTrackState(2);
147 BOOST_CHECK(ts3.hasSmoothed());
149 BOOST_CHECK_EQUAL(ts3.filtered(), ts3.smoothed());
151 expPars << 0.3300000, 0.4300000, 1.5707963, 0.0000000, 0.0100000, 0.0000000;