/
/
/
1#include "fast_slam/Measurement.hpp"
2
3namespace fastslam{
4
5GOTMeasurement::GOTMeasurement(uint32_t identifier, Eigen::VectorXd GOT_meas):
6 Measurement(){
7 c_id_ = identifier;
8 measurement_ = GOT_meas;
9}
10
11Eigen::VectorXd GOTMeasurement::MeasurementModel(StateVector pose, Eigen::VectorXd landmark){
12 return pose - landmark;
13}
14
15Eigen::VectorXd GOTMeasurement::inverseMeasurementModel(StateVector pose){
16 return pose - measurement_;
17}
18
19Eigen::MatrixXd GOTMeasurement::calculateHs(StateVector pose, Eigen::VectorXd landmark){
20 return Eigen::Matrix3d::Identity();
21}
22
23Eigen::MatrixXd GOTMeasurement::calculateHl(StateVector pose, Eigen::VectorXd landmark){
24 return -1.0*Eigen::Matrix3d::Identity();
25};
26
27Eigen::MatrixXd GOTMeasurement::measurement_covariance_ = 0.05*Eigen::Matrix3d::Identity(); // static variable - has to be declared outside class!
28
29
30
31LandmarkXYMeasurement::LandmarkXYMeasurement(uint32_t identifier, Eigen::VectorXd xy_measurement):
32 Measurement(){
33 c_id_ = identifier;
34 measurement_ = xy_measurement;
35}
36
37Eigen::VectorXd LandmarkXYMeasurement::MeasurementModel(StateVector pose, Eigen::VectorXd landmark){
38 Eigen::Matrix2d T;
39 Eigen::Vector2d d;
40
41 d << landmark(0) - pose(0), landmark(1) - pose(1);
42
43 auto cos_p = std::cos(pose(2));
44 auto sin_p = std::sin(pose(2));
45
46 T << cos_p, sin_p,
47 -sin_p, cos_p;
48
49 return T * d;
50}
51
52Eigen::VectorXd LandmarkXYMeasurement::inverseMeasurementModel(StateVector pose){
53 Eigen::Matrix2d T;
54 Eigen::Vector2d d;
55
56 d << pose(0), pose(1);
57
58 auto cos_p = std::cos(pose(2));
59 auto sin_p = std::sin(pose(2));
60
61 T << cos_p, -sin_p,
62 sin_p, cos_p;
63
64 return T * measurement_ + d;
65}
66
67
68Eigen::MatrixXd LandmarkXYMeasurement::calculateHs(StateVector pose, Eigen::VectorXd landmark){
69 Eigen::MatrixXd Hs(2, 3);
70
71 auto dx = landmark(0) - pose(0);
72 auto dy = landmark(1) - pose(1);
73 auto cos_p = std::cos(pose(2));
74 auto sin_p = std::sin(pose(2));
75
76 Hs << -cos_p, -sin_p, -dx*sin_p + dy*cos_p,
77 sin_p, -cos_p, -dx*cos_p - dy*sin_p;
78
79 return Hs;
80}
81
82Eigen::MatrixXd LandmarkXYMeasurement::calculateHl(StateVector pose, Eigen::VectorXd landmark){
83 Eigen::MatrixXd Hl(2,2);
84
85 auto cos_p = std::cos(pose(2));
86 auto sin_p = std::sin(pose(2));
87
88 Hl << cos_p, sin_p,
89 -sin_p, cos_p;
90
91 return Hl;
92}
93
94Eigen::MatrixXd LandmarkXYMeasurement::measurement_covariance_ = 0.05*Eigen::Matrix2d::Identity(); // static variable - has to be declared outside class!
95
96
97
98
99
100LandmarkXYYawMeasurement::LandmarkXYYawMeasurement(uint32_t identifier, Eigen::VectorXd xy_yaw_measurement):
101 Measurement(){
102 c_id_ = identifier;
103 measurement_ = xy_yaw_measurement;
104}
105
106Eigen::VectorXd LandmarkXYYawMeasurement::MeasurementModel(StateVector pose, Eigen::VectorXd landmark){
107 Eigen::Matrix3d T;
108 Eigen::Vector3d d;
109
110 d << landmark(0) - pose(0), landmark(1) - pose(1), landmark(2) - pose(2);
111
112 auto cos_p = std::cos(pose(2));
113 auto sin_p = std::sin(pose(2));
114
115 T << cos_p, sin_p, 0,
116 -sin_p, cos_p, 0,
117 0, 0, 1;
118
119 return T * d;
120}
121
122Eigen::VectorXd LandmarkXYYawMeasurement::inverseMeasurementModel(StateVector pose){
123 Eigen::Matrix3d T;
124
125 auto cos_p = std::cos(pose(2));
126 auto sin_p = std::sin(pose(2));
127
128 T << cos_p, -sin_p, 0,
129 sin_p, cos_p, 0,
130 0, 0, 1;
131
132 return T * measurement_ + pose;
133}
134
135
136Eigen::MatrixXd LandmarkXYYawMeasurement::calculateHs(StateVector pose, Eigen::VectorXd landmark){
137 Eigen::MatrixXd Hs(3, 3);
138
139 auto dx = landmark(0) - pose(0);
140 auto dy = landmark(1) - pose(1);
141 auto cos_p = std::cos(pose(2));
142 auto sin_p = std::sin(pose(2));
143
144 Hs << -cos_p, -sin_p, -dx*sin_p + dy*cos_p,
145 sin_p, -cos_p, -dx*cos_p - dy*sin_p,
146 0, 0, -1;
147
148 return Hs;
149}
150
151Eigen::MatrixXd LandmarkXYYawMeasurement::calculateHl(StateVector pose, Eigen::VectorXd landmark){
152 Eigen::MatrixXd Hl(3,3);
153
154 auto cos_p = std::cos(pose(2));
155 auto sin_p = std::sin(pose(2));
156
157 Hl << cos_p, sin_p, 0,
158 -sin_p, cos_p, 0,
159 0, 0, 1;
160
161 return Hl;
162}
163
164Eigen::MatrixXd LandmarkXYYawMeasurement::measurement_covariance_ = 0.05*Eigen::Matrix3d::Identity(); // static variable - has to be declared outside class!
165
166
167} //namespace fastslam
168