MIRA
TransformCast.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 by
3  * MetraLabs GmbH (MLAB), GERMANY
4  * and
5  * Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
6  * All rights reserved.
7  *
8  * Contact: info@mira-project.org
9  *
10  * Commercial Usage:
11  * Licensees holding valid commercial licenses may use this file in
12  * accordance with the commercial license agreement provided with the
13  * software or, alternatively, in accordance with the terms contained in
14  * a written agreement between you and MLAB or NICR.
15  *
16  * GNU General Public License Usage:
17  * Alternatively, this file may be used under the terms of the GNU
18  * General Public License version 3.0 as published by the Free Software
19  * Foundation and appearing in the file LICENSE.GPL3 included in the
20  * packaging of this file. Please review the following information to
21  * ensure the GNU General Public License version 3.0 requirements will be
22  * met: http://www.gnu.org/copyleft/gpl.html.
23  * Alternatively you may (at your option) use any later version of the GNU
24  * General Public License if such license has been publicly approved by
25  * MLAB and NICR (or its successors, if any).
26  *
27  * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
28  * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
29  * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
30  * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32  * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
33  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
34  * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
35  * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
36  * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
37  */
38 
47 #ifndef _MIRA_TRANSFORMCAST_H__
48 #define _MIRA_TRANSFORMCAST_H__
49 
51 #include <math/YawPitchRoll.h>
52 
53 namespace mira {
54 
56 
58 
60 template<typename TargetTransform,
61  typename TargetT, int TargetDim,
62  typename SourceTransform,
63  typename SourceT, int SourceDim>
64 struct TransformCast {
65  static TargetTransform invoke(const SourceTransform& other) {
66  RigidTransform<TargetT,SourceDim> tfWithTargetT(other.t.template cast<TargetT>(), other.r.template cast<TargetT>());
67  return TransformCast<RigidTransform<TargetT,TargetDim>, TargetT, TargetDim,
68  RigidTransform<TargetT,SourceDim>, TargetT, SourceDim>::invoke(tfWithTargetT);
69  }
70 };
71 
75 template<typename TargetT, int TargetDim,
76  typename SourceTransform,
77  typename SourceT, int SourceDim>
78 struct TransformCast<RigidTransformCov<TargetT, TargetDim>, TargetT, TargetDim, SourceTransform, SourceT, SourceDim> {
79  static RigidTransformCov<TargetT, TargetDim> invoke(const SourceTransform& other) {
80  RigidTransformCov<TargetT,SourceDim> tfWithTargetT(other.template cast<TargetT>());
81  return TransformCast<RigidTransformCov<TargetT, TargetDim>, TargetT, TargetDim,
82  RigidTransformCov<TargetT,SourceDim>, TargetT, SourceDim>::invoke(tfWithTargetT);
83  }
84 };
85 
86 // RigidTransform and RigidTransformCov with same T and Dim need separate specializations,
87 // otherwise some uses are ambiguous between this and the previous one
88 
90 template<typename T, int Dim>
91 struct TransformCast<RigidTransform<T, Dim>, T, Dim, RigidTransform<T, Dim>, T, Dim> {
92  static RigidTransform<T, Dim> invoke(const RigidTransform<T, Dim>& other) {
93  return other;
94  }
95 };
96 
98 template<typename T, int Dim>
99 struct TransformCast<RigidTransformCov<T, Dim>, T, Dim, RigidTransformCov<T, Dim>, T, Dim> {
100  static RigidTransformCov<T, Dim> invoke(const RigidTransformCov<T, Dim>& other) {
101  return other;
102  }
103 };
104 
105 // specializations for valid combinations of transforms:
106 
107 // converts from 3D RigidTransform to 2D RigidTransform
108 template<typename T>
109 struct TransformCast<RigidTransform<T,2>, T, 2, RigidTransform<T,3>, T, 3> {
110  static RigidTransform<T,2> invoke(const RigidTransform<T,3>& other) {
111  // extract: yaw (phi), pitch (theta), roll (psi)
112  Eigen::Matrix<T,3,1> euler =
113  eulerAngles(other.r.toRotationMatrix(), 2, 1, 0);
114  return RigidTransform<T,2>(other.x(), other.y(), euler(0,0) );
115  }
116 };
117 
118 // converts from 2D RigidTransform to 3D RigidTransform, z and the two
119 // additional euler angles are set to 0
120 template<typename T>
121 struct TransformCast<RigidTransform<T,3>, T, 3, RigidTransform<T,2>, T, 2> {
122  static RigidTransform<T,3> invoke(const RigidTransform<T,2>& other) {
123  return RigidTransform<T,3>(Eigen::Matrix<T,3,1>(other.x(), other.y(),0),
126  }
127 };
128 
129 // converts from 3D RigidTransformCov to 2D RigidTransformCov
130 template<typename T>
131 struct TransformCast<RigidTransformCov<T,2>, T, 2, RigidTransformCov<T,3>, T, 3> {
132  static RigidTransformCov<T,2> invoke(const RigidTransformCov<T,3>& other) {
133  // extract: yaw (phi), pitch (theta), roll (psi)
134  Eigen::Matrix<T,3,1> euler =
135  eulerAngles(other.r.toRotationMatrix(), 2, 1, 0);
136 
138 
140  quaternionCovToYawPitchRollCov(other.cov, other.r);
141 
142  // copy 2x2 translation cov part: xy-xy
143  cov.template block<2,2>(0,0) = otherEulerCov.template block<2,2>(0,0);
144  // xy-phi
145  cov.template block<2,1>(0,2) = otherEulerCov.template block<2,1>(0,3);
146  // xy-phi
147  cov.template block<1,2>(2,0) = otherEulerCov.template block<1,2>(3,0);
148  // phi-phi
149  cov(2,2) = otherEulerCov(3,3);
150 
151  return RigidTransformCov<T,2>(other.x(), other.y(), euler(0,0), cov);
152  }
153 };
154 
155 // converts from 2D RigidTransformCov to 3D RigidTransformCov,
156 // z and the two additional euler angles are set to 0
157 template<typename T>
158 struct TransformCast<RigidTransformCov<T,3>, T, 3, RigidTransformCov<T,2>, T, 2> {
159  static RigidTransformCov<T,3> invoke(const RigidTransformCov<T,2>& other) {
162 
163  // copy 2x2 translation cov part: xy-xy
164  eulerCov.template block<2,2>(0,0) = other.cov.template block<2,2>(0,0);
165  // xy-phi
166  eulerCov.template block<2,1>(0,3) = other.cov.template block<2,1>(0,2);
167  // xy-phi
168  eulerCov.template block<1,2>(3,0) = other.cov.template block<1,2>(2,0);
169  // phi-phi
170  eulerCov(3,3) = other.cov(2,2);
171 
174 
175  return RigidTransformCov<T, 3>(
176  Eigen::Matrix<T, 3, 1>(other.x(), other.y(), 0),
177  orientation, eulerCov);
178  }
179 };
180 
181 // matches "conversions" from the corresponding RigidTransform to
182 // RigidTransformCov type
183 template<typename T, int D>
184 struct TransformCast<RigidTransformCov<T,D>, T, D, RigidTransform<T,D>, T, D> {
185  static RigidTransformCov<T,D> invoke(const RigidTransform<T,D>& other) {
186  return RigidTransformCov<T,D>(other);
187  }
188 };
189 
190 // matches "conversions" from the corresponding RigidTransformCov to
191 // RigidTransform type
192 template<typename T, int D>
193 struct TransformCast<RigidTransform<T,D>, T, D, RigidTransformCov<T,D>, T, D> {
194  static RigidTransform<T,D> invoke(const RigidTransformCov<T,D>& other) {
195  return RigidTransform<T,D>(other);
196  }
197 };
198 
199 // matches "conversions" from the RigidTransform type of different
200 // dimensionality to RigidTransformCov
201 template<typename T, int TargetD, int SourceD>
202 struct TransformCast<RigidTransformCov<T,TargetD>, T, TargetD,
203  RigidTransform<T,SourceD>, T, SourceD> {
204  static RigidTransformCov<T,TargetD> invoke(const RigidTransform<T,SourceD>& other) {
205  // first cast RigidTransform to the target dimensionality
206  typedef TransformCast<RigidTransform<T,TargetD>, T, TargetD,
207  RigidTransform<T,SourceD>, T, SourceD> Cast;
208  RigidTransform<T,TargetD> tmp = Cast::invoke(other);
209 
210  // finally create a RigidTransformCov from it
211  return RigidTransformCov<T,TargetD>(tmp);
212  }
213 };
214 
215 // matches "conversions" from the RigidTransformCov type of different
216 // dimensionality to RigidTransform
217 template<typename T, int TargetD, int SourceD>
218 struct TransformCast<RigidTransform<T,TargetD>, T, TargetD,
219  RigidTransformCov<T,SourceD>, T, SourceD> {
220  static RigidTransform<T,TargetD> invoke(const RigidTransformCov<T,SourceD>& other) {
221  // cast RigidTransform to the target dimensionality
222  typedef TransformCast<RigidTransform<T,TargetD>, T, TargetD,
223  RigidTransform<T,SourceD>, T, SourceD> Cast;
224  return Cast::invoke(other);
225  }
226 };
227 
229 
231 
248 template<typename TargetTransform, typename OtherT, int OtherD>
249 TargetTransform transform_cast(const RigidTransform<OtherT,OtherD>& other)
250 {
251  return TransformCast<TargetTransform, typename TargetTransform::Type, TargetTransform::Dim,
252  RigidTransform<OtherT,OtherD>, OtherT, OtherD>::invoke(other);
253 }
254 
269 template<typename TargetTransform, typename OtherT, int OtherD>
271 {
272  return TransformCast<TargetTransform, typename TargetTransform::Type, TargetTransform::Dim,
273  RigidTransformCov<OtherT,OtherD>, OtherT, OtherD>::invoke(other);
274 }
275 
277 
278 } // namespace
279 
280 #endif
TargetTransform transform_cast(const RigidTransform< OtherT, OtherD > &other)
Casts a RigidTransform or RigidTransformCov to a RigidTransform of a different type.
Definition: TransformCast.h:249
Eigen::Matrix< T, 6, 6 > quaternionCovToYawPitchRollCov(const Eigen::Matrix< T, 7, 7 > &covariance, const Eigen::Quaternion< T > &q)
Converts a 7x7 dimensional quaternion covariance (3D + Quaternion) back to a 6x6 dimensional euler co...
Definition: YawPitchRoll.h:318
specialize cv::DataType for our ImgPixel and inherit from cv::DataType<Vec>
Definition: IOService.h:67
This class represents an affine transformation that supports a translation followed by a rotation (a ...
Definition: RigidTransform.h:126
Eigen::Matrix< T, CovarianceDim, CovarianceDim > CovMatrixType
The type of the matrix representing the covariance of the transformation.
Definition: RigidTransform.h:415
Eigen::Matrix< T, EulerCovarianceDim, EulerCovarianceDim > YawPitchRollCovMatrixType
The type of the matrix representing the euler covariance of the transformation.
Definition: RigidTransform.h:782
Conversion from yaw, pitch and roll to Quaternion and vice versa.
Rigid transformation class.
static YawPitchRollCovMatrixType nullYawPitchRollCov()
Returns an "null covariance matrix" witch yaw, pitch, roll rotation representation.
Definition: RigidTransform.h:797
Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 > eulerAngles(const Eigen::MatrixBase< Derived > &mat, typename Eigen::MatrixBase< Derived >::Index a0, typename Eigen::MatrixBase< Derived >::Index a1, typename Eigen::MatrixBase< Derived >::Index a2)
Returns the Euler-angles of the rotation matrix mat using the convention defined by the triplet (a0...
Definition: YawPitchRoll.h:86
This class represents an affine transformation that supports a translation followed by a rotation (a ...
Definition: RigidTransform.h:99