Gazebo Math

API Reference

7.4.0
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>
29#include <gz/math/Pose3.hh>
30#include <gz/math/Quaternion.hh>
31#include <gz/math/Vector3.hh>
33
34namespace gz
35{
36 namespace math
37 {
38 namespace eigen3
39 {
44 inline Eigen::Matrix3d covarianceMatrix(
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
101 Eigen::Vector3d centroid = math::eigen3::convert(mean);
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
122 Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32);
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