/
/
/
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.0005*Eigen::Matrix2d::Identity(); // static variable - has to be declared outside class!
95
96
97
98LandmarkXYYawMeasurement::LandmarkXYYawMeasurement(uint32_t identifier, Eigen::VectorXd xy_yaw_measurement):
99 Measurement(){
100 c_id_ = identifier;
101 measurement_ = xy_yaw_measurement;
102}
103
104Eigen::VectorXd LandmarkXYYawMeasurement::MeasurementModel(StateVector pose, Eigen::VectorXd landmark){
105 Eigen::Matrix3d T;
106 Eigen::Vector3d d;
107
108 d << landmark(0) - pose(0), landmark(1) - pose(1), landmark(2) - pose(2);
109
110 auto cos_p = std::cos(pose(2));
111 auto sin_p = std::sin(pose(2));
112
113 T << cos_p, sin_p, 0,
114 -sin_p, cos_p, 0,
115 0, 0, 1;
116
117 return T * d;
118}
119
120Eigen::VectorXd LandmarkXYYawMeasurement::inverseMeasurementModel(StateVector pose){
121 Eigen::Matrix3d T;
122
123 auto cos_p = std::cos(pose(2));
124 auto sin_p = std::sin(pose(2));
125
126 T << cos_p, -sin_p, 0,
127 sin_p, cos_p, 0,
128 0, 0, 1;
129
130 return T * measurement_ + pose;
131}
132
133
134Eigen::MatrixXd LandmarkXYYawMeasurement::calculateHs(StateVector pose, Eigen::VectorXd landmark){
135 Eigen::MatrixXd Hs(3, 3);
136
137 auto dx = landmark(0) - pose(0);
138 auto dy = landmark(1) - pose(1);
139 auto cos_p = std::cos(pose(2));
140 auto sin_p = std::sin(pose(2));
141
142 Hs << -cos_p, -sin_p, -dx*sin_p + dy*cos_p,
143 sin_p, -cos_p, -dx*cos_p - dy*sin_p,
144 0, 0, -1;
145
146 return Hs;
147}
148
149Eigen::MatrixXd LandmarkXYYawMeasurement::calculateHl(StateVector pose, Eigen::VectorXd landmark){
150 Eigen::MatrixXd Hl(3,3);
151
152 auto cos_p = std::cos(pose(2));
153 auto sin_p = std::sin(pose(2));
154
155 Hl << cos_p, sin_p, 0,
156 -sin_p, cos_p, 0,
157 0, 0, 1;
158
159 return Hl;
160}
161
162Eigen::MatrixXd LandmarkXYYawMeasurement::measurement_covariance_ = 0.05*Eigen::Matrix3d::Identity(); // static variable - has to be declared outside class!
163
164
165
166LandmarkLineMeasurement::LandmarkLineMeasurement(uint32_t identifier, Eigen::VectorXd line_measurement):
167 Measurement(){
168 c_id_ = identifier;
169 measurement_ = line_measurement;
170}
171
172Eigen::VectorXd LandmarkLineMeasurement::MeasurementModel(StateVector pose, Eigen::VectorXd landmark){
173 Eigen::Vector2d d;
174
175 d << landmark(0) + std::cos(landmark(1)) * pose(0) + std::sin(landmark(1)) * pose(1),
176 landmark(1) - pose(2);
177
178 return d;
179}
180
181Eigen::VectorXd LandmarkLineMeasurement::inverseMeasurementModel(StateVector pose){
182 Eigen::Vector2d d;
183
184 d << measurement_(0) + std::sin(measurement_(1) + pose(2)) * pose(0) - std::cos(measurement_(1) + pose(2)) * pose(1),
185 measurement_(1) + pose(2);
186
187 return d;
188}
189
190
191Eigen::MatrixXd LandmarkLineMeasurement::calculateHs(StateVector pose, Eigen::VectorXd landmark){
192 Eigen::MatrixXd Hs(2,3);
193
194 auto cos_p = std::cos(landmark(1));
195 auto sin_p = std::sin(landmark(1));
196
197 Hs << cos_p, sin_p, 0,
198 0, 0, -1;
199
200 return Hs;
201}
202
203Eigen::MatrixXd LandmarkLineMeasurement::calculateHl(StateVector pose, Eigen::VectorXd landmark){
204 Eigen::MatrixXd Hl(2,2);
205
206 auto cos_p = std::cos(landmark(1));
207 auto sin_p = std::sin(landmark(1));
208
209 Hl << 1, -sin_p * pose(0) - cos_p * pose(1),
210 0, 1;
211
212 return Hl;
213}
214
215Eigen::MatrixXd LandmarkLineMeasurement::measurement_covariance_ = 0.05*Eigen::Matrix2d::Identity(); // static variable - has to be declared outside class!
216
217
218} //namespace fastslam
219