1/**
2 * @file testBTransformBtwRobotsUnaryFactor.cpp
3 * @brief Unit test for the TransformBtwRobotsUnaryFactor
4 * @author Vadim Indelman
5 */
6
7#include <CppUnitLite/TestHarness.h>
8
9
10#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h>
11#include <gtsam/geometry/Pose2.h>
12#include <gtsam/nonlinear/Values.h>
13#include <gtsam/base/numericalDerivative.h>
14
15#include <gtsam/slam/BetweenFactor.h>
16
17#include <gtsam/nonlinear/NonlinearOptimizer.h>
18#include <gtsam/nonlinear/NonlinearFactorGraph.h>
19#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
20
21using namespace std::placeholders;
22using namespace std;
23using namespace gtsam;
24
25/* ************************************************************************* */
26Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor<gtsam::Pose2>& factor){
27 gtsam::Values values;
28 values.insert(j: key, val: org1_T_org2);
29 return factor.whitenedError(x: values);
30}
31
32/* ************************************************************************* */
33//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
34// gtsam::Values values;
35// values.insert(keyA, p1);
36// values.insert(keyB, p2);
37// // Vector err = factor.whitenedError(values);
38// // return err;
39// return Vector::Expmap(factor.whitenedError(values));
40//}
41
42/* ************************************************************************* */
43TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals)
44{
45 gtsam::Key key(0);
46 gtsam::Key keyA(1);
47 gtsam::Key keyB(2);
48
49 gtsam::Pose2 p1(10.0, 15.0, 0.1);
50 gtsam::Pose2 p2(15.0, 15.0, 0.3);
51 gtsam::Pose2 noise(0.5, 0.4, 0.01);
52 gtsam::Pose2 rel_pose_ideal = p1.between(g: p2);
53 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
54
55 SharedGaussian model(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
56
57 gtsam::Values valA, valB;
58 valA.insert(j: keyA, val: p1);
59 valB.insert(j: keyB, val: p2);
60
61 // Constructor
62 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
63 TransformBtwRobotsUnaryFactor<gtsam::Pose2> h(key, rel_pose_msr, keyA, keyB, valA, valB, model);
64
65 // Equals
66 CHECK(assert_equal(g, h, 1e-5));
67}
68
69/* ************************************************************************* */
70TEST( TransformBtwRobotsUnaryFactor, unwhitenedError)
71{
72 gtsam::Key key(0);
73 gtsam::Key keyA(1);
74 gtsam::Key keyB(2);
75
76 gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
77 gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
78
79 gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
80
81 gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(g: orgB_T_2);
82
83 gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(g: orgA_T_2);
84 gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
85
86 SharedGaussian model(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
87
88 gtsam::Values valA, valB;
89 valA.insert(j: keyA, val: orgA_T_1);
90 valB.insert(j: keyB, val: orgB_T_2);
91
92 // Constructor
93 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
94
95 gtsam::Values values;
96 values.insert(j: key, val: orgA_T_orgB);
97 Vector err = g.unwhitenedError(x: values);
98
99 // Equals
100 CHECK(assert_equal(err, (Vector) Z_3x1, 1e-5));
101}
102
103/* ************************************************************************* */
104TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2)
105{
106 gtsam::Key key(0);
107 gtsam::Key keyA(1);
108 gtsam::Key keyB(2);
109
110 gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
111 gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1);
112
113 gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0);
114
115 gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(g: orgB_T_currB);
116
117 gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(g: orgA_T_currB);
118 gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
119
120 SharedGaussian model(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
121
122 gtsam::Values valA, valB;
123 valA.insert(j: keyA, val: orgA_T_currA);
124 valB.insert(j: keyB, val: orgB_T_currB);
125
126 // Constructor
127 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
128
129 gtsam::Values values;
130 values.insert(j: key, val: orgA_T_orgB);
131 Vector err = g.unwhitenedError(x: values);
132
133 // Equals
134 CHECK(assert_equal(err, Z_3x1, 1e-5));
135}
136
137/* ************************************************************************* */
138TEST( TransformBtwRobotsUnaryFactor, Optimize)
139{
140 gtsam::Key key(0);
141 gtsam::Key keyA(1);
142 gtsam::Key keyB(2);
143
144 gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
145 gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05);
146
147 gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0);
148 gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(g: orgB_T_currB);
149 gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(g: orgA_T_currB_tr);
150
151 // some error in measurements
152 // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01));
153 // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01));
154 // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01));
155 // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01));
156
157 // ideal measurements
158 gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(g: gtsam::Pose2(0.0, 0.0, 0.0));
159 gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1;
160 gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1;
161 gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1;
162
163 SharedGaussian model(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
164
165 gtsam::Values valA, valB;
166 valA.insert(j: keyA, val: orgA_T_currA);
167 valB.insert(j: keyB, val: orgB_T_currB);
168
169 // Constructor
170 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, model);
171
172 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, model);
173
174 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, model);
175
176 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, model);
177
178 gtsam::Values values;
179 values.insert(j: key, val: gtsam::Pose2());
180
181 gtsam::NonlinearFactorGraph graph;
182 graph.push_back(factor: g1);
183 graph.push_back(factor: g2);
184 graph.push_back(factor: g3);
185 graph.push_back(factor: g4);
186
187 gtsam::GaussNewtonParams params;
188 gtsam::GaussNewtonOptimizer optimizer(graph, values, params);
189 gtsam::Values result = optimizer.optimize();
190
191 gtsam::Pose2 orgA_T_orgB_opt = result.at<gtsam::Pose2>(j: key);
192
193 CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5));
194}
195
196
197/* ************************************************************************* */
198TEST( TransformBtwRobotsUnaryFactor, Jacobian)
199{
200 gtsam::Key key(0);
201 gtsam::Key keyA(1);
202 gtsam::Key keyB(2);
203
204 gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
205 gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
206
207 gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
208
209 gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(g: orgB_T_2);
210
211 gtsam::Pose2 noise(0.5, 0.4, 0.01);
212
213 gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(g: orgA_T_2);
214 gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(g: noise);
215
216 SharedGaussian model(noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.05)));
217
218 gtsam::Values valA, valB;
219 valA.insert(j: keyA, val: orgA_T_1);
220 valB.insert(j: keyB, val: orgB_T_2);
221
222 // Constructor
223 TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
224
225 gtsam::Values values;
226 values.insert(j: key, val: orgA_T_orgB);
227
228 std::vector<gtsam::Matrix> H_actual(1);
229 Vector actual_err_wh = g.whitenedError(x: values, H&: H_actual);
230
231 Matrix H1_actual = H_actual[0];
232
233 double stepsize = 1.0e-9;
234 Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(h: std::bind(f: &predictionError, args: std::placeholders::_1, args&: key, args&: g), x: orgA_T_orgB, delta: stepsize);
235// CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
236}
237
238
239
240
241/////* ************************************************************************** */
242//TEST (TransformBtwRobotsUnaryFactor, jacobian ) {
243//
244// gtsam::Key keyA(1);
245// gtsam::Key keyB(2);
246//
247// // Inlier test
248// gtsam::Pose2 p1(10.0, 15.0, 0.1);
249// gtsam::Pose2 p2(15.0, 15.0, 0.3);
250// gtsam::Pose2 noise(0.5, 0.4, 0.01);
251// gtsam::Pose2 rel_pose_ideal = p1.between(p2);
252// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
253//
254// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
255// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
256//
257// gtsam::Values values;
258// values.insert(keyA, p1);
259// values.insert(keyB, p2);
260//
261// double prior_outlier = 0.0;
262// double prior_inlier = 1.0;
263//
264// TransformBtwRobotsUnaryFactor<gtsam::Pose2> f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier,
265// prior_inlier, prior_outlier);
266//
267// std::vector<gtsam::Matrix> H_actual(2);
268// Vector actual_err_wh = f.whitenedError(values, H_actual);
269//
270// Matrix H1_actual = H_actual[0];
271// Matrix H2_actual = H_actual[1];
272//
273// // compare to standard between factor
274// BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier );
275// Vector actual_err_wh_stnd = h.whitenedError(values);
276// Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
277// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
278// std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
279// (void)h.unwhitenedError(values, H_actual_stnd_unwh);
280// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
281// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
282// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
283// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
284//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8));
285//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
286//
287// double stepsize = 1.0e-9;
288// Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
289// Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
290//
291//
292// // try to check numerical derivatives of a standard between factor
293// Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
294// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
295//
296//
297// CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
298// CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
299//
300//}
301
302/* ************************************************************************* */
303 int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
304/* ************************************************************************* */
305