Gazebo Math

API Reference

6.15.1
gz/math/eigen3/Util.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef GZ_MATH_EIGEN3_UTIL_HH_
19 #define GZ_MATH_EIGEN3_UTIL_HH_
20 
21 #include <vector>
22 
23 #include <Eigen/Geometry>
24 #include <Eigen/Eigenvalues>
25 
27 #include <gz/math/Matrix3.hh>
28 #include <gz/math/OrientedBox.hh>
29 #include <gz/math/Pose3.hh>
30 #include <gz/math/Quaternion.hh>
31 #include <gz/math/Vector3.hh>
33 
34 namespace ignition
35 {
36  namespace math
37  {
38  namespace eigen3
39  {
45  const std::vector<math::Vector3d> &_vertices)
46  {
47  if (_vertices.empty())
48  return Eigen::Matrix3d::Identity();
49 
50  Eigen::Matrix<double, 9, 1> cumulants;
51  cumulants.setZero();
52  for (const auto &vertex : _vertices)
53  {
54  const Eigen::Vector3d &point = math::eigen3::convert(vertex);
55  cumulants(0) += point(0);
56  cumulants(1) += point(1);
57  cumulants(2) += point(2);
58  cumulants(3) += point(0) * point(0);
59  cumulants(4) += point(0) * point(1);
60  cumulants(5) += point(0) * point(2);
61  cumulants(6) += point(1) * point(1);
62  cumulants(7) += point(1) * point(2);
63  cumulants(8) += point(2) * point(2);
64  }
65 
66  Eigen::Matrix3d covariance;
67 
68  cumulants /= static_cast<double>(_vertices.size());
69 
70  covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
71  covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
72  covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
73  covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
74  covariance(1, 0) = covariance(0, 1);
75  covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
76  covariance(2, 0) = covariance(0, 2);
77  covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
78  covariance(2, 1) = covariance(1, 2);
79  return covariance;
80  }
81 
88  const std::vector<math::Vector3d> &_vertices)
89  {
91 
92  // Return an empty box if there are no vertices
93  if (_vertices.empty())
94  return box;
95 
97  for (const auto &point : _vertices)
98  mean += point;
99  mean /= static_cast<double>(_vertices.size());
100 
102  Eigen::Matrix3d covariance = covarianceMatrix(_vertices);
103 
104  // Eigen Vectors
105  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
106  eigenSolver(covariance, Eigen::ComputeEigenvectors);
107  Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors();
108 
109  // This line is necessary for proper orientation in some cases.
110  // The numbers come out the same without it, but the signs are
111  // different and the box doesn't get correctly oriented in some cases.
112  eigenVectorsPCA.col(2) =
113  eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
114 
115  // Transform the original cloud to the origin where the principal
116  // components correspond to the axes.
117  Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity());
118  projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
119  projectionTransform.block<3, 1>(0, 3) =
120  -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid);
121 
123  Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32);
124 
125  // Get the minimum and maximum points of the transformed cloud.
126  for (const auto &point : _vertices)
127  {
128  Eigen::Vector4d pt(0, 0, 0, 1);
129  pt.head<3>() = math::eigen3::convert(point);
130  Eigen::Vector4d tfPoint = projectionTransform * pt;
131  minPoint = minPoint.cwiseMin(tfPoint.head<3>());
132  maxPoint = maxPoint.cwiseMax(tfPoint.head<3>());
133  }
134 
135  const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint);
136 
137  // quaternion is calculated using the eigenvectors (which determines
138  // how the final box gets rotated), and the transform to put the box
139  // in correct location is calculated
140  const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA);
141  const Eigen::Vector3d bboxTransform =
142  eigenVectorsPCA * meanDiagonal + centroid;
143 
144  math::Vector3d size(
145  maxPoint.x() - minPoint.x(),
146  maxPoint.y() - minPoint.y(),
147  maxPoint.z() - minPoint.z()
148  );
149  math::Pose3d pose;
150  pose.Rot() = math::eigen3::convert(bboxQuaternion);
151  pose.Pos() = math::eigen3::convert(bboxTransform);
152 
153  box.Size(size);
154  box.Pose(pose);
155  return box;
156  }
157  }
158  }
159 }
160 
161 #endif
OrientedBoxd verticesToOrientedBox(const std::vector< math::Vector3d > &_vertices)
Get the oriented 3d bounding box of a set of 3d vertices using PCA http://codextechnicanum....
Definition: gz/math/eigen3/Util.hh:87
Matrix3< double > Matrix3d
Definition: gz/math/Matrix3.hh:548
static const int32_t INF_I32
32-bit unsigned integer positive infinite value
Definition: gz/math/Helpers.hh:336
Encapsulates a position and rotation in three space.
Definition: gz/math/Pose3.hh:34
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
Eigen::Vector3d convert(const Vector3d &_v)
Convert from Vector3d to Eigen::Vector3d.
Definition: gz/math/eigen3/Conversions.hh:38
STL class.
const Quaternion< T > & Rot() const
Get the rotation.
Definition: gz/math/Pose3.hh:420
OrientedBox< double > OrientedBoxd
Definition: gz/math/OrientedBox.hh:281
Vector4< double > Vector4d
Definition: gz/math/Vector4.hh:750
Quaternion< double > Quaterniond
Definition: gz/math/Quaternion.hh:1101
const Vector3< T > & Pos() const
Get the position.
Definition: gz/math/Pose3.hh:361
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
Mathematical representation of a box which can be arbitrarily positioned and rotated.
Definition: gz/math/OrientedBox.hh:39
Matrix4< double > Matrix4d
Definition: gz/math/Matrix4.hh:925
const Vector3< T > & Size() const
Get the size of the box.
Definition: gz/math/OrientedBox.hh:119
Eigen::Matrix3d covarianceMatrix(const std::vector< math::Vector3d > &_vertices)
Get covariance matrix from a set of 3d vertices https://github.com/isl-org/Open3D/blob/76c2baf9debd46...
Definition: gz/math/eigen3/Util.hh:44
T empty(T... args)
const Pose3< T > & Pose() const
Get the box pose, which is the pose of its center.
Definition: gz/math/OrientedBox.hh:126
T mean(const std::vector< T > &_values)
get mean of vector of values
Definition: gz/math/Helpers.hh:503
Vector3< double > Vector3d
Definition: gz/math/Vector3.hh:770