transform_var_test.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
4 
8 
9 static tf2::Quaternion quaternionfromrpy(double roll, double pitch, double yaw) {
11  q.setRPY(roll, pitch, yaw);
12  return q;
13 }
14 
15 TEST (TransformWithVariance, simple_fusion) {
16  auto t1 = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0));
17  auto t2 = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.1,0,0));
18 
19  auto tv1 = TransformWithVariance(t1, 0.3);
20  auto tv2 = TransformWithVariance(t2, 0.3);
21 
22  auto out_tv = averageTransforms(tv1, tv2);
23 
24  // Make sure that the new position is between the originals
25  ASSERT_GT(out_tv.transform.getOrigin().x(), 0);
26  ASSERT_LT(out_tv.transform.getOrigin().x(), 0.1);
27 
28  // The new variance should be less than the origial, but non negative
29  ASSERT_LT(out_tv.variance, 0.3);
30  ASSERT_GT(out_tv.variance, 0);
31 }
32 
33 TEST (TransformWithVariance, simple_rotation_fusion) {
34  auto t1 = tf2::Transform(quaternionfromrpy(0,0,0), tf2::Vector3(0,0,0));
35  auto t2 = tf2::Transform(quaternionfromrpy(0.1,0,0), tf2::Vector3(0,0,0));
36 
37  auto tv1 = TransformWithVariance(t1, 0.3);
38  auto tv2 = TransformWithVariance(t2, 0.3);
39 
40  auto out_tv = averageTransforms(tv1, tv2);
41 
42  // Make sure that the new position is between the originals
43  ASSERT_GT(out_tv.transform.getRotation().getAngle(), 0);
44  ASSERT_LT(out_tv.transform.getRotation().getAngle(), 0.1);
45 
46  // The new variance should be less than the origial, but non negative
47  ASSERT_LT(out_tv.variance, 0.3);
48  ASSERT_GT(out_tv.variance, 0);
49 }
50 
51 TEST (TransformWithVariance, same_fusion_iterative) {
52  auto t1 = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0));
53  auto t2 = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0));
54 
55  auto tv1 = TransformWithVariance(t1, 0.3);
56  auto tv2 = TransformWithVariance(t2, 0.3);
57 
58  auto out_tv = averageTransforms(tv1, tv2);
59 
60  // Make sure that the new position is between the originals
61  ASSERT_DOUBLE_EQ(out_tv.transform.getOrigin().x(), 0);
62 
63  // The new variance should be less than the origial, but non negative
64  ASSERT_LT(out_tv.variance, 0.3);
65  ASSERT_GT(out_tv.variance, 0);
66 
67  // Ensure no catastrophic effects when fusing over and over
68  for (int i = 0; i < 10000; ++i) {
69  out_tv.update(tv2);
70  // Make sure that the new position is between the originals
71  ASSERT_DOUBLE_EQ(out_tv.transform.getOrigin().x(), 0);
72 
73  // The new variance should be less than the origial, but non negative
74  ASSERT_LT(out_tv.variance, 0.3);
75  ASSERT_GT(out_tv.variance, 1e-9);
76  }
77 }
78 
79 TEST (TransformWithVariance, outlier_with_large_variance) {
80  auto t1 = tf2::Transform(quaternionfromrpy(0,0,0), tf2::Vector3(0,0,0));
81  auto t2 = tf2::Transform(quaternionfromrpy(0,0,0), tf2::Vector3(0.1,0,0));
82  auto t3 = tf2::Transform(quaternionfromrpy(0,0,0), tf2::Vector3(0.1,0,0));
83  auto t4 = tf2::Transform(quaternionfromrpy(0,1,0), tf2::Vector3(1.0,0,0));
84 
85  auto tv1 = TransformWithVariance(t1, 0.2);
86  auto tv2 = TransformWithVariance(t2, 0.2);
87  auto tv3 = TransformWithVariance(t3, 0.2);
88  auto tv4 = TransformWithVariance(t4, 2.0);
89 
90  auto out_tv = averageTransforms(tv1, tv2);
91  out_tv = averageTransforms(out_tv, tv3);
92  out_tv = averageTransforms(out_tv, tv4);
93 
94  // Make sure that the new position is between the originals
95  ASSERT_GT(out_tv.transform.getOrigin().x(), 0);
96  ASSERT_LT(out_tv.transform.getOrigin().x(), 1.0);
97  ASSERT_GT(out_tv.transform.getRotation().getAngle(), 0);
98  ASSERT_LT(out_tv.transform.getRotation().getAngle(), 1.0);
99 
100  // The new variance shouldn't be large
101  ASSERT_LT(out_tv.variance, 1.0);
102  ASSERT_GT(out_tv.variance, 0);
103 
104  // The new mean shouldn't be affected much by the outlier
105  ASSERT_NEAR(out_tv.transform.getOrigin().x(), 0.1, 0.05);
106  ASSERT_NEAR(out_tv.transform.getRotation().getAngle(), 0, 0.1);
107 }
108 
109 TEST (TransformWithVariance, different_with_similar_variance) {
110  auto t1 = tf2::Transform(quaternionfromrpy(0,0,0), tf2::Vector3(0,0,0));
111  auto t2 = tf2::Transform(quaternionfromrpy(1,0,0), tf2::Vector3(1.0,0,0));
112 
113  auto tv1 = TransformWithVariance(t1, 0.1);
114  auto tv2 = TransformWithVariance(t2, 0.2);
115 
116  auto out_tv = averageTransforms(tv1, tv2);
117 
118  // Make sure that the new position is between the originals
119  ASSERT_GT(out_tv.transform.getOrigin().x(), 0);
120  ASSERT_LT(out_tv.transform.getOrigin().x(), 1.0);
121  ASSERT_GT(out_tv.transform.getRotation().getAngle(), 0);
122  ASSERT_LT(out_tv.transform.getRotation().getAngle(), 1.0);
123 
124  // The new variance should be larger because of disagreeing data
125  ASSERT_GT(out_tv.variance, 0.2);
126 }
127 
128 int main(int argc, char **argv) {
129  ::testing::InitGoogleTest(&argc, argv);
130  return RUN_ALL_TESTS();
131 }
132 
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
TEST(TransformWithVariance, simple_fusion)
static tf2::Quaternion quaternionfromrpy(double roll, double pitch, double yaw)
int main(int argc, char **argv)
TransformWithVariance averageTransforms(const TransformWithVariance &t1, const TransformWithVariance &t2)
list roll
Definition: fit_plane.py:42
list pitch
Definition: fit_plane.py:43


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Fri May 1 2020 04:04:05