testTransformBtwRobotsUnaryFactorEM.cpp
Go to the documentation of this file.
1 
7 #include <CppUnitLite/TestHarness.h>
8 
9 
10 #include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.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 
21 using namespace std::placeholders;
22 using namespace std;
23 using namespace gtsam;
24 
25 /* ************************************************************************* */
26 Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM<gtsam::Pose2>& factor){
27  gtsam::Values values;
28  values.insert(key, org1_T_org2);
29  return factor.whitenedError(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 /* ************************************************************************* */
43 TEST( TransformBtwRobotsUnaryFactorEM, 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(p2);
53  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
54 
55  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
56  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0)));
57 
58  double prior_outlier = 0.5;
59  double prior_inlier = 0.5;
60 
61  gtsam::Values valA, valB;
62  valA.insert(keyA, p1);
63  valB.insert(keyB, p2);
64 
65  // Constructor
66  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
67  model_inlier, model_outlier,prior_inlier, prior_outlier);
68  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> h(key, rel_pose_msr, keyA, keyB, valA, valB,
69  model_inlier, model_outlier,prior_inlier, prior_outlier);
70 
71  // Equals
72  CHECK(assert_equal(g, h, 1e-5));
73 }
74 
75 /* ************************************************************************* */
76 TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError)
77 {
78  gtsam::Key key(0);
79  gtsam::Key keyA(1);
80  gtsam::Key keyB(2);
81 
82  gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
83  gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
84 
85  gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
86 
87  gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
88 
89  gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
90  gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
91 
92  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
93  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0)));
94 
95  double prior_outlier = 0.01;
96  double prior_inlier = 0.99;
97 
98  gtsam::Values valA, valB;
99  valA.insert(keyA, orgA_T_1);
100  valB.insert(keyB, orgB_T_2);
101 
102  // Constructor
103  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
104  model_inlier, model_outlier,prior_inlier, prior_outlier);
105 
106  gtsam::Values values;
107  values.insert(key, orgA_T_orgB);
108  Vector err = g.unwhitenedError(values);
109 
110  // Equals
111  CHECK(assert_equal(err, Z_3x1, 1e-5));
112 }
113 
114 /* ************************************************************************* */
115 TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2)
116 {
117  gtsam::Key key(0);
118  gtsam::Key keyA(1);
119  gtsam::Key keyB(2);
120 
121  gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
122  gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1);
123 
124  gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0);
125 
126  gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
127 
128  gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB);
129  gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
130 
131  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
132  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0)));
133 
134  double prior_outlier = 0.01;
135  double prior_inlier = 0.99;
136 
137  gtsam::Values valA, valB;
138  valA.insert(keyA, orgA_T_currA);
139  valB.insert(keyB, orgB_T_currB);
140 
141  // Constructor
142  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
143  model_inlier, model_outlier,prior_inlier, prior_outlier);
144 
145  gtsam::Values values;
146  values.insert(key, orgA_T_orgB);
147  Vector err = g.unwhitenedError(values);
148 
149  // Equals
150  CHECK(assert_equal(err, Z_3x1, 1e-5));
151 }
152 
153 /* ************************************************************************* */
154 TEST( TransformBtwRobotsUnaryFactorEM, Optimize)
155 {
156  gtsam::Key key(0);
157  gtsam::Key keyA(1);
158  gtsam::Key keyB(2);
159 
160  gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
161  gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05);
162 
163  gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0);
164  gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB);
165  gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr);
166 
167  // some error in measurements
168  // gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01));
169  // gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01));
170  // gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01));
171  // gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01));
172 
173  // ideal measurements
174  gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0));
175  gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1;
176  gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1;
177  gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1;
178 
179  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
180  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0)));
181 
182  double prior_outlier = 0.01;
183  double prior_inlier = 0.99;
184 
185  gtsam::Values valA, valB;
186  valA.insert(keyA, orgA_T_currA);
187  valB.insert(keyB, orgB_T_currB);
188 
189  // Constructor
190  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB,
191  model_inlier, model_outlier,prior_inlier, prior_outlier);
192 
193  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB,
194  model_inlier, model_outlier,prior_inlier, prior_outlier);
195 
196  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB,
197  model_inlier, model_outlier,prior_inlier, prior_outlier);
198 
199  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB,
200  model_inlier, model_outlier,prior_inlier, prior_outlier);
201 
202  gtsam::Values values;
203  values.insert(key, gtsam::Pose2());
204 
205  gtsam::NonlinearFactorGraph graph;
206  graph.push_back(g1);
207  graph.push_back(g2);
208  graph.push_back(g3);
209  graph.push_back(g4);
210 
211  gtsam::GaussNewtonParams params;
212  gtsam::GaussNewtonOptimizer optimizer(graph, values, params);
213  gtsam::Values result = optimizer.optimize();
214 
215  gtsam::Pose2 orgA_T_orgB_opt = result.at<gtsam::Pose2>(key);
216 
217  CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5));
218 }
219 
220 
221 /* ************************************************************************* */
222 TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
223 {
224  gtsam::Key key(0);
225  gtsam::Key keyA(1);
226  gtsam::Key keyB(2);
227 
228  gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
229  gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
230 
231  gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
232 
233  gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
234 
235  gtsam::Pose2 noise(0.5, 0.4, 0.01);
236 
237  gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
238  gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
239 
240  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05)));
241  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0)));
242 
243  double prior_outlier = 0.5;
244  double prior_inlier = 0.5;
245 
246  gtsam::Values valA, valB;
247  valA.insert(keyA, orgA_T_1);
248  valB.insert(keyB, orgB_T_2);
249 
250  // Constructor
251  TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB,
252  model_inlier, model_outlier,prior_inlier, prior_outlier);
253 
254  gtsam::Values values;
255  values.insert(key, orgA_T_orgB);
256 
257  std::vector<gtsam::Matrix> H_actual(1);
258  Vector actual_err_wh = g.whitenedError(values, H_actual);
259 
260  Matrix H1_actual = H_actual[0];
261 
262  double stepsize = 1.0e-9;
263  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(
264  std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB,
265  stepsize);
266  // CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
267 }
269 //TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) {
270 //
271 // gtsam::Key keyA(1);
272 // gtsam::Key keyB(2);
273 //
274 // // Inlier test
275 // gtsam::Pose2 p1(10.0, 15.0, 0.1);
276 // gtsam::Pose2 p2(15.0, 15.0, 0.3);
277 // gtsam::Pose2 noise(0.5, 0.4, 0.01);
278 // gtsam::Pose2 rel_pose_ideal = p1.between(p2);
279 // gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
280 //
281 // SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05)));
282 // SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0)));
283 //
284 // gtsam::Values values;
285 // values.insert(keyA, p1);
286 // values.insert(keyB, p2);
287 //
288 // double prior_outlier = 0.0;
289 // double prior_inlier = 1.0;
290 //
291 // TransformBtwRobotsUnaryFactorEM<gtsam::Pose2> f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier,
292 // prior_inlier, prior_outlier);
293 //
294 // std::vector<gtsam::Matrix> H_actual(2);
295 // Vector actual_err_wh = f.whitenedError(values, H_actual);
296 //
297 // Matrix H1_actual = H_actual[0];
298 // Matrix H2_actual = H_actual[1];
299 //
300 // // compare to standard between factor
301 // BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier );
302 // Vector actual_err_wh_stnd = h.whitenedError(values);
303 // Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
304 // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
305 // std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
306 // (void)h.unwhitenedError(values, H_actual_stnd_unwh);
307 // Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
308 // Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
309 // Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
310 // Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
313 //
314 // double stepsize = 1.0e-9;
315 // Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize);
316 // Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, std::placeholders::_1, keyA, keyB, f), p2, stepsize);
317 //
318 //
319 // // try to check numerical derivatives of a standard between factor
320 // Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize);
321 // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
322 //
323 //
324 // CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
325 // CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
326 //
327 //}
328 
329 /* ************************************************************************* */
330  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
331 /* ************************************************************************* */
key
const gtsam::Symbol key('X', 0)
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
Values.h
A non-templated config holding any types of Manifold-group elements.
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:204
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
TEST
TEST(TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals)
Definition: testTransformBtwRobotsUnaryFactorEM.cpp:43
leaf::values
leaf::MyValues values
std
Definition: BFloat16.h:88
numericalDerivative.h
Some functions to compute numerical derivatives.
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::TransformBtwRobotsUnaryFactorEM::whitenedError
Vector whitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const
Definition: TransformBtwRobotsUnaryFactorEM.h:185
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
TransformBtwRobotsUnaryFactorEM.h
Unary factor for determining transformation between given trajectories of two robots.
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam
traits
Definition: chartTesting.h:28
h
const double h
Definition: testSimpleHelicopter.cpp:19
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Pose2.h
2D Pose
g2
Pose3 g2(g1.expmap(h *V1_g1))
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
predictionError
Vector predictionError(const Pose2 &org1_T_org2, const gtsam::Key &key, const TransformBtwRobotsUnaryFactorEM< gtsam::Pose2 > &factor)
Definition: testTransformBtwRobotsUnaryFactorEM.cpp:26
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:742

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:46