1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 H. Strasdat
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 //   this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 //   notice, this list of conditions and the following disclaimer in the
13 //   documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #ifndef G2O_SE3QUAT_H_
28 #define G2O_SE3QUAT_H_
29 
30 #include "se3_ops.h"
31 #include "g2o/stuff/misc.h"
32 
33 #include <Eigen/Core>
34 #include <Eigen/Geometry>
35 
36 namespace g2o {
37 
38   class G2O_TYPES_SLAM3D_API SE3Quat {
39     public:
40       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
41 
42     protected:
43 
44       Quaternion _r;
45       Vector3 _t;
46 
47     public:
SE3Quat()48       SE3Quat(){
49         _r.setIdentity();
50         _t.setZero();
51       }
52 
SE3Quat(const Matrix3 & R,const Vector3 & t)53       SE3Quat(const Matrix3& R, const Vector3& t):_r(Quaternion(R)),_t(t){
54         normalizeRotation();
55       }
56 
SE3Quat(const Quaternion & q,const Vector3 & t)57       SE3Quat(const Quaternion& q, const Vector3& t):_r(q),_t(t){
58         normalizeRotation();
59       }
60 
61       /**
62        * templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6 or Map<Vector6>
63        */
64       template <typename Derived>
SE3Quat(const Eigen::MatrixBase<Derived> & v)65         explicit SE3Quat(const Eigen::MatrixBase<Derived>& v)
66         {
67           assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match");
68           if (v.size() == 6) {
69             for (int i=0; i<3; i++){
70               _t[i]=v[i];
71               _r.coeffs()(i)=v[i+3];
72             }
73             _r.w() = 0.; // recover the positive w
74             if (_r.norm()>1.){
75               _r.normalize();
76             } else {
77               number_t w2= cst(1.)-_r.squaredNorm();
78               _r.w()= (w2<cst(0.)) ? cst(0.) : std::sqrt(w2);
79             }
80           }
81           else if (v.size() == 7) {
82             int idx = 0;
83             for (int i=0; i<3; ++i, ++idx)
84               _t(i) = v(idx);
85             for (int i=0; i<4; ++i, ++idx)
86               _r.coeffs()(i) = v(idx);
87             normalizeRotation();
88           }
89         }
90 
translation()91       inline const Vector3& translation() const {return _t;}
92 
setTranslation(const Vector3 & t_)93       inline void setTranslation(const Vector3& t_) {_t = t_;}
94 
rotation()95       inline const Quaternion& rotation() const {return _r;}
96 
setRotation(const Quaternion & r_)97       void setRotation(const Quaternion& r_) {_r=r_;}
98 
99       inline SE3Quat operator* (const SE3Quat& tr2) const{
100         SE3Quat result(*this);
101         result._t += _r*tr2._t;
102         result._r*=tr2._r;
103         result.normalizeRotation();
104         return result;
105       }
106 
107       inline SE3Quat& operator*= (const SE3Quat& tr2){
108         _t+=_r*tr2._t;
109         _r*=tr2._r;
110         normalizeRotation();
111         return *this;
112       }
113 
114       inline Vector3 operator* (const Vector3& v) const {
115         return _t+_r*v;
116       }
117 
inverse()118       inline SE3Quat inverse() const{
119         SE3Quat ret;
120         ret._r=_r.conjugate();
121         ret._t=ret._r*(_t*-cst(1.));
122         return ret;
123       }
124 
125       inline number_t operator [](int i) const {
126         assert(i<7);
127         if (i<3)
128           return _t[i];
129         return _r.coeffs()[i-3];
130       }
131 
132 
toVector()133       inline Vector7 toVector() const{
134         Vector7 v;
135         v[0]=_t(0);
136         v[1]=_t(1);
137         v[2]=_t(2);
138         v[3]=_r.x();
139         v[4]=_r.y();
140         v[5]=_r.z();
141         v[6]=_r.w();
142         return v;
143       }
144 
fromVector(const Vector7 & v)145       inline void fromVector(const Vector7& v){
146         _r=Quaternion(v[6], v[3], v[4], v[5]);
147         _t=Vector3(v[0], v[1], v[2]);
148       }
149 
toMinimalVector()150       inline Vector6 toMinimalVector() const{
151         Vector6 v;
152         v[0]=_t(0);
153         v[1]=_t(1);
154         v[2]=_t(2);
155         v[3]=_r.x();
156         v[4]=_r.y();
157         v[5]=_r.z();
158         return v;
159       }
160 
fromMinimalVector(const Vector6 & v)161       inline void fromMinimalVector(const Vector6& v){
162         number_t w = cst(1.)-v[3]*v[3]-v[4]*v[4]-v[5]*v[5];
163         if (w>0){
164           _r=Quaternion(std::sqrt(w), v[3], v[4], v[5]);
165         } else {
166           _r=Quaternion(0, -v[3], -v[4], -v[5]);
167         }
168         _t=Vector3(v[0], v[1], v[2]);
169       }
170 
171 
172 
log()173       Vector6 log() const {
174         Vector6 res;
175         Matrix3 _R = _r.toRotationMatrix();
176         number_t d = cst(0.5)*(_R(0,0)+_R(1,1)+_R(2,2)-1);
177         Vector3 omega;
178         Vector3 upsilon;
179 
180 
181         Vector3 dR = deltaR(_R);
182         Matrix3 V_inv;
183 
184         if (std::abs(d)>cst(0.99999))
185         {
186 
187           omega=0.5*dR;
188           Matrix3 Omega = skew(omega);
189           V_inv = Matrix3::Identity()- cst(0.5)*Omega + (cst(1.)/ cst(12.))*(Omega*Omega);
190         }
191         else
192         {
193           number_t theta = std::acos(d);
194           omega = theta/(2*std::sqrt(1-d*d))*dR;
195           Matrix3 Omega = skew(omega);
196           V_inv = ( Matrix3::Identity() - cst(0.5)*Omega
197               + ( 1-theta/(2*std::tan(theta/2)))/(theta*theta)*(Omega*Omega) );
198         }
199 
200         upsilon = V_inv*_t;
201         for (int i=0; i<3;i++){
202           res[i]=omega[i];
203         }
204         for (int i=0; i<3;i++){
205           res[i+3]=upsilon[i];
206         }
207 
208         return res;
209 
210       }
211 
map(const Vector3 & xyz)212       Vector3 map(const Vector3 & xyz) const
213       {
214         return _r*xyz + _t;
215       }
216 
217 
exp(const Vector6 & update)218       static SE3Quat exp(const Vector6 & update)
219       {
220         Vector3 omega;
221         for (int i=0; i<3; i++)
222           omega[i]=update[i];
223         Vector3 upsilon;
224         for (int i=0; i<3; i++)
225           upsilon[i]=update[i+3];
226 
227         number_t theta = omega.norm();
228         Matrix3 Omega = skew(omega);
229 
230         Matrix3 R;
231         Matrix3 V;
232         if (theta<cst(0.00001))
233         {
234           Matrix3 Omega2 = Omega*Omega;
235 
236           R = (Matrix3::Identity()
237               + Omega
238               + cst(0.5) * Omega2);
239 
240           V = (Matrix3::Identity()
241               + cst(0.5) * Omega
242               + cst(1.) / cst(6.) * Omega2);
243         }
244         else
245         {
246           Matrix3 Omega2 = Omega*Omega;
247 
248           R = (Matrix3::Identity()
249               + std::sin(theta)/theta *Omega
250               + (1-std::cos(theta))/(theta*theta)*Omega2);
251 
252           V = (Matrix3::Identity()
253               + (1-std::cos(theta))/(theta*theta)*Omega
254               + (theta-std::sin(theta))/(std::pow(theta,3))*Omega2);
255         }
256         return SE3Quat(Quaternion(R),V*upsilon);
257       }
258 
adj()259       Eigen::Matrix<number_t, 6, 6, Eigen::ColMajor> adj() const
260       {
261         Matrix3 R = _r.toRotationMatrix();
262         Eigen::Matrix<number_t, 6, 6, Eigen::ColMajor> res;
263         res.block(0,0,3,3) = R;
264         res.block(3,3,3,3) = R;
265         res.block(3,0,3,3) = skew(_t)*R;
266         res.block(0,3,3,3) = Matrix3::Zero(3,3);
267         return res;
268       }
269 
to_homogeneous_matrix()270       Eigen::Matrix<number_t,4,4,Eigen::ColMajor> to_homogeneous_matrix() const
271       {
272         Eigen::Matrix<number_t,4,4,Eigen::ColMajor> homogeneous_matrix;
273         homogeneous_matrix.setIdentity();
274         homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
275         homogeneous_matrix.col(3).head(3) = translation();
276 
277         return homogeneous_matrix;
278       }
279 
normalizeRotation()280       void normalizeRotation(){
281         if (_r.w()<0){
282           _r.coeffs() *= -1;
283         }
284         _r.normalize();
285       }
286 
287       /**
288        * cast SE3Quat into an Isometry3
289        */
Isometry3()290       operator Isometry3() const
291       {
292         Isometry3 result = (Isometry3) rotation();
293         result.translation() = translation();
294         return result;
295       }
296   };
297 
298   inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3)
299   {
300     out_str << se3.to_homogeneous_matrix()  << std::endl;
301     return out_str;
302   }
303 
304   //G2O_TYPES_SLAM3D_API Quaternion euler_to_quat(number_t yaw, number_t pitch, number_t roll);
305   //G2O_TYPES_SLAM3D_API void quat_to_euler(const Quaternion& q, number_t& yaw, number_t& pitch, number_t& roll);
306   //G2O_TYPES_SLAM3D_API void jac_quat3_euler3(Eigen::Matrix<number_t, 6, 6, Eigen::ColMajor>& J, const SE3Quat& t);
307 
308 } // end namespace
309 
310 #endif
311