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