lifelong_metrics_test.cpp
Go to the documentation of this file.
1 /*
2  * slam_toolbox
3  * Copyright (c) 2019, Steve Macenski
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
19 #ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_
20 #define SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_
21 
22 #include <gtest/gtest.h>
23 #include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp"
24 
25 using namespace karto;
26 
27 // 3 potential test cases, t1 is used.
28 //t1 = IOU([3.5, 4.0, 3.0, 4.0], [3.5, 5.5, 3.0, 3.0]) == 6.0
29 //t2 = IOU([4.5, 3.0, 5.0, 2.0], [4.5, 4.5, 3, 3]) == 3.0
30 //t3 = IOU([4.5, 3.5, 3.0, 3.0], [2.5, 5.5, 3, 3]) == 1.0
31 
32 namespace
33 {
34 
35 TEST(LifelingMetricsTests, TestBounds)
36 {
37  LocalizedRangeScan* s1 = new LocalizedRangeScan();
38  LocalizedRangeScan* s2 = new LocalizedRangeScan();
39  Pose2 p1 = Pose2(3.5, 4.0, 0.0);
40  Pose2 p2 = Pose2(3.5, 5.5, 0.0);
41  s1->SetBarycenterPose(p1);
42  s2->SetBarycenterPose(p2);
43  BoundingBox2 bb1, bb2;
44  bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
45  bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
46  bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
47  bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
48  s1->SetBoundingBox(bb1);
49  s2->SetBoundingBox(bb2);
50  PointVectorDouble pts;
51  pts.push_back(Vector2<double>(3.0, 5.0)); //inside
52  pts.push_back(Vector2<double>(3.0, 3.0)); //outside
53  s2->SetPointReadings(pts, true);
54  double x_l, x_u, y_l, y_u;
55  bool dirty = false;
56  s1->SetIsDirty(dirty);
57  s2->SetIsDirty(dirty);
58  slam_toolbox::LifelongSlamToolbox::computeIntersectBounds(s1, s2, x_l, x_u, y_l, y_u);
59  EXPECT_EQ(x_l, 2.0);
60  EXPECT_EQ(x_u, 5.0);
61  EXPECT_EQ(y_l, 4.0);
62  EXPECT_EQ(y_u, 6.0);
63  delete s1;
64  delete s2;
65 }
66 
67 TEST(LifelingMetricsTests, TestIntersect)
68 {
69  LocalizedRangeScan* s1 = new LocalizedRangeScan();
70  LocalizedRangeScan* s2 = new LocalizedRangeScan();
71  Pose2 p1 = Pose2(3.5, 4.0, 0.0);
72  Pose2 p2 = Pose2(3.5, 5.5, 0.0);
73  s1->SetBarycenterPose(p1);
74  s2->SetBarycenterPose(p2);
75  BoundingBox2 bb1, bb2;
76  bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
77  bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
78  bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
79  bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
80  s1->SetBoundingBox(bb1);
81  s2->SetBoundingBox(bb2);
82  PointVectorDouble pts;
83  pts.push_back(Vector2<double>(3.0, 5.0)); //inside
84  pts.push_back(Vector2<double>(3.0, 3.0)); //outside
85  s2->SetPointReadings(pts, true);
86  bool dirty = false;
87  s1->SetIsDirty(dirty);
88  s2->SetIsDirty(dirty);
89  double intersect = slam_toolbox::LifelongSlamToolbox::computeIntersect(s1, s2);
90  EXPECT_EQ(intersect, 6.0);
91  delete s1;
92  delete s2;
93 }
94 
95 TEST(LifelingMetricsTests, TestIntersectOverUnion)
96 {
97  LocalizedRangeScan* s1 = new LocalizedRangeScan();
98  LocalizedRangeScan* s2 = new LocalizedRangeScan();
99  Pose2 p1 = Pose2(3.5, 4.0, 0.0);
100  Pose2 p2 = Pose2(3.5, 5.5, 0.0);
101  s1->SetBarycenterPose(p1);
102  s2->SetBarycenterPose(p2);
103  BoundingBox2 bb1, bb2;
104  bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
105  bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
106  bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
107  bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
108  s1->SetBoundingBox(bb1);
109  s2->SetBoundingBox(bb2);
110  PointVectorDouble pts;
111  pts.push_back(Vector2<double>(3.0, 5.0)); //inside
112  pts.push_back(Vector2<double>(3.0, 3.0)); //outside
113  s2->SetPointReadings(pts, true);
114  bool dirty = false;
115  s1->SetIsDirty(dirty);
116  s2->SetIsDirty(dirty);
117  double intersect_over_union = slam_toolbox::LifelongSlamToolbox::computeIntersectOverUnion(s1, s2);
118  EXPECT_EQ(intersect_over_union, 0.4);
119  delete s1;
120  delete s2;
121 }
122 
123 TEST(LifelingMetricsTests, TestAreaOverlap)
124 {
125  LocalizedRangeScan* s1 = new LocalizedRangeScan();
126  LocalizedRangeScan* s2 = new LocalizedRangeScan();
127  Pose2 p1 = Pose2(3.5, 4.0, 0.0);
128  Pose2 p2 = Pose2(3.5, 5.5, 0.0);
129  s1->SetBarycenterPose(p1);
130  s2->SetBarycenterPose(p2);
131  BoundingBox2 bb1, bb2;
132  bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
133  bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
134  bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
135  bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
136  s1->SetBoundingBox(bb1);
137  s2->SetBoundingBox(bb2);
138  PointVectorDouble pts;
139  pts.push_back(Vector2<double>(3.0, 5.0)); //inside
140  pts.push_back(Vector2<double>(3.0, 3.0)); //outside
141  s2->SetPointReadings(pts, true);
142  bool dirty = false;
143  s1->SetIsDirty(dirty);
144  s2->SetIsDirty(dirty);
145  double area = slam_toolbox::LifelongSlamToolbox::computeAreaOverlapRatio(s1, s2);
146  EXPECT_NEAR(area, 0.6666, 0.01);
147  delete s1;
148  delete s2;
149 }
150 
151 TEST(LifelingMetricsTests, TestPtOverlap)
152 {
153  LocalizedRangeScan* s1 = new LocalizedRangeScan();
154  LocalizedRangeScan* s2 = new LocalizedRangeScan();
155  Pose2 p1 = Pose2(3.5, 4.0, 0.0);
156  Pose2 p2 = Pose2(3.5, 5.5, 0.0);
157  s1->SetBarycenterPose(p1);
158  s2->SetBarycenterPose(p2);
159  BoundingBox2 bb1, bb2;
160  bb1.SetMinimum(Vector2<kt_double>(2.0, 2.0));
161  bb1.SetMaximum(Vector2<kt_double>(5.0, 6.0));
162  bb2.SetMinimum(Vector2<kt_double>(2.0, 4.0));
163  bb2.SetMaximum(Vector2<kt_double>(5.0, 7.0));
164  s1->SetBoundingBox(bb1);
165  s2->SetBoundingBox(bb2);
166  PointVectorDouble pts;
167  pts.push_back(Vector2<double>(3.0, 5.0)); //inside
168  pts.push_back(Vector2<double>(3.0, 3.0)); //outside
169  s2->SetPointReadings(pts, true);
170  bool dirty = false;
171  s1->SetIsDirty(dirty);
172  s2->SetIsDirty(dirty);
173  double area = slam_toolbox::LifelongSlamToolbox::computeReadingOverlapRatio(s1, s2);
174  EXPECT_EQ(area, 0.5);
175  delete s1;
176  delete s2;
177 }
178 
179 }
180 
181 int main(int argc, char** argv)
182 {
183  testing::InitGoogleTest(&argc, argv);
184  return RUN_ALL_TESTS();
185 }
186 
187 #endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_
karto::BoundingBox2::SetMinimum
void SetMinimum(const Vector2< kt_double > &mMinimum)
Definition: Karto.h:2893
TEST
TEST(ActionClientDestruction, persistent_goal_handles_1)
karto::LocalizedRangeScan::SetPointReadings
void SetPointReadings(PointVectorDouble &points, kt_bool setFiltered=false)
Definition: Karto.h:5731
karto::LocalizedRangeScan
Definition: Karto.h:5505
karto::LocalizedRangeScan::SetIsDirty
void SetIsDirty(kt_bool &rIsDirty)
Definition: Karto.h:5639
karto::LocalizedRangeScan::SetBoundingBox
void SetBoundingBox(BoundingBox2 &bbox)
Definition: Karto.h:5702
karto::LocalizedRangeScan::SetBarycenterPose
void SetBarycenterPose(Pose2 &bcenter)
Definition: Karto.h:5606
slam_toolbox::LifelongSlamToolbox::computeIntersectOverUnion
static double computeIntersectOverUnion(LocalizedRangeScan *s1, LocalizedRangeScan *s2)
Definition: slam_toolbox_lifelong.cpp:381
karto::BoundingBox2::SetMaximum
void SetMaximum(const Vector2< kt_double > &rMaximum)
Definition: Karto.h:2909
karto::PointVectorDouble
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1283
karto::Vector2< kt_double >
slam_toolbox::LifelongSlamToolbox::computeAreaOverlapRatio
static double computeAreaOverlapRatio(LocalizedRangeScan *ref_scan, LocalizedRangeScan *candidate_scan)
Definition: slam_toolbox_lifelong.cpp:400
slam_toolbox::LifelongSlamToolbox::computeIntersectBounds
static void computeIntersectBounds(LocalizedRangeScan *s1, LocalizedRangeScan *s2, double &x_l, double &x_u, double &y_l, double &y_u)
Definition: slam_toolbox_lifelong.cpp:336
main
int main(int argc, char **argv)
Definition: lifelong_metrics_test.cpp:181
karto::Pose2
Definition: Karto.h:2046
slam_toolbox::LifelongSlamToolbox::computeReadingOverlapRatio
static double computeReadingOverlapRatio(LocalizedRangeScan *ref_scan, LocalizedRangeScan *candidate_scan)
Definition: slam_toolbox_lifelong.cpp:418
slam_toolbox_lifelong.hpp
karto::BoundingBox2
Definition: Karto.h:2869
slam_toolbox::LifelongSlamToolbox::computeIntersect
static double computeIntersect(LocalizedRangeScan *s1, LocalizedRangeScan *s2)
Definition: slam_toolbox_lifelong.cpp:364
karto
Definition: Karto.h:88

slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55