test_geometry_util.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #include <cmath>
31 #include <limits>
32 #include <vector>
33 
34 #include <gtest/gtest.h>
35 
36 #include <opencv2/core/core.hpp>
37 
38 #include <tf/transform_datatypes.h>
39 
40 #include <swri_math_util/constants.h>
41 #include <swri_image_util/geometry_util.h>
42 
43 TEST(GeometryUtilTests, Intersects)
44 {
45  swri_image_util::BoundingBox b1(-2, -2, 4, 4);
46  swri_image_util::BoundingBox b2(-1, -1, 2, 2);
47  swri_image_util::BoundingBox b3(-4, -4, 2.5, 2.5);
48  swri_image_util::BoundingBox b4(1.5, 1.5, 6, 6);
49  swri_image_util::BoundingBox b5(4, 4, 6, 6);
50 
51  EXPECT_TRUE(swri_image_util::Intersects(b1, b2));
52  EXPECT_TRUE(swri_image_util::Intersects(b1, b3));
53  EXPECT_TRUE(swri_image_util::Intersects(b1, b4));
54  EXPECT_FALSE(swri_image_util::Intersects(b1, b5));
55 
56  EXPECT_TRUE(swri_image_util::Intersects(b2, b1));
57  EXPECT_FALSE(swri_image_util::Intersects(b2, b3));
58  EXPECT_FALSE(swri_image_util::Intersects(b2, b4));
59  EXPECT_FALSE(swri_image_util::Intersects(b2, b5));
60 
61  EXPECT_TRUE(swri_image_util::Intersects(b3, b1));
62  EXPECT_FALSE(swri_image_util::Intersects(b3, b2));
63  EXPECT_FALSE(swri_image_util::Intersects(b3, b4));
64  EXPECT_FALSE(swri_image_util::Intersects(b3, b5));
65 
66  EXPECT_TRUE(swri_image_util::Intersects(b4, b1));
67  EXPECT_FALSE(swri_image_util::Intersects(b4, b2));
68  EXPECT_FALSE(swri_image_util::Intersects(b4, b3));
69  EXPECT_TRUE(swri_image_util::Intersects(b4, b5));
70 
71  EXPECT_FALSE(swri_image_util::Intersects(b5, b1));
72  EXPECT_FALSE(swri_image_util::Intersects(b5, b2));
73  EXPECT_FALSE(swri_image_util::Intersects(b5, b3));
74  EXPECT_TRUE(swri_image_util::Intersects(b5, b4));
75 }
76 
77 TEST(GeometryUtilTests, GetOverlappingArea1)
78 {
79  cv::Rect rect(-5, -5, 10, 10);
80  cv::Rect rect2(-10, -5, 20, 10);
81 
82  cv::Mat identity(2, 3, CV_32FC1);
83  identity.at<float>(0,0) = 1;
84  identity.at<float>(0,1) = 0;
85  identity.at<float>(0,2) = 0;
86  identity.at<float>(1,0) = 0;
87  identity.at<float>(1,1) = 1;
88  identity.at<float>(1,2) = 0;
89 
90  EXPECT_FLOAT_EQ(100, swri_image_util::GetOverlappingArea(rect, identity));
91  EXPECT_FLOAT_EQ(200, swri_image_util::GetOverlappingArea(rect2, identity));
92 
93  cv::Mat shift_15_x(2, 3, CV_32FC1);
94  shift_15_x.at<float>(0,0) = 1;
95  shift_15_x.at<float>(0,1) = 0;
96  shift_15_x.at<float>(0,2) = 15;
97  shift_15_x.at<float>(1,0) = 0;
98  shift_15_x.at<float>(1,1) = 1;
99  shift_15_x.at<float>(1,2) = 0;
100 
101  EXPECT_FLOAT_EQ(0, swri_image_util::GetOverlappingArea(rect, shift_15_x));
102  EXPECT_FLOAT_EQ(50, swri_image_util::GetOverlappingArea(rect2, shift_15_x));
103 
104  cv::Mat shift_5_x(2, 3, CV_32FC1);
105  shift_5_x.at<float>(0,0) = 1;
106  shift_5_x.at<float>(0,1) = 0;
107  shift_5_x.at<float>(0,2) = 5;
108  shift_5_x.at<float>(1,0) = 0;
109  shift_5_x.at<float>(1,1) = 1;
110  shift_5_x.at<float>(1,2) = 0;
111 
112  EXPECT_FLOAT_EQ(50, swri_image_util::GetOverlappingArea(rect, shift_5_x));
113  EXPECT_FLOAT_EQ(150, swri_image_util::GetOverlappingArea(rect2, shift_5_x));
114 
115  cv::Mat shift_5_xy(2, 3, CV_32FC1);
116  shift_5_xy.at<float>(0,0) = 1;
117  shift_5_xy.at<float>(0,1) = 0;
118  shift_5_xy.at<float>(0,2) = 5;
119  shift_5_xy.at<float>(1,0) = 0;
120  shift_5_xy.at<float>(1,1) = 1;
121  shift_5_xy.at<float>(1,2) = 5;
122 
123  EXPECT_FLOAT_EQ(25, swri_image_util::GetOverlappingArea(rect, shift_5_xy));
124  EXPECT_FLOAT_EQ(75, swri_image_util::GetOverlappingArea(rect2, shift_5_xy));
125 }
126 
127 TEST(GeometryUtilTests, GetOverlappingArea2)
128 {
129  cv::Rect rect(-5, -5, 10, 10);
130  cv::Rect rect2(-10, -5, 20, 10);
131 
132  cv::Mat rotate90(2, 3, CV_32FC1);
133  rotate90.at<float>(0,0) = 0;
134  rotate90.at<float>(0,1) = 1;
135  rotate90.at<float>(0,2) = 0;
136  rotate90.at<float>(1,0) = -1;
137  rotate90.at<float>(1,1) = 0;
138  rotate90.at<float>(1,2) = 0;
139 
140  EXPECT_FLOAT_EQ(100, swri_image_util::GetOverlappingArea(rect, rotate90));
141  EXPECT_FLOAT_EQ(100, swri_image_util::GetOverlappingArea(rect2, rotate90));
142 
143  cv::Mat rotate180(2, 3, CV_32FC1);
144  rotate180.at<float>(0,0) = -1;
145  rotate180.at<float>(0,1) = 0;
146  rotate180.at<float>(0,2) = 0;
147  rotate180.at<float>(1,0) = 0;
148  rotate180.at<float>(1,1) = -1;
149  rotate180.at<float>(1,2) = 0;
150 
151  EXPECT_FLOAT_EQ(100, swri_image_util::GetOverlappingArea(rect, rotate180));
152  EXPECT_FLOAT_EQ(200, swri_image_util::GetOverlappingArea(rect2, rotate180));
153 
154  cv::Mat rotate45(2, 3, CV_32FC1);
155  rotate45.at<float>(0,0) = std::cos(swri_math_util::_half_pi * 0.5);
156  rotate45.at<float>(0,1) = std::sin(swri_math_util::_half_pi * 0.5);
157  rotate45.at<float>(0,2) = 0;
158  rotate45.at<float>(1,0) = -std::sin(swri_math_util::_half_pi * 0.5);
159  rotate45.at<float>(1,1) = std::cos(swri_math_util::_half_pi * 0.5);
160  rotate45.at<float>(1,2) = 0;
161 
162  EXPECT_FLOAT_EQ(82.842712, swri_image_util::GetOverlappingArea(rect, rotate45));
163  EXPECT_FLOAT_EQ(136.3961, swri_image_util::GetOverlappingArea(rect2, rotate45));
164 }
165 
166 TEST(GeometryUtilTests, TestProjectEllipse1)
167 {
168  cv::Mat ellipsoid1(3, 3, CV_32FC1);
169  ellipsoid1.at<float>(0,0) = 1;
170  ellipsoid1.at<float>(0,1) = 0;
171  ellipsoid1.at<float>(0,2) = 0;
172  ellipsoid1.at<float>(1,0) = 0;
173  ellipsoid1.at<float>(1,1) = 1;
174  ellipsoid1.at<float>(1,2) = 0;
175  ellipsoid1.at<float>(2,0) = 0;
176  ellipsoid1.at<float>(2,1) = 0;
177  ellipsoid1.at<float>(2,2) = 1;
178 
179  cv::Mat ellipse1(2, 2, CV_32FC1);
180  ellipse1.at<float>(0,0) = 1;
181  ellipse1.at<float>(0,1) = 0;
182  ellipse1.at<float>(1,0) = 0;
183  ellipse1.at<float>(1,1) = 1;
184 
185  cv::Mat projected1 = swri_image_util::ProjectEllipsoid(ellipsoid1);
186 
187  EXPECT_EQ(ellipse1.at<float>(0, 0), projected1.at<float>(0, 0));
188  EXPECT_EQ(ellipse1.at<float>(0, 1), projected1.at<float>(0, 1));
189  EXPECT_EQ(ellipse1.at<float>(1, 0), projected1.at<float>(1, 0));
190  EXPECT_EQ(ellipse1.at<float>(1, 1), projected1.at<float>(1, 1));
191 }
192 
193 TEST(GeometryUtilTests, TestProjectEllipse2)
194 {
195  cv::Mat ellipsoid1(3, 3, CV_32FC1);
196  ellipsoid1.at<float>(0,0) = 10;
197  ellipsoid1.at<float>(0,1) = 0;
198  ellipsoid1.at<float>(0,2) = 0;
199  ellipsoid1.at<float>(1,0) = 0;
200  ellipsoid1.at<float>(1,1) = 15;
201  ellipsoid1.at<float>(1,2) = 0;
202  ellipsoid1.at<float>(2,0) = 0;
203  ellipsoid1.at<float>(2,1) = 0;
204  ellipsoid1.at<float>(2,2) = -35;
205 
206  cv::Mat ellipse1(2, 2, CV_32FC1);
207  ellipse1.at<float>(0,0) = 10;
208  ellipse1.at<float>(0,1) = 0;
209  ellipse1.at<float>(1,0) = 0;
210  ellipse1.at<float>(1,1) = 15;
211 
212  cv::Mat projected1 = swri_image_util::ProjectEllipsoid(ellipsoid1);
213 
214  EXPECT_EQ(ellipse1.at<float>(0, 0), projected1.at<float>(0, 0));
215  EXPECT_EQ(ellipse1.at<float>(0, 1), projected1.at<float>(0, 1));
216  EXPECT_EQ(ellipse1.at<float>(1, 0), projected1.at<float>(1, 0));
217  EXPECT_EQ(ellipse1.at<float>(1, 1), projected1.at<float>(1, 1));
218 }
219 
220 TEST(GeometryUtilTests, TestProjectEllipse3)
221 {
222  cv::Mat ellipsoid1(3, 3, CV_32FC1);
223  ellipsoid1.at<float>(0,0) = 10;
224  ellipsoid1.at<float>(0,1) = 0;
225  ellipsoid1.at<float>(0,2) = 0;
226  ellipsoid1.at<float>(1,0) = 0;
227  ellipsoid1.at<float>(1,1) = 15;
228  ellipsoid1.at<float>(1,2) = 0;
229  ellipsoid1.at<float>(2,0) = 0;
230  ellipsoid1.at<float>(2,1) = 0;
231  ellipsoid1.at<float>(2,2) = std::numeric_limits<double>::max() * 0.5;
232 
233  cv::Mat ellipse1(2, 2, CV_32FC1);
234  ellipse1.at<float>(0,0) = 10;
235  ellipse1.at<float>(0,1) = 0;
236  ellipse1.at<float>(1,0) = 0;
237  ellipse1.at<float>(1,1) = 15;
238 
239  cv::Mat projected1 = swri_image_util::ProjectEllipsoid(ellipsoid1);
240 
241  EXPECT_EQ(ellipse1.at<float>(0, 0), projected1.at<float>(0, 0));
242  EXPECT_EQ(ellipse1.at<float>(0, 1), projected1.at<float>(0, 1));
243  EXPECT_EQ(ellipse1.at<float>(1, 0), projected1.at<float>(1, 0));
244  EXPECT_EQ(ellipse1.at<float>(1, 1), projected1.at<float>(1, 1));
245 }
246 
247 
248 // TODO(malban): Test projecting an ellipsoid that is not axis aligned.
249 
250 TEST(GeometryUtilTests, TestProjectEllipseInvalid1)
251 {
252  cv::Mat ellipsoid2(3, 3, CV_32FC1);
253  ellipsoid2.at<float>(0,0) = 1;
254  ellipsoid2.at<float>(0,1) = 0;
255  ellipsoid2.at<float>(0,2) = 0;
256  ellipsoid2.at<float>(1,0) = 0;
257  ellipsoid2.at<float>(1,1) = 1;
258  ellipsoid2.at<float>(1,2) = 0;
259  ellipsoid2.at<float>(2,0) = 0;
260  ellipsoid2.at<float>(2,1) = 0;
261  ellipsoid2.at<float>(2,2) = 0;
262 
263  cv::Mat projected2 = swri_image_util::ProjectEllipsoid(ellipsoid2);
264 
265  EXPECT_TRUE(projected2.empty());
266 }
267 
268 TEST(GeometryUtilTests, TestProjectEllipseInvalid2)
269 {
270  cv::Mat ellipsoid2(2, 2, CV_32FC1);
271  ellipsoid2.at<float>(0,0) = 1;
272  ellipsoid2.at<float>(0,1) = 0;
273  ellipsoid2.at<float>(1,0) = 0;
274  ellipsoid2.at<float>(1,1) = 1;
275 
276  cv::Mat projected2 = swri_image_util::ProjectEllipsoid(ellipsoid2);
277 
278  EXPECT_TRUE(projected2.empty());
279 }
280 
281 TEST(GeometryUtilTests, TestProjectEllipseInvalid3)
282 {
283  cv::Mat ellipsoid2(2, 2, CV_32SC1);
284  ellipsoid2.at<int32_t>(0,0) = 1;
285  ellipsoid2.at<int32_t>(0,1) = 0;
286  ellipsoid2.at<int32_t>(1,0) = 0;
287  ellipsoid2.at<int32_t>(1,1) = 1;
288 
289  cv::Mat projected2 = swri_image_util::ProjectEllipsoid(ellipsoid2);
290 
291  EXPECT_TRUE(projected2.empty());
292 }
293 
294 TEST(GeometryUtilTests, TestGetEllipsePoints1)
295 {
296  cv::Mat ellipse(2, 2, CV_32FC1);
297  ellipse.at<float>(0,0) = 1;
298  ellipse.at<float>(0,1) = 0;
299  ellipse.at<float>(1,0) = 0;
300  ellipse.at<float>(1,1) = 1;
301 
302  std::vector<tf::Vector3> points = swri_image_util::GetEllipsePoints(
303  ellipse, tf::Vector3(0, 0, 0), 1, 8);
304 
305  ASSERT_EQ(8, points.size());
306 
307  EXPECT_FLOAT_EQ(1, points[0].x());
308  EXPECT_NEAR(0, points[0].y(), 0.000000001);
309 
310  EXPECT_NEAR(0.7071067811865475243818940365, points[1].x(), 0.000000001);
311  EXPECT_NEAR(0.7071067811865475243818940365, points[1].y(), 0.000000001);
312 
313  EXPECT_NEAR(0, points[2].x(), 0.000000001);
314  EXPECT_FLOAT_EQ(1, points[2].y());
315 
316  EXPECT_NEAR(-0.7071067811865475243818940365, points[3].x(), 0.000000001);
317  EXPECT_NEAR(0.7071067811865475243818940365, points[3].y(), 0.000000001);
318 
319  EXPECT_FLOAT_EQ(-1, points[4].x());
320  EXPECT_NEAR(0, points[4].y(), 0.000000001);
321 
322  EXPECT_NEAR(-0.7071067811865475243818940365, points[5].x(), 0.000000001);
323  EXPECT_NEAR(-0.7071067811865475243818940365, points[5].y(), 0.000000001);
324 
325  EXPECT_NEAR(0, points[6].x(), 0.000000001);
326  EXPECT_FLOAT_EQ(-1, points[6].y());
327 
328  EXPECT_NEAR(0.7071067811865475243818940365, points[7].x(), 0.000000001);
329  EXPECT_NEAR(-0.7071067811865475243818940365, points[7].y(), 0.000000001);
330 }
331 
332 // Run all the tests that were declared with TEST()
333 int main(int argc, char **argv)
334 {
335  testing::InitGoogleTest(&argc, argv);
336 
337  return RUN_ALL_TESTS();
338 }
swri_math_util::_half_pi
static const long double _half_pi
swri_image_util::ProjectEllipsoid
cv::Mat ProjectEllipsoid(const cv::Mat &ellipsiod)
Definition: geometry_util.cpp:73
swri_image_util::GetOverlappingArea
double GetOverlappingArea(const cv::Rect &rect, const cv::Mat &rigid_transform)
Definition: geometry_util.cpp:51
TEST
TEST(GeometryUtilTests, Intersects)
Definition: test_geometry_util.cpp:43
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
transform_datatypes.h
swri_image_util::Intersects
bool Intersects(const BoundingBox &box1, const BoundingBox &box2)
Definition: geometry_util.cpp:46
main
int main(int argc, char **argv)
Definition: test_geometry_util.cpp:333
swri_image_util::GetEllipsePoints
std::vector< tf::Vector3 > GetEllipsePoints(const cv::Mat &ellipse, const tf::Vector3 &center, double scale, int32_t num_points)
Definition: geometry_util.cpp:195
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
geometry_util.h
swri_image_util::BoundingBox
cv::Rect_< double > BoundingBox
Definition: geometry_util.h:31

swri_image_util
Author(s): Kris Kozak
autogenerated on Fri Jun 7 2019 22:05:56