Branch data Line data Source code
1 : : // Copyright 2020 the Autoware Foundation
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : :
17 : : #ifndef NDT__UTILS_HPP_
18 : : #define NDT__UTILS_HPP_
19 : :
20 : : #include <Eigen/Core>
21 : : #include <Eigen/Geometry>
22 : : #include <geometry_msgs/msg/transform.hpp>
23 : : #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
24 : : #include <helper_functions/float_comparisons.hpp>
25 : : #include <geometry_msgs/msg/pose.hpp>
26 : : #include <Eigen/Eigenvalues>
27 : : #include <algorithm>
28 : : #include <limits>
29 : : #include <tuple>
30 : :
31 : : namespace autoware
32 : : {
33 : : namespace localization
34 : : {
35 : : namespace ndt
36 : : {
37 : :
38 : : struct PointWithCovariances
39 : : {
40 : : double x;
41 : : double y;
42 : : double z;
43 : : double icov_xx;
44 : : double icov_xy;
45 : : double icov_xz;
46 : : double icov_yy;
47 : : double icov_yz;
48 : : double icov_zz;
49 : 0 : friend bool operator==(const PointWithCovariances & p1, const PointWithCovariances & p2)
50 : : {
51 : : constexpr auto eps = std::numeric_limits<double>::epsilon();
52 [ # # # # ]: 0 : return common::helper_functions::comparisons::rel_eq(p1.x, p2.x, eps) &&
53 [ # # # # ]: 0 : common::helper_functions::comparisons::rel_eq(p1.y, p2.y, eps) &&
54 [ # # # # ]: 0 : common::helper_functions::comparisons::rel_eq(p1.z, p2.z, eps) &&
55 [ # # # # ]: 0 : common::helper_functions::comparisons::rel_eq(p1.icov_xx, p2.icov_xx, eps) &&
56 [ # # # # ]: 0 : common::helper_functions::comparisons::rel_eq(p1.icov_xy, p2.icov_xy, eps) &&
57 [ # # # # ]: 0 : common::helper_functions::comparisons::rel_eq(p1.icov_xz, p2.icov_xz, eps) &&
58 [ # # # # ]: 0 : common::helper_functions::comparisons::rel_eq(p1.icov_yy, p2.icov_yy, eps) &&
59 [ # # # # : 0 : common::helper_functions::comparisons::rel_eq(p1.icov_yz, p2.icov_yz, eps) &&
# # ]
60 [ # # ]: 0 : common::helper_functions::comparisons::rel_eq(p1.icov_zz, p2.icov_zz, eps);
61 : : }
62 : : };
63 [ + - + - ]: 118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xx);
64 [ + - + - ]: 118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xy);
65 [ + - + - ]: 118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xz);
66 [ + - + - ]: 118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_yy);
67 [ + - + - ]: 118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_yz);
68 [ + - + - ]: 118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_zz);
69 : :
70 : : using PointWithCovariancesFieldGenerators = std::tuple<
71 : : point_cloud_msg_wrapper::field_x_generator,
72 : : point_cloud_msg_wrapper::field_y_generator,
73 : : point_cloud_msg_wrapper::field_z_generator,
74 : : field_icov_xx_generator,
75 : : field_icov_xy_generator,
76 : : field_icov_xz_generator,
77 : : field_icov_yy_generator,
78 : : field_icov_yz_generator,
79 : : field_icov_zz_generator>;
80 : :
81 : : using NdtMapCloudModifier =
82 : : point_cloud_msg_wrapper::PointCloud2Modifier<PointWithCovariances,
83 : : PointWithCovariancesFieldGenerators>;
84 : :
85 : : using NdtMapCloudView =
86 : : point_cloud_msg_wrapper::PointCloud2View<PointWithCovariances,
87 : : PointWithCovariancesFieldGenerators>;
88 : :
89 : : /// This function will check if the covariance is valid based on its eigenvalues. If the covariance
90 : : /// is valid, eigen values smaller than a fraction of the biggest eigen value will be capped to the
91 : : /// threshold. Covariance then will be reconstructed from the modified set of eigen values and
92 : : /// vectors. This should result in increased numerical stability as stated in [Magnusson 2009].
93 : : /// If the covariance is invalid, it will not be updated and false will be returned.
94 : : /// \tparam Derived Deduced Eigen Matrix type.
95 : : /// \param covariance [in, out] Covariance matrix to get stabilized.
96 : : /// \param scaling_factor [in] The ratio between the max. eigen value and the minimum
97 : : /// allowed eigenvalue. Default value is 0.01 as suggested in [Magnusson 2009], page 60.
98 : : /// \return True if the covariance matrix is valid.
99 : : template<typename Derived>
100 : 19959 : bool try_stabilize_covariance(
101 : : Eigen::MatrixBase<Derived> & covariance,
102 : : typename Derived::PlainMatrix::Scalar scaling_factor = 0.1)
103 : : {
104 : : using CovMatrixT = typename Derived::PlainMatrix;
105 : : using ScalarT = typename CovMatrixT::Scalar;
106 : : using IndexT = typename CovMatrixT::Index;
107 : : using SolverT = Eigen::SelfAdjointEigenSolver<CovMatrixT>;
108 : : using VectorT = typename SolverT::RealVectorType;
109 : :
110 : : Eigen::SelfAdjointEigenSolver<CovMatrixT> solver(covariance);
111 [ + - ]: 19959 : if (solver.info() != Eigen::Success) {return false;}
112 : :
113 : : VectorT eigen_values = solver.eigenvalues(); // Sorted in increasing order.
114 : 19959 : const auto min_eigen_value = eigen_values[0];
115 : 19959 : const auto max_eigen_value = eigen_values[eigen_values.size() - IndexT{1U}];
116 : 19959 : const auto stabilized_min_eigen_value = scaling_factor * max_eigen_value;
117 [ + + ]: 19959 : if (stabilized_min_eigen_value < std::numeric_limits<ScalarT>::epsilon()) {return false;}
118 [ + + ]: 16690 : if (min_eigen_value > scaling_factor * max_eigen_value) {return true;} // Already stable.
119 [ + + ]: 48044 : for (auto i = IndexT{0U}; i < eigen_values.size(); ++i) {
120 : 36033 : eigen_values[i] = std::max(eigen_values[i], stabilized_min_eigen_value);
121 : : }
122 : : covariance =
123 : 12011 : solver.eigenvectors() * eigen_values.asDiagonal() * solver.eigenvectors().transpose();
124 : 12011 : return true;
125 : : }
126 : :
127 : : template<typename T>
128 : : using EigenPose = Eigen::Matrix<T, 6U, 1U>;
129 : : template<typename T>
130 : : using EigenTransform = Eigen::Transform<T, 3, Eigen::Affine, Eigen::ColMajor>;
131 : : using RosTransform = geometry_msgs::msg::Transform;
132 : : using RosPose = geometry_msgs::msg::Pose;
133 : : namespace transform_adapters
134 : : {
135 : : /// Template function to convert a 6D pose to a transformation matrix.
136 : : /// This function should be specialized and implemented for the supported types.
137 : : /// \tparam PoseT Pose type.
138 : : /// \tparam TransformT Transform type.
139 : : /// \param[in] pose pose to convert
140 : : /// \param[out] transform resulting transform
141 : : template<typename PoseT, typename TransformT>
142 : : void pose_to_transform(const PoseT & pose, TransformT & transform);
143 : :
144 : : /// Template function to convert a 6D pose to a transformation matrix.
145 : : /// This function should be specialized and implemented for the supported types.
146 : : /// \tparam PoseT Pose type.
147 : : /// \tparam TransformT Transform type.
148 : : /// \param[in] transform resulting transform
149 : : /// \param[out] pose pose to convert
150 : : template<typename PoseT, typename TransformT>
151 : : void transform_to_pose(const TransformT & transform, PoseT & pose);
152 : :
153 : :
154 : : template<typename T>
155 : 692 : void pose_to_transform(
156 : : const EigenPose<T> & pose,
157 : : EigenTransform<T> & transform)
158 : : {
159 : : static_assert(std::is_floating_point<T>::value, "Eigen transform should use floating points");
160 : : transform.setIdentity();
161 : : transform.translation() = pose.head(3);
162 : 692 : transform.rotate(Eigen::AngleAxis<T>(pose(3), Eigen::Vector3d::UnitX()));
163 : 692 : transform.rotate(Eigen::AngleAxis<T>(pose(4), Eigen::Vector3d::UnitY()));
164 : 692 : transform.rotate(Eigen::AngleAxis<T>(pose(5), Eigen::Vector3d::UnitZ()));
165 : 692 : }
166 : :
167 : : /// Specialization to convert from the eigen pose to the ros transform type.
168 : : /// \tparam T Eigen scalar type
169 : : template<typename T>
170 : 424 : void pose_to_transform(
171 : : const EigenPose<T> & pose,
172 : : RosTransform & transform)
173 : : {
174 : : static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
175 : 424 : Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
176 : : eig_rot.setIdentity();
177 : 424 : eig_rot =
178 : : Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
179 : : Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
180 : : Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());
181 : :
182 : : decltype(RosTransform::translation) trans;
183 : : decltype(RosTransform::rotation) rot;
184 : :
185 : 424 : trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
186 : : transform.set__translation(trans);
187 : :
188 : : rot.set__x(eig_rot.x()).
189 : : set__y(eig_rot.y()).
190 : : set__z(eig_rot.z()).
191 : : set__w(eig_rot.w());
192 : : transform.set__rotation(rot);
193 : 424 : }
194 : :
195 : : /// Specialization to convert from the eigen pose to the ros pose type.
196 : : /// `pose_to_transform` template is used as conversion to `RosPose`
197 : : /// is identical to conversion to `RosTransform`
198 : : /// \tparam T Eigen scalar type
199 : : template<typename T>
200 : 25 : void pose_to_transform(
201 : : const EigenPose<T> & pose,
202 : : RosPose & ros_pose)
203 : : {
204 : : static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
205 : 25 : Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
206 : 25 : eig_rot =
207 : : Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
208 : : Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
209 : : Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());
210 : :
211 : : decltype(RosPose::position) trans;
212 : : decltype(RosTransform::rotation) rot;
213 : :
214 : 25 : trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
215 : : ros_pose.set__position(trans);
216 : :
217 : : rot.set__x(eig_rot.x()).
218 : : set__y(eig_rot.y()).
219 : : set__z(eig_rot.z()).
220 : : set__w(eig_rot.w());
221 : : ros_pose.set__orientation(rot);
222 : 25 : }
223 : :
224 : : /// Specialization to convert from the ros pose type to an eigen one.
225 : : /// \tparam T Eigen scalar type
226 : : template<typename T>
227 : 25 : void transform_to_pose(const RosTransform & transform, EigenPose<T> & pose)
228 : : {
229 : : static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
230 : : const auto & ros_rot = transform.rotation;
231 : : const auto & ros_trans = transform.translation;
232 : : Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
233 : 25 : pose(0) = ros_trans.x;
234 : 25 : pose(1) = ros_trans.y;
235 : 25 : pose(2) = ros_trans.z;
236 : :
237 : 25 : const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
238 : 25 : pose(3) = rot(0);
239 : 25 : pose(4) = rot(1);
240 : 25 : pose(5) = rot(2);
241 : 25 : }
242 : :
243 : : /// Specialization to convert from the ros pose type to an eigen one.
244 : : /// `transform_to_pose` template is used as conversion from `RosPose`
245 : : /// is identical to conversion from `RosTransform`
246 : : /// \tparam T Eigen scalar type
247 : : template<typename T>
248 : 20 : void transform_to_pose(const RosPose & ros_pose, EigenPose<T> & pose)
249 : : {
250 : : static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
251 : : const auto & ros_rot = ros_pose.orientation;
252 : : const auto & ros_trans = ros_pose.position;
253 : : Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
254 : 20 : pose(0) = ros_trans.x;
255 : 20 : pose(1) = ros_trans.y;
256 : 20 : pose(2) = ros_trans.z;
257 : :
258 : 20 : const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
259 : 20 : pose(3) = rot(0);
260 : 20 : pose(4) = rot(1);
261 : 20 : pose(5) = rot(2);
262 : 20 : }
263 : :
264 : : } // namespace transform_adapters
265 : : } // namespace ndt
266 : : } // namespace localization
267 : : } // namespace autoware
268 : :
269 : : #endif // NDT__UTILS_HPP_
|