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 
31 
32 #include <cmath>
33 #include <limits>
34 #include <vector>
35 
36 #include <opencv2/imgproc/imgproc.hpp>
37 
38 #include <Eigen/Dense>
39 #include <Eigen/SVD>
40 
43 
44 namespace swri_image_util
45 {
46  bool Intersects(const BoundingBox& box1, const BoundingBox& box2)
47  {
48  return (box1 & box2).area() > 0;
49  }
50 
51  double GetOverlappingArea(const cv::Rect& rect, const cv::Mat& rigid_transform)
52  {
53  // List of points corresponding to the input rectangle.
54  std::vector<cv::Vec2d> points;
55 
56  // List of points correspondng to the transformed rectangle.
57  std::vector<cv::Vec2d> points_t;
58 
59  // Create a point for each corner of the input rectangle.
60  points.push_back(cv::Vec2d(rect.x, rect.y));
61  points.push_back(cv::Vec2d(rect.x + rect.width, rect.y));
62  points.push_back(cv::Vec2d(rect.x + rect.width, rect.y + rect.height));
63  points.push_back(cv::Vec2d(rect.x, rect.y + rect.height));
64 
65  // Transform the input points to the transformed points using the rigid
66  // transform.
67  cv::transform(cv::InputArray(points), cv::OutputArray(points_t), rigid_transform);
68 
69  // Use swri_geometry_util to get the overlapping area
70  return swri_geometry_util::PolygonIntersectionArea(points, points_t);
71  }
72 
73  cv::Mat ProjectEllipsoid(const cv::Mat& ellipsoid)
74  {
75  cv::Mat ellipse;
76 
77  if (ellipsoid.rows == 3 && ellipsoid.cols == 3 && ellipsoid.type() == CV_32FC1)
78  {
79  if (ellipsoid.at<float>(2, 2) >= std::numeric_limits<double>::max() * 0.5)
80  {
81  ellipse.create(2, 2, CV_32FC1);
82  ellipse.at<float>(0, 0) = ellipsoid.at<float>(0, 0);
83  ellipse.at<float>(0, 1) = ellipsoid.at<float>(0, 1);
84  ellipse.at<float>(1, 0) = ellipsoid.at<float>(1, 0);
85  ellipse.at<float>(1, 1) = ellipsoid.at<float>(1, 1);
86 
87  return ellipse;
88  }
89 
90  Eigen::Matrix3d A;
91  for (size_t r = 0; r < 3; r++)
92  {
93  for (size_t c = 0; c < 3; c++)
94  {
95  A(r, c) = ellipsoid.at<float>(r, c);
96  }
97  }
98 
99  // Specify the main vector directions (x and y)
100  Eigen::Vector3d ax1;
101  Eigen::Vector3d ax2;
102  ax1 << 1, 0, 0;
103  ax2 << 0, 1, 0;
104 
105  // Initialize the normal vector (here the normal vector, in the state
106  // space is in the theta direction).
107  Eigen::Vector3d n_ax;
108  n_ax << 0, 0, 1;
109 
111  // Calculate A prime //
113 
114  Eigen::Matrix3d A_sym_temp = A.transpose() + A;
115 
116  // N_ax_temp = (n_ax*n_ax')
117  Eigen::Matrix3d N_ax_temp = n_ax * n_ax.transpose();
118 
119  // A_prime_1 = A_sym_temp*N_ax_temp
120  // = (A+A')*(n_ax*n_ax')
121  Eigen::Matrix3d A_prime_1 = A_sym_temp * N_ax_temp;
122 
123  // A_prime_2 = A_prime_1*A_sym_temp
124  // = (A+A')*(n_ax*n_ax')*(A+A')
125  Eigen::Matrix3d A_prime_2 = A_prime_1 * A_sym_temp;
126 
127  // scale_1 = n_ax'*A
128  Eigen::RowVector3d scale_1 = n_ax.transpose() * A;
129 
130  // scale_2 = (scale_1*n_ax)*-4
131  // = (n_ax'*A*n_ax)*-4
132  double scale = (scale_1 * n_ax)(0,0) * -4.0;
133 
134  // A_temp = scale*A_temp
135  // = scale_2*A = -4*(n_ax'*A*n_ax)*A
136  Eigen::Matrix3d A_temp = A * scale;
137 
138  // Aprime = A_prime_2 + A_temp
139  // = (A+A')*(n_ax*n_ax')*(A+A') - 4*(n_ax'*A*n_ax)*A
140  Eigen::Matrix3d Aprime = A_prime_2 + A_temp;
141 
143  // Calculate C prime //
145 
146  // C_temp = n_ax'*A
147  Eigen::RowVector3d C_temp = n_ax.transpose() * A;
148 
149  // Cprime = -4.0*C_temp*n_ax
150  // = -4.0*n_ax'*A*n_ax
151  double cp = (-4.0 * C_temp) * n_ax;
152 
153  // Bprime = Aprime/Cprime;
154  Eigen::Matrix3d Bprime = Aprime / cp;
155 
156  // Jp = axes_def;
157  // = [ax1(:),ax2(:)] = [1,0;0,1;0,0];
158  Eigen::Matrix<double, 3, 2> Jp;
159  Jp << 1, 0,
160  0, 1,
161  0, 0;
162 
164  // Calculate D prime //
166 
167  // Dprime_temp = Jp'*Bprime
168  Eigen::Matrix<double, 2, 3> Dprime_temp = Jp.transpose() * Bprime;
169 
170  // Dprime = Dprime_temp * Jp
171  // = Jp'*Bprime*Jp
172  Eigen::Matrix2d Dprime = Dprime_temp * Jp;
173 
174  for (size_t r = 0; r < 2; r++)
175  {
176  for (size_t c = 0; c < 2; c++)
177  {
178  if (Dprime(r, c) != Dprime(r, c))
179  {
180  return ellipse;
181  }
182  }
183  }
184 
185  ellipse.create(2, 2, CV_32FC1);
186  ellipse.at<float>(0, 0) = Dprime(0, 0);
187  ellipse.at<float>(0, 1) = Dprime(0, 1);
188  ellipse.at<float>(1, 0) = Dprime(1, 0);
189  ellipse.at<float>(1, 1) = Dprime(1, 1);
190  }
191 
192  return ellipse;
193  }
194 
195  std::vector<tf::Vector3> GetEllipsePoints(
196  const cv::Mat& ellipse,
197  const tf::Vector3& center,
198  double scale,
199  int32_t num_points)
200  {
201  std::vector<tf::Vector3> perimeter;
202 
203  if (ellipse.rows == 2 && ellipse.cols == 2 && ellipse.type() == CV_32FC1 &&
204  num_points > 2)
205  {
206  Eigen::Matrix2d Dprime;
207  Dprime(0, 0) = ellipse.at<float>(0, 0);
208  Dprime(0, 1) = ellipse.at<float>(0, 1);
209  Dprime(1, 0) = ellipse.at<float>(1, 0);
210  Dprime(1, 1) = ellipse.at<float>(1, 1);
211 
212  Eigen::JacobiSVD<Eigen::Matrix2d> svd(Dprime, Eigen::ComputeFullV);
213 
214  Eigen::Vector2d Sigma = svd.singularValues();
215  Eigen::Matrix2d Vt = svd.matrixV().transpose();
216 
217  double xprime_scale = std::sqrt(Sigma(0));
218  double yprime_scale = std::sqrt(Sigma(1));
219 
220  if (xprime_scale <= 0 || yprime_scale <= 0)
221  {
222  return perimeter;
223  }
224 
225  Eigen::MatrixX2d Xp1(num_points, 2);
226  for (int32_t i = 0; i < num_points; i++)
227  {
228  double phi =
229  (static_cast<double>(i) / static_cast<double>(num_points))
231 
232  Xp1(i, 0) = xprime_scale * std::cos(phi) * scale;
233  Xp1(i, 1) = yprime_scale * std::sin(phi) * scale;
234  }
235 
236  // Xell = Xp1*(V')'
237  Eigen::MatrixX2d Xell = Xp1 * Vt;
238 
239  perimeter.resize(num_points);
240  for (int32_t i = 0; i < num_points; i++)
241  {
242  perimeter[i].setX(Xell(i, 0) + center.x());
243  perimeter[i].setY(Xell(i, 1) + center.y());
244  }
245  }
246 
247  return perimeter;
248  }
249 }
cv::Mat ProjectEllipsoid(const cv::Mat &ellipsiod)
double GetOverlappingArea(const cv::Rect &rect, const cv::Mat &rigid_transform)
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool Intersects(const BoundingBox &box1, const BoundingBox &box2)
static const long double _2pi
std::vector< tf::Vector3 > GetEllipsePoints(const cv::Mat &ellipse, const tf::Vector3 &center, double scale, int32_t num_points)
TFSIMD_FORCE_INLINE const tfScalar & y() const
double PolygonIntersectionArea(const std::vector< cv::Vec2d > &a, const std::vector< cv::Vec2d > &b)
cv::Rect_< double > BoundingBox
Definition: geometry_util.h:31


swri_image_util
Author(s): Kris Kozak
autogenerated on Tue Apr 6 2021 02:50:39