se3quat_gradients.cpp
Go to the documentation of this file.
00001 // g2o - General Graph Optimization
00002 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
00003 // 
00004 // g2o is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // g2o is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include <Eigen/Core>
00018 
00019 namespace g2o {
00020   using namespace Eigen;
00021 
00022 void  jacobian_3d_qman ( Matrix< double, 6 , 6> &  Ji , Matrix< double, 6 , 6> &  Jj,
00023     const double&  z11 , const double&  z12 , const double&  z13 , const double&  z14 , const double&  z21 , const double&  z22 , const double&  z23 , const double&  z24 , const double&  z31 , const double&  z32 , const double&  z33 , const double&  z34 , const double&  xab11 , const double&  xab12 , const double&  xab13 , const double&  xab14 , const double&  xab21 , const double&  xab22 , const double&  xab23 , const double&  xab24 , const double&  xab31 , const double&  xab32 , const double&  xab33 , const double&  xab34 ) { 
00024   (void) z14;
00025   (void) z24;
00026   (void) z34;
00027   double  _aux1 = xab11*z11 ; 
00028   double  _aux2 = xab21*z12 ; 
00029   double  _aux3 = xab31*z13 ; 
00030   double  _aux4 = xab13*z11 ; 
00031   double  _aux5 = xab23*z12 ; 
00032   double  _aux6 = xab33*z13 ; 
00033   double  _aux7 = xab11*z21 ; 
00034   double  _aux8 = xab21*z22 ; 
00035   double  _aux9 = xab31*z23 ; 
00036   double  _aux10 = xab12*z21 ; 
00037   double  _aux11 = xab22*z22 ; 
00038   double  _aux12 = xab32*z23 ; 
00039   double  _aux13 = xab12*z31 ; 
00040   double  _aux14 = xab22*z32 ; 
00041   double  _aux15 = xab32*z33 ; 
00042   double  _aux16 = xab13*z31 ; 
00043   double  _aux17 = xab23*z32 ; 
00044   double  _aux18 = xab33*z33 ; 
00045   double  _aux19 = _aux15+_aux14+_aux13-xab33*z23-xab23*z22-xab13*z21 ; 
00046   double  _aux20 = -2*xab21*z23+2*xab31*z22+2*xab22*z13-2*xab32*z12 ; 
00047   double  _aux21 = _aux9+_aux8+_aux7-xab32*z13-xab22*z12-xab12*z11 ; 
00048   double  _aux22 = 2*xab21*z33-2*xab31*z32-2*xab23*z13+2*xab33*z12 ; 
00049   double  _aux23 = _aux6+_aux5+_aux4-xab31*z33-xab21*z32-xab11*z31 ; 
00050   double  _aux24 = -2*xab22*z33+2*xab32*z32+2*xab23*z23-2*xab33*z22 ; 
00051   double  _aux25 = _aux3+_aux2+_aux18+_aux17+_aux16+_aux12+_aux11+_aux10+_aux1+1 ; 
00052   double  _aux26 = 2*(-2*xab23*z33+2*xab33*z32-2*xab22*z23+2*xab32*z22-2*xab21*z13+2*xab31*z12)*_aux25+2*_aux19*_aux24+2*_aux22*_aux23+2*_aux20*_aux21 ; 
00053   double  _aux27 = sqrt(pow(_aux25,2)+pow(_aux23,2)+pow(_aux21,2)+pow(_aux19,2)) ; 
00054   double  _aux28 = 1/pow(_aux27,3) ; 
00055   double  _aux29 = 1/_aux27 ; 
00056   double  _aux30 = 2*xab11*z23-2*xab31*z21-2*xab12*z13+2*xab32*z11 ; 
00057   double  _aux31 = -2*xab11*z33+2*xab31*z31+2*xab13*z13-2*xab33*z11 ; 
00058   double  _aux32 = 2*xab12*z33-2*xab32*z31-2*xab13*z23+2*xab33*z21 ; 
00059   double  _aux33 = 2*_aux19*_aux32+2*_aux23*_aux31+2*_aux21*_aux30+2*(2*xab13*z33-2*xab33*z31+2*xab12*z23-2*xab32*z21+2*xab11*z13-2*xab31*z11)*_aux25 ; 
00060   double  _aux34 = -2*xab11*z22+2*xab21*z21+2*xab12*z12-2*xab22*z11 ; 
00061   double  _aux35 = 2*xab11*z32-2*xab21*z31-2*xab13*z12+2*xab23*z11 ; 
00062   double  _aux36 = -2*xab12*z32+2*xab22*z31+2*xab13*z22-2*xab23*z21 ; 
00063   double  _aux37 = 2*_aux19*_aux36+2*_aux23*_aux35+2*_aux21*_aux34+2*(-2*xab13*z32+2*xab23*z31-2*xab12*z22+2*xab22*z21-2*xab11*z12+2*xab21*z11)*_aux25 ; 
00064   double  _aux38 = 2*xab12*z21 ; 
00065   double  _aux39 = 2*xab22*z22 ; 
00066   double  _aux40 = 2*xab32*z23 ; 
00067   double  _aux41 = 2*xab13*z31 ; 
00068   double  _aux42 = 2*xab23*z32 ; 
00069   double  _aux43 = 2*xab33*z33 ; 
00070   double  _aux44 = _aux43+_aux42+_aux41+_aux40+_aux39+_aux38 ; 
00071   double  _aux45 = -2*xab13*z11 ; 
00072   double  _aux46 = -2*xab23*z12 ; 
00073   double  _aux47 = -2*xab33*z13 ; 
00074   double  _aux48 = _aux47+_aux46+_aux45 ; 
00075   double  _aux49 = -2*xab32*z13-2*xab22*z12-2*xab12*z11 ; 
00076   double  _aux50 = -2*xab12*z31 ; 
00077   double  _aux51 = -2*xab22*z32 ; 
00078   double  _aux52 = -2*xab32*z33 ; 
00079   double  _aux53 = 2*_aux25*(_aux52+_aux51+_aux50+2*xab33*z23+2*xab23*z22+2*xab13*z21)+2*_aux23*_aux49+2*_aux21*_aux48+2*_aux19*_aux44 ; 
00080   double  _aux54 = -2*xab11*z21 ; 
00081   double  _aux55 = -2*xab21*z22 ; 
00082   double  _aux56 = -2*xab31*z23 ; 
00083   double  _aux57 = _aux56+_aux55+_aux54 ; 
00084   double  _aux58 = -2*xab33*z23-2*xab23*z22-2*xab13*z21 ; 
00085   double  _aux59 = 2*xab11*z11 ; 
00086   double  _aux60 = 2*xab21*z12 ; 
00087   double  _aux61 = 2*xab31*z13 ; 
00088   double  _aux62 = _aux61+_aux60+_aux59+_aux43+_aux42+_aux41 ; 
00089   double  _aux63 = 2*_aux23*_aux62+2*_aux21*_aux58+2*_aux19*_aux57+2*_aux25*(_aux47+_aux46+_aux45+2*xab31*z33+2*xab21*z32+2*xab11*z31) ; 
00090   double  _aux64 = _aux61+_aux60+_aux59+_aux40+_aux39+_aux38 ; 
00091   double  _aux65 = _aux52+_aux51+_aux50 ; 
00092   double  _aux66 = -2*xab31*z33-2*xab21*z32-2*xab11*z31 ; 
00093   double  _aux67 = 2*_aux19*_aux66+2*_aux23*_aux65+2*_aux21*_aux64+2*_aux25*(_aux56+_aux55+_aux54+2*xab32*z13+2*xab22*z12+2*xab12*z11) ; 
00094 
00095   Ji ( 0 , 0 ) = -z11 ; 
00096   Ji ( 0 , 1 ) = -z12 ; 
00097   Ji ( 0 , 2 ) = -z13 ; 
00098   Ji ( 0 , 3 ) = 2*xab34*z12-2*xab24*z13 ; 
00099   Ji ( 0 , 4 ) = 2*xab14*z13-2*xab34*z11 ; 
00100   Ji ( 0 , 5 ) = 2*xab24*z11-2*xab14*z12 ; 
00101   Ji ( 1 , 0 ) = -z21 ; 
00102   Ji ( 1 , 1 ) = -z22 ; 
00103   Ji ( 1 , 2 ) = -z23 ; 
00104   Ji ( 1 , 3 ) = 2*xab34*z22-2*xab24*z23 ; 
00105   Ji ( 1 , 4 ) = 2*xab14*z23-2*xab34*z21 ; 
00106   Ji ( 1 , 5 ) = 2*xab24*z21-2*xab14*z22 ; 
00107   Ji ( 2 , 0 ) = -z31 ; 
00108   Ji ( 2 , 1 ) = -z32 ; 
00109   Ji ( 2 , 2 ) = -z33 ; 
00110   Ji ( 2 , 3 ) = 2*xab34*z32-2*xab24*z33 ; 
00111   Ji ( 2 , 4 ) = 2*xab14*z33-2*xab34*z31 ; 
00112   Ji ( 2 , 5 ) = 2*xab24*z31-2*xab14*z32 ; 
00113   Ji ( 3 , 0 ) = 0 ; 
00114   Ji ( 3 , 1 ) = 0 ; 
00115   Ji ( 3 , 2 ) = 0 ; 
00116   Ji ( 3 , 3 ) = _aux24*_aux29-0.5*_aux19*_aux26*_aux28 ; 
00117   Ji ( 3 , 4 ) = _aux29*_aux32-0.5*_aux19*_aux28*_aux33 ; 
00118   Ji ( 3 , 5 ) = _aux29*_aux36-0.5*_aux19*_aux28*_aux37 ; 
00119   Ji ( 4 , 0 ) = 0 ; 
00120   Ji ( 4 , 1 ) = 0 ; 
00121   Ji ( 4 , 2 ) = 0 ; 
00122   Ji ( 4 , 3 ) = _aux22*_aux29-0.5*_aux23*_aux26*_aux28 ; 
00123   Ji ( 4 , 4 ) = _aux29*_aux31-0.5*_aux23*_aux28*_aux33 ; 
00124   Ji ( 4 , 5 ) = _aux29*_aux35-0.5*_aux23*_aux28*_aux37 ; 
00125   Ji ( 5 , 0 ) = 0 ; 
00126   Ji ( 5 , 1 ) = 0 ; 
00127   Ji ( 5 , 2 ) = 0 ; 
00128   Ji ( 5 , 3 ) = _aux20*_aux29-0.5*_aux21*_aux26*_aux28 ; 
00129   Ji ( 5 , 4 ) = _aux29*_aux30-0.5*_aux21*_aux28*_aux33 ; 
00130   Ji ( 5 , 5 ) = _aux29*_aux34-0.5*_aux21*_aux28*_aux37 ; 
00131 
00132   Jj( 0 , 0 ) = _aux3+_aux2+_aux1 ; 
00133   Jj( 0 , 1 ) = xab32*z13+xab22*z12+xab12*z11 ; 
00134   Jj( 0 , 2 ) = _aux6+_aux5+_aux4 ; 
00135   Jj( 0 , 3 ) = 0 ; 
00136   Jj( 0 , 4 ) = 0 ; 
00137   Jj( 0 , 5 ) = 0 ; 
00138   Jj( 1 , 0 ) = _aux9+_aux8+_aux7 ; 
00139   Jj( 1 , 1 ) = _aux12+_aux11+_aux10 ; 
00140   Jj( 1 , 2 ) = xab33*z23+xab23*z22+xab13*z21 ; 
00141   Jj( 1 , 3 ) = 0 ; 
00142   Jj( 1 , 4 ) = 0 ; 
00143   Jj( 1 , 5 ) = 0 ; 
00144   Jj( 2 , 0 ) = xab31*z33+xab21*z32+xab11*z31 ; 
00145   Jj( 2 , 1 ) = _aux15+_aux14+_aux13 ; 
00146   Jj( 2 , 2 ) = _aux18+_aux17+_aux16 ; 
00147   Jj( 2 , 3 ) = 0 ; 
00148   Jj( 2 , 4 ) = 0 ; 
00149   Jj( 2 , 5 ) = 0 ; 
00150   Jj( 3 , 0 ) = 0 ; 
00151   Jj( 3 , 1 ) = 0 ; 
00152   Jj( 3 , 2 ) = 0 ; 
00153   Jj( 3 , 3 ) = _aux29*_aux44-0.5*_aux19*_aux28*_aux53 ; 
00154   Jj( 3 , 4 ) = _aux29*_aux57-0.5*_aux19*_aux28*_aux63 ; 
00155   Jj( 3 , 5 ) = _aux29*_aux66-0.5*_aux19*_aux28*_aux67 ; 
00156   Jj( 4 , 0 ) = 0 ; 
00157   Jj( 4 , 1 ) = 0 ; 
00158   Jj( 4 , 2 ) = 0 ; 
00159   Jj( 4 , 3 ) = _aux29*_aux49-0.5*_aux23*_aux28*_aux53 ; 
00160   Jj( 4 , 4 ) = _aux29*_aux62-0.5*_aux23*_aux28*_aux63 ; 
00161   Jj( 4 , 5 ) = _aux29*_aux65-0.5*_aux23*_aux28*_aux67 ; 
00162   Jj( 5 , 0 ) = 0 ; 
00163   Jj( 5 , 1 ) = 0 ; 
00164   Jj( 5 , 2 ) = 0 ; 
00165   Jj( 5 , 3 ) = _aux29*_aux48-0.5*_aux21*_aux28*_aux53 ; 
00166   Jj( 5 , 4 ) = _aux29*_aux58-0.5*_aux21*_aux28*_aux63 ; 
00167   Jj( 5 , 5 ) = _aux29*_aux64-0.5*_aux21*_aux28*_aux67 ; 
00168 }
00169 
00170 }


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:32:24