1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 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 #include "dquat2mat.h" 28 #include <iostream> 29 namespace g2o { 30 namespace internal { 31 using namespace std; 32 33 #include "dquat2mat_maxima_generated.cpp" 34 _q2m(number_t & S,number_t & qw,const number_t & r00,const number_t & r10,const number_t & r20,const number_t & r01,const number_t & r11,const number_t & r21,const number_t & r02,const number_t & r12,const number_t & r22)35 int _q2m(number_t& S, number_t& qw, const number_t& r00 , const number_t& r10 , const number_t& r20 , const number_t& r01 , const number_t& r11 , const number_t& r21 , const number_t& r02 , const number_t& r12 , const number_t& r22 ){ 36 number_t tr=r00 + r11 + r22; 37 if (tr > 0) { 38 S = sqrt(tr + 1.0) * 2; // S=4*qw 39 qw = 0.25 * S; 40 // qx = (r21 - r12) / S; 41 // qy = (r02 - r20) / S; 42 // qz = (r10 - r01) / S; 43 return 0; 44 } else if ((r00 > r11)&(r00 > r22)) { 45 S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx 46 qw = (r21 - r12) / S; 47 // qx = 0.25 * S; 48 // qy = (r01 + r10) / S; 49 // qz = (r02 + r20) / S; 50 return 1; 51 } else if (r11 > r22) { 52 S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy 53 qw = (r02 - r20) / S; 54 // qx = (r01 + r10) / S; 55 // qy = 0.25 * S; 56 return 2; 57 } else { 58 S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz 59 qw = (r10 - r01) / S; 60 // qx = (r02 + r20) / S; 61 // qy = (r12 + r21) / S; 62 // qz = 0.25 * S; 63 return 3; 64 } 65 } 66 compute_dq_dR(Eigen::Matrix<number_t,3,9,Eigen::ColMajor> & dq_dR,const number_t & r11,const number_t & r21,const number_t & r31,const number_t & r12,const number_t & r22,const number_t & r32,const number_t & r13,const number_t & r23,const number_t & r33)67 void compute_dq_dR ( Eigen::Matrix<number_t, 3, 9, Eigen::ColMajor>& dq_dR , const number_t& r11 , const number_t& r21 , const number_t& r31 , const number_t& r12 , const number_t& r22 , const number_t& r32 , const number_t& r13 , const number_t& r23 , const number_t& r33 ){ 68 number_t qw; 69 number_t S; 70 int whichCase=_q2m( S, qw, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 71 S*=.25; 72 switch(whichCase){ 73 case 0: compute_dq_dR_w(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 74 break; 75 case 1: compute_dq_dR_x(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 76 break; 77 case 2: compute_dq_dR_y(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 78 break; 79 case 3: compute_dq_dR_z(dq_dR, S, r11 , r21 , r31 , r12 , r22 , r32 , r13 , r23 , r33 ); 80 break; 81 } 82 if (qw<=0) 83 dq_dR *= -1; 84 } 85 } 86 } 87