hogman_minimal: hogman_minimal: posegraph3d

00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 // 
00004 // HOG-Man 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 // HOG-Man 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 #ifndef POSE_GRAPH_3D_GRADIENT_H
00018 #define POSE_GRAPH_3D_GRADIENT_H
00019 
00020 #include "hogman_minimal/math/transformation.h"
00021 #include <cmath>
00022 
00023 namespace AISNavigation {
00024 
00025 inline void manifoldGradientXi(Matrix6& mat, const Vector6& e, const Vector6& xi, const Vector6& xj)
00026 {
00027   //const double& ex  = e[0];
00028   //const double& ey  = e[1];
00029   //const double& ez  = e[2];
00030   const double& er  = e[3];
00031   const double& ep  = e[4];
00032   const double& eya = e[5];
00033 
00034   const double& x1 = xi[0];
00035   const double& y1 = xi[1];
00036   const double& z1 = xi[2];
00037   const double& a1 = xi[3];
00038   const double& b1 = xi[4];
00039   const double& c1 = xi[5];
00040 
00041   const double& x2 = xj[0];
00042   const double& y2 = xj[1];
00043   const double& z2 = xj[2];
00044   const double& a2 = xj[3];
00045   const double& b2 = xj[4];
00046   const double& c2 = xj[5];
00047 
00048   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 , aux_12 ,
00049          aux_13 , aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 , aux_21 , aux_22 , aux_23 , aux_24 ,
00050          aux_25 , aux_26 , aux_27 , aux_28 , aux_29 , aux_30 , aux_31 , aux_32 , aux_33 , aux_34 , aux_35 , aux_36 ,
00051          aux_37 , aux_38 , aux_39 , aux_40 , aux_41 , aux_42 , aux_43 , aux_44 , aux_45 , aux_46 , aux_47 , aux_48 ,
00052          aux_49 , aux_50 , aux_51 , aux_52 , aux_53 , aux_54 , aux_55 , aux_56 , aux_57 , aux_58 , aux_59 , aux_60 ,
00053          aux_61 , aux_62 , aux_63 , aux_64 ;
00054   aux_1 = cos(a1) ;
00055   aux_2 = sin(b1) ;
00056   aux_3 = cos(c1) ;
00057   aux_4 = sin(a1) ;
00058   aux_5 = sin(c1) ;
00059   aux_6 = -14745600*aux_4*aux_5-14745600*aux_1*aux_2*aux_3 ;
00060   aux_7 = sin(ep) ;
00061   aux_8 = cos(b1) ;
00062   aux_9 = cos(ep) ;
00063   aux_10 = cos(eya) ;
00064   aux_11 = 14745600*aux_4*aux_2*aux_3-14745600*aux_1*aux_5 ;
00065   aux_12 = sin(eya) ;
00066   aux_13 = -14745600*aux_4*aux_2*aux_5-14745600*aux_1*aux_3 ;
00067   aux_14 = -aux_4*aux_5-aux_1*aux_2*aux_3 ;
00068   aux_15 = aux_4*aux_5+aux_1*aux_2*aux_3 ;
00069   aux_16 = aux_1*aux_2*aux_5-aux_4*aux_3 ;
00070   aux_17 = aux_1*aux_8*z2-aux_1*aux_8*z1+aux_16*y2-aux_16*y1+aux_15*x2+aux_14*x1 ;
00071   aux_18 = aux_4*aux_2*aux_3-aux_1*aux_5 ;
00072   aux_19 = aux_1*aux_5-aux_4*aux_2*aux_3 ;
00073   aux_20 = -aux_4*aux_2*aux_5-aux_1*aux_3 ;
00074   aux_21 = -aux_4*aux_8*z2+aux_4*aux_8*z1+aux_20*y2-aux_20*y1+aux_19*x2+aux_18*x1 ;
00075   aux_22 = aux_4*aux_3-aux_1*aux_2*aux_5 ;
00076   aux_23 = -aux_1*aux_8*z2+aux_1*aux_8*z1+aux_22*y2-aux_22*y1+aux_14*x2+aux_15*x1 ;
00077   aux_24 = -aux_2*z2+aux_2*z1+aux_8*aux_5*y2-aux_8*aux_5*y1+aux_8*aux_3*x2-aux_8*aux_3*x1 ;
00078   aux_25 = aux_4*aux_2*aux_5+aux_1*aux_3 ;
00079   aux_26 = aux_4*aux_8*z2-aux_4*aux_8*z1+aux_25*y2-aux_25*y1+aux_18*x2+aux_19*x1 ;
00080   aux_27 = aux_2*z2-aux_2*z1-aux_8*aux_5*y2+aux_8*aux_5*y1-aux_8*aux_3*x2+aux_8*aux_3*x1 ;
00081   aux_28 = sin(er) ;
00082   aux_29 = cos(er) ;
00083   aux_30 = 14745600*aux_4*aux_3-14745600*aux_1*aux_2*aux_5 ;
00084   aux_31 = aux_7*aux_28*aux_12+aux_29*aux_10 ;
00085   aux_32 = aux_7*aux_28*aux_10-aux_29*aux_12 ;
00086   aux_33 = aux_7*aux_29*aux_12-aux_28*aux_10 ;
00087   aux_34 = aux_28*aux_12+aux_7*aux_29*aux_10 ;
00088   aux_35 = 0 ;
00089   aux_36 = sin(a2) ;
00090   aux_37 = cos(b2) ;
00091   aux_38 = sin(b2) ;
00092   aux_39 = cos(c2) ;
00093   aux_40 = cos(a2) ;
00094   aux_41 = sin(c2) ;
00095   aux_42 = aux_36*aux_38*aux_39-aux_40*aux_41 ;
00096   aux_43 = aux_36*aux_38*aux_41+aux_40*aux_39 ;
00097   aux_44 = aux_16*aux_43+aux_15*aux_42+aux_1*aux_36*aux_8*aux_37 ;
00098   aux_45 = aux_36*aux_41+aux_40*aux_38*aux_39 ;
00099   aux_46 = aux_40*aux_38*aux_41-aux_36*aux_39 ;
00100   aux_47 = aux_16*aux_46+aux_15*aux_45+aux_1*aux_40*aux_8*aux_37 ;
00101   aux_48 = aux_25*aux_46+aux_18*aux_45+aux_4*aux_40*aux_8*aux_37 ;
00102   aux_49 = aux_8*aux_5*aux_46+aux_8*aux_3*aux_45-aux_40*aux_2*aux_37 ;
00103   aux_50 = aux_49*aux_34+aux_48*aux_33+aux_47*aux_9*aux_29 ;
00104   aux_51 = aux_25*aux_43+aux_18*aux_42+aux_4*aux_36*aux_8*aux_37 ;
00105   aux_52 = aux_8*aux_5*aux_43+aux_8*aux_3*aux_42-aux_36*aux_2*aux_37 ;
00106   aux_53 = aux_52*aux_34+aux_51*aux_33+aux_44*aux_9*aux_29 ;
00107   aux_54 = 1/(pow(aux_53,2)+pow(aux_50,2)) ;
00108   aux_55 = aux_37*aux_20*aux_41+aux_37*aux_19*aux_39+aux_4*aux_8*aux_38 ;
00109   aux_56 = aux_37*aux_16*aux_41+aux_37*aux_15*aux_39-aux_1*aux_8*aux_38 ;
00110   aux_57 = aux_37*aux_25*aux_41+aux_37*aux_18*aux_39-aux_4*aux_8*aux_38 ;
00111   aux_58 = aux_8*aux_37*aux_5*aux_41+aux_8*aux_37*aux_3*aux_39+aux_2*aux_38 ;
00112   aux_59 = 1/sqrt(1-pow(aux_58*aux_34+aux_57*aux_33+aux_56*aux_9*aux_29,2)) ;
00113   aux_60 = aux_37*aux_22*aux_41+aux_37*aux_14*aux_39+aux_1*aux_8*aux_38 ;
00114   aux_61 = -aux_8*aux_37*aux_5*aux_41-aux_8*aux_37*aux_3*aux_39-aux_2*aux_38 ;
00115   aux_62 = aux_57*aux_9*aux_12+aux_58*aux_9*aux_10-aux_56*aux_7 ;
00116   aux_63 = aux_57*aux_31+aux_58*aux_32+aux_56*aux_9*aux_28 ;
00117   aux_64 = 1/(pow(aux_63,2)+pow(aux_62,2)) ;
00118 
00119 
00120   mat[0][0] = -(aux_11*aux_9*aux_12+14745600*aux_8*aux_3*aux_9*aux_10+aux_6*aux_7)/14745600;
00121   mat[0][1] = (aux_13*aux_9*aux_12-14745600*aux_8*aux_5*aux_9*aux_10+(14745600*aux_1*aux_2*aux_5-14745600*aux_4*aux_3)*aux_7)/14745600;
00122   mat[0][2] = (-14745600*aux_4*aux_8*aux_9*aux_12+14745600*aux_2*aux_9*aux_10+14745600*aux_1*aux_8*aux_7)/14745600;
00123   mat[0][3] = aux_9*aux_12*aux_17-aux_7*aux_21;
00124   mat[0][4] = aux_9*aux_10*aux_23-aux_7*aux_24;
00125   mat[0][5] = aux_9*aux_12*aux_27+aux_9*aux_10*aux_26;
00126   mat[1][0] = -((aux_11*aux_7*aux_28-14745600*aux_8*aux_3*aux_29)*aux_12+(14745600*aux_8*aux_3*aux_7*aux_28+aux_11*aux_29)*aux_10+(14745600*aux_4*aux_5+14745600*aux_1*aux_2*aux_3)*aux_9*aux_28)/14745600;
00127   mat[1][1] = ((aux_13*aux_7*aux_28+14745600*aux_8*aux_5*aux_29)*aux_12+(aux_13*aux_29-14745600*aux_8*aux_5*aux_7*aux_28)*aux_10+aux_30*aux_9*aux_28)/14745600;
00128   mat[1][2] = ((-14745600*aux_4*aux_8*aux_7*aux_28-14745600*aux_2*aux_29)*aux_12+(14745600*aux_2*aux_7*aux_28-14745600*aux_4*aux_8*aux_29)*aux_10-14745600*aux_1*aux_8*aux_9*aux_28)/14745600;
00129   mat[1][3] = aux_9*aux_28*aux_21+aux_31*aux_17;
00130   mat[1][4] = aux_9*aux_28*aux_24+aux_32*aux_23;
00131   mat[1][5] = aux_31*aux_27+aux_32*aux_26;
00132   mat[2][0] = (((14745600*aux_1*aux_5-14745600*aux_4*aux_2*aux_3)*aux_7*aux_29-14745600*aux_8*aux_3*aux_28)*aux_12+(aux_11*aux_28-14745600*aux_8*aux_3*aux_7*aux_29)*aux_10+aux_6*aux_9*aux_29)/14745600;
00133   mat[2][1] = ((aux_13*aux_7*aux_29-14745600*aux_8*aux_5*aux_28)*aux_12+((14745600*aux_4*aux_2*aux_5+14745600*aux_1*aux_3)*aux_28-14745600*aux_8*aux_5*aux_7*aux_29)*aux_10+aux_30*aux_9*aux_29)/14745600;
00134   mat[2][2] = -((14745600*aux_4*aux_8*aux_7*aux_29-14745600*aux_2*aux_28)*aux_12+(-14745600*aux_4*aux_8*aux_28-14745600*aux_2*aux_7*aux_29)*aux_10+14745600*aux_1*aux_8*aux_9*aux_29)/14745600;
00135   mat[2][3] = aux_9*aux_29*aux_21+aux_33*aux_17;
00136   mat[2][4] = aux_9*aux_29*aux_24+aux_34*aux_23;
00137   mat[2][5] = aux_33*aux_27+aux_34*aux_26;
00138   mat[3][0] = aux_35;
00139   mat[3][1] = aux_35;
00140   mat[3][2] = aux_35;
00141   mat[3][3] = (aux_44*aux_33+(aux_20*aux_43+aux_19*aux_42-aux_4*aux_36*aux_8*aux_37)*aux_9*aux_29)*aux_50*aux_54-(aux_47*aux_33+(aux_20*aux_46+aux_19*aux_45-aux_4*aux_40*aux_8*aux_37)*aux_9*aux_29)*aux_53*aux_54;
00142   mat[3][4] = aux_50*((aux_22*aux_43+aux_14*aux_42-aux_1*aux_36*aux_8*aux_37)*aux_34+aux_52*aux_9*aux_29)*aux_54-((aux_22*aux_46+aux_14*aux_45-aux_1*aux_40*aux_8*aux_37)*aux_34+aux_49*aux_9*aux_29)*aux_53*aux_54;
00143   mat[3][5] = aux_50*(aux_51*aux_34+(-aux_8*aux_5*aux_43-aux_8*aux_3*aux_42+aux_36*aux_2*aux_37)*aux_33)*aux_54-(aux_48*aux_34+(-aux_8*aux_5*aux_46-aux_8*aux_3*aux_45+aux_40*aux_2*aux_37)*aux_33)*aux_53*aux_54;
00144   mat[4][0] = aux_35;
00145   mat[4][1] = aux_35;
00146   mat[4][2] = aux_35;
00147   mat[4][3] = -(aux_56*aux_33+aux_55*aux_9*aux_29)*aux_59;
00148   mat[4][4] = -(aux_60*aux_34+aux_58*aux_9*aux_29)*aux_59;
00149   mat[4][5] = -(aux_57*aux_34+aux_61*aux_33)*aux_59;
00150   mat[5][0] = aux_35;
00151   mat[5][1] = aux_35;
00152   mat[5][2] = aux_35;
00153   mat[5][3] = aux_62*(aux_56*aux_31+aux_55*aux_9*aux_28)*aux_64-(aux_56*aux_9*aux_12-aux_55*aux_7)*aux_63*aux_64;
00154   mat[5][4] = aux_62*(aux_60*aux_32+aux_58*aux_9*aux_28)*aux_64-(aux_60*aux_9*aux_10-aux_58*aux_7)*aux_63*aux_64;
00155   mat[5][5] = aux_62*(aux_61*aux_31+aux_57*aux_32)*aux_64-(aux_61*aux_9*aux_12+aux_57*aux_9*aux_10)*aux_63*aux_64;
00156 }  
00157 
00158 inline void manifoldGradientXj(Matrix6& mat, const Vector6& e, const Vector6& xi, const Vector6& xj)
00159 {
00160   //const double& ex  = e[0];
00161   //const double& ey  = e[1];
00162   //const double& ez  = e[2];
00163   const double& er  = e[3];
00164   const double& ep  = e[4];
00165   const double& eya = e[5];
00166 
00167   //const double& x1 = xi[0];
00168   //const double& y1 = xi[1];
00169   //const double& z1 = xi[2];
00170   const double& a1 = xi[3];
00171   const double& b1 = xi[4];
00172   const double& c1 = xi[5];
00173 
00174   //const double& x2 = xj[0];
00175   //const double& y2 = xj[1];
00176   //const double& z2 = xj[2];
00177   const double& a2 = xj[3];
00178   const double& b2 = xj[4];
00179   const double& c2 = xj[5];
00180 
00181   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 , aux_12 , aux_13 ,
00182          aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 , aux_21 , aux_22 , aux_23 , aux_24 , aux_25 ,
00183          aux_26 , aux_27 , aux_28 , aux_29 , aux_30 , aux_31 , aux_32 , aux_33 , aux_34 , aux_35 , aux_36 , aux_37 ,
00184          aux_38 , aux_39 , aux_40 , aux_41 , aux_42 , aux_43 , aux_44 , aux_45 , aux_46 , aux_47 , aux_48 , aux_49 ,
00185          aux_50 , aux_51 , aux_52 , aux_53 ;
00186   aux_1 = cos(a1) ;
00187   aux_2 = sin(b1) ;
00188   aux_3 = cos(c1) ;
00189   aux_4 = sin(a1) ;
00190   aux_5 = sin(c1) ;
00191   aux_6 = sin(ep) ;
00192   aux_7 = cos(b1) ;
00193   aux_8 = cos(ep) ;
00194   aux_9 = cos(eya) ;
00195   aux_10 = aux_4*aux_2*aux_3-aux_1*aux_5 ;
00196   aux_11 = sin(eya) ;
00197   aux_12 = aux_4*aux_2*aux_5+aux_1*aux_3 ;
00198   aux_13 = 0 ;
00199   aux_14 = aux_4*aux_5+aux_1*aux_2*aux_3 ;
00200   aux_15 = sin(er) ;
00201   aux_16 = cos(er) ;
00202   aux_17 = aux_1*aux_2*aux_5-aux_4*aux_3 ;
00203   aux_18 = cos(a2) ;
00204   aux_19 = cos(b2) ;
00205   aux_20 = sin(b2) ;
00206   aux_21 = cos(c2) ;
00207   aux_22 = sin(a2) ;
00208   aux_23 = sin(c2) ;
00209   aux_24 = aux_22*aux_23+aux_18*aux_20*aux_21 ;
00210   aux_25 = aux_18*aux_20*aux_23-aux_22*aux_21 ;
00211   aux_26 = aux_6*aux_16*aux_11-aux_15*aux_9 ;
00212   aux_27 = aux_15*aux_11+aux_6*aux_16*aux_9 ;
00213   aux_28 = (aux_7*aux_5*aux_25+aux_7*aux_3*aux_24-aux_18*aux_2*aux_19)*aux_27+(aux_12*aux_25+aux_10*aux_24+aux_4*aux_18*aux_7*aux_19)*aux_26+(aux_17*aux_25+aux_14*aux_24+aux_1*aux_18*aux_7*aux_19)*aux_8*aux_16 ;
00214   aux_29 = pow(aux_28,2) ;
00215   aux_30 = aux_22*aux_20*aux_21-aux_18*aux_23 ;
00216   aux_31 = aux_22*aux_20*aux_23+aux_18*aux_21 ;
00217   aux_32 = aux_17*aux_31+aux_14*aux_30+aux_1*aux_22*aux_7*aux_19 ;
00218   aux_33 = aux_12*aux_31+aux_10*aux_30+aux_4*aux_22*aux_7*aux_19 ;
00219   aux_34 = aux_7*aux_5*aux_31+aux_7*aux_3*aux_30-aux_22*aux_2*aux_19 ;
00220   aux_35 = aux_34*aux_27+aux_33*aux_26+aux_32*aux_8*aux_16 ;
00221   aux_36 = 1/(pow(aux_35,2)+aux_29) ;
00222   aux_37 = aux_18*aux_23-aux_22*aux_20*aux_21 ;
00223   aux_38 = -aux_22*aux_20*aux_23-aux_18*aux_21 ;
00224   aux_39 = aux_19*aux_17*aux_23+aux_19*aux_14*aux_21-aux_1*aux_7*aux_20 ;
00225   aux_40 = aux_19*aux_12*aux_23+aux_19*aux_10*aux_21-aux_4*aux_7*aux_20 ;
00226   aux_41 = aux_7*aux_19*aux_5*aux_23+aux_7*aux_19*aux_3*aux_21+aux_2*aux_20 ;
00227   aux_42 = aux_41*aux_27+aux_40*aux_26+aux_39*aux_8*aux_16 ;
00228   aux_43 = -aux_22*aux_23-aux_18*aux_20*aux_21 ;
00229   aux_44 = aux_22*aux_21-aux_18*aux_20*aux_23 ;
00230   aux_45 = aux_17*aux_44+aux_14*aux_43-aux_1*aux_18*aux_7*aux_19 ;
00231   aux_46 = aux_12*aux_44+aux_10*aux_43-aux_4*aux_18*aux_7*aux_19 ;
00232   aux_47 = aux_7*aux_5*aux_44+aux_7*aux_3*aux_43+aux_18*aux_2*aux_19 ;
00233   aux_48 = 1/sqrt(1-pow(aux_42,2)) ;
00234   aux_49 = aux_6*aux_15*aux_9-aux_16*aux_11 ;
00235   aux_50 = aux_6*aux_15*aux_11+aux_16*aux_9 ;
00236   aux_51 = aux_40*aux_50+aux_41*aux_49+aux_39*aux_8*aux_15 ;
00237   aux_52 = aux_40*aux_8*aux_11+aux_41*aux_8*aux_9-aux_39*aux_6 ;
00238   aux_53 = 1/(pow(aux_52,2)+pow(aux_51,2)) ;
00239 
00240   mat[0][0] = aux_10*aux_8*aux_11+aux_7*aux_3*aux_8*aux_9+(-aux_4*aux_5-aux_1*aux_2*aux_3)*aux_6;
00241   mat[0][1] = aux_12*aux_8*aux_11+aux_7*aux_5*aux_8*aux_9+(aux_4*aux_3-aux_1*aux_2*aux_5)*aux_6;
00242   mat[0][2] = aux_4*aux_7*aux_8*aux_11-aux_2*aux_8*aux_9-aux_1*aux_7*aux_6;
00243   mat[0][3] = aux_13;
00244   mat[0][4] = aux_13;
00245   mat[0][5] = aux_13;
00246   mat[1][0] = (aux_10*aux_6*aux_15-aux_7*aux_3*aux_16)*aux_11+(aux_7*aux_3*aux_6*aux_15+aux_10*aux_16)*aux_9+aux_14*aux_8*aux_15;
00247   mat[1][1] = (aux_12*aux_6*aux_15-aux_7*aux_5*aux_16)*aux_11+(aux_7*aux_5*aux_6*aux_15+aux_12*aux_16)*aux_9+aux_17*aux_8*aux_15;
00248   mat[1][2] = (aux_4*aux_7*aux_6*aux_15+aux_2*aux_16)*aux_11+(aux_4*aux_7*aux_16-aux_2*aux_6*aux_15)*aux_9+aux_1*aux_7*aux_8*aux_15;
00249   mat[1][3] = aux_13;
00250   mat[1][4] = aux_13;
00251   mat[1][5] = aux_13;
00252   mat[2][0] = (aux_7*aux_3*aux_15+aux_10*aux_6*aux_16)*aux_11+((aux_1*aux_5-aux_4*aux_2*aux_3)*aux_15+aux_7*aux_3*aux_6*aux_16)*aux_9+aux_14*aux_8*aux_16;
00253   mat[2][1] = (aux_7*aux_5*aux_15+aux_12*aux_6*aux_16)*aux_11+((-aux_4*aux_2*aux_5-aux_1*aux_3)*aux_15+aux_7*aux_5*aux_6*aux_16)*aux_9+aux_17*aux_8*aux_16;
00254   mat[2][2] = (aux_4*aux_7*aux_6*aux_16-aux_2*aux_15)*aux_11+(-aux_4*aux_7*aux_15-aux_2*aux_6*aux_16)*aux_9+aux_1*aux_7*aux_8*aux_16;
00255   mat[2][3] = aux_13;
00256   mat[2][4] = aux_13;
00257   mat[2][5] = aux_13;
00258   mat[3][0] = aux_13;
00259   mat[3][1] = aux_13;
00260   mat[3][2] = aux_13;
00261   mat[3][3] = aux_29*aux_36-((aux_7*aux_5*aux_38+aux_7*aux_3*aux_37+aux_22*aux_2*aux_19)*aux_27+(aux_12*aux_38+aux_10*aux_37-aux_4*aux_22*aux_7*aux_19)*aux_26+(aux_17*aux_38+aux_14*aux_37-aux_1*aux_22*aux_7*aux_19)*aux_8*aux_16)*aux_35*aux_36;
00262   mat[3][4] = -aux_42*aux_35*aux_36;
00263   mat[3][5] = ((-aux_7*aux_19*aux_5*aux_23-aux_7*aux_19*aux_3*aux_21-aux_2*aux_20)*aux_27+(-aux_19*aux_12*aux_23-aux_19*aux_10*aux_21+aux_4*aux_7*aux_20)*aux_26+(-aux_19*aux_17*aux_23-aux_19*aux_14*aux_21+aux_1*aux_7*aux_20)*aux_8*aux_16)*aux_28*aux_36;
00264   mat[4][0] = aux_13;
00265   mat[4][1] = aux_13;
00266   mat[4][2] = aux_13;
00267   mat[4][3] = aux_13;
00268   mat[4][4] = -(aux_47*aux_27+aux_46*aux_26+aux_45*aux_8*aux_16)*aux_48;
00269   mat[4][5] = -aux_35*aux_48;
00270   mat[5][0] = aux_13;
00271   mat[5][1] = aux_13;
00272   mat[5][2] = aux_13;
00273   mat[5][3] = aux_13;
00274   mat[5][4] = aux_52*(aux_46*aux_50+aux_47*aux_49+aux_45*aux_8*aux_15)*aux_53-(aux_46*aux_8*aux_11+aux_47*aux_8*aux_9-aux_45*aux_6)*aux_51*aux_53;
00275   mat[5][5] = aux_52*(aux_33*aux_50+aux_34*aux_49+aux_32*aux_8*aux_15)*aux_53-(aux_33*aux_8*aux_11+aux_34*aux_8*aux_9-aux_32*aux_6)*aux_51*aux_53;
00276 }  
00277 
00278 inline void manifold2euler(Matrix6& mat, double manifold[6])
00279 {
00280   //const double& mx = manifold[0];
00281   //const double& my = manifold[1];
00282   //const double& mz = manifold[2];
00283   const double& ma = manifold[3];
00284   const double& mb = manifold[4];
00285   const double& mc = manifold[5];
00286 
00287   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 , aux_12 ,
00288          aux_13 , aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 , aux_21 , aux_22 , aux_23 ,
00289          aux_24 , aux_25 , aux_26 , aux_27 , aux_28 , aux_29 , aux_30 , aux_31 , aux_32 , aux_33 , aux_34 ,
00290          aux_35 , aux_36 , aux_37 , aux_38 , aux_39 , aux_40 , aux_41 , aux_42 , aux_43 , aux_44 , aux_45 ,
00291          aux_46 , aux_47 , aux_48 , aux_49 , aux_50 , aux_51 , aux_52 , aux_53 , aux_54 ;
00292   aux_1 = 1 ;
00293   aux_2 = 0 ;
00294   aux_3 = std::pow(ma,3) ;
00295   aux_4 = std::pow(ma,2) ;
00296   aux_5 = std::pow(mb,2) ;
00297   aux_6 = std::pow(mc,2) ;
00298   aux_7 = aux_6+aux_5+aux_4 ;
00299   aux_8 = sqrt(aux_7) ;
00300   aux_9 = 1/std::pow(aux_8,3) ;
00301   aux_10 = aux_8/2 ;
00302   aux_11 = cos(aux_10) ;
00303   aux_12 = sin(aux_10) ;
00304   aux_13 = -ma*aux_5*aux_9*aux_11*aux_12 ;
00305   aux_14 = ma*aux_6*aux_9*aux_11*aux_12 ;
00306   aux_15 = 1/aux_8 ;
00307   aux_16 = -ma*aux_15*aux_11*aux_12 ;
00308   aux_17 = 1/std::pow(aux_7,2) ;
00309   aux_18 = std::pow(aux_12,2) ;
00310   aux_19 = 2*ma*aux_5*aux_17*aux_18 ;
00311   aux_20 = -2*ma*aux_6*aux_17*aux_18 ;
00312   aux_21 = 1/aux_7 ;
00313   aux_22 = mb*mc*aux_21*aux_18+ma*aux_15*aux_11*aux_12 ;
00314   aux_23 = std::pow(aux_11,2) ;
00315   aux_24 = -aux_5*aux_21*aux_18 ;
00316   aux_25 = aux_6*aux_21*aux_18+aux_24-aux_4*aux_21*aux_18+aux_23 ;
00317   aux_26 = 1/(std::pow(aux_25,2)+4*std::pow(aux_22,2)) ;
00318   aux_27 = ma*mb*mc*aux_9*aux_11*aux_12 ;
00319   aux_28 = aux_15*aux_11*aux_12 ;
00320   aux_29 = -2*ma*mb*mc*aux_17*aux_18 ;
00321   aux_30 = std::pow(mb,3) ;
00322   aux_31 = -aux_30*aux_9*aux_11*aux_12 ;
00323   aux_32 = mb*aux_6*aux_9*aux_11*aux_12 ;
00324   aux_33 = -mb*aux_15*aux_11*aux_12 ;
00325   aux_34 = 2*aux_30*aux_17*aux_18 ;
00326   aux_35 = -2*mb*aux_6*aux_17*aux_18 ;
00327   aux_36 = -2*mb*aux_21*aux_18 ;
00328   aux_37 = mc*aux_21*aux_18 ;
00329   aux_38 = -aux_5*mc*aux_9*aux_11*aux_12 ;
00330   aux_39 = std::pow(mc,3) ;
00331   aux_40 = -mc*aux_15*aux_11*aux_12 ;
00332   aux_41 = 2*aux_5*mc*aux_17*aux_18 ;
00333   aux_42 = ma*mc*aux_21*aux_23/2 ;
00334   aux_43 = -ma*mc*aux_9*aux_11*aux_12 ;
00335   aux_44 = mb*aux_21*aux_18 ;
00336   aux_45 = -ma*mc*aux_21*aux_18/2 ;
00337   aux_46 = aux_4*mc*aux_9*aux_11*aux_12 ;
00338   aux_47 = -2*aux_4*mc*aux_17*aux_18 ;
00339   aux_48 = 1/sqrt(1-4*std::pow(ma*mc*aux_21*aux_18+aux_33,2)) ;
00340   aux_49 = ma*aux_21*aux_18 ;
00341   aux_50 = ma*mb*aux_21*aux_18+mc*aux_15*aux_11*aux_12 ;
00342   aux_51 = -aux_6*aux_21*aux_18+aux_24+aux_4*aux_21*aux_18+aux_23 ;
00343   aux_52 = 1/(std::pow(aux_51,2)+4*std::pow(aux_50,2)) ;
00344   aux_53 = aux_4*mb*aux_9*aux_11*aux_12 ;
00345   aux_54 = -2*aux_4*mb*aux_17*aux_18 ;
00346 
00347   mat[0][0] = aux_1;
00348   mat[0][1] = aux_2;
00349   mat[0][2] = aux_2;
00350   mat[0][3] = aux_2;
00351   mat[0][4] = aux_2;
00352   mat[0][5] = aux_2;
00353   mat[1][0] = aux_2;
00354   mat[1][1] = aux_1;
00355   mat[1][2] = aux_2;
00356   mat[1][3] = aux_2;
00357   mat[1][4] = aux_2;
00358   mat[1][5] = aux_2;
00359   mat[2][0] = aux_2;
00360   mat[2][1] = aux_2;
00361   mat[2][2] = aux_1;
00362   mat[2][3] = aux_2;
00363   mat[2][4] = aux_2;
00364   mat[2][5] = aux_2;
00365   mat[3][0] = aux_2;
00366   mat[3][1] = aux_2;
00367   mat[3][2] = aux_2;
00368   mat[3][3] = 2*(-aux_4*aux_21*aux_18/2+aux_29+aux_28+aux_27-aux_4*aux_9*aux_11*aux_12+aux_4*aux_21*aux_23/2)*aux_25*aux_26-2*(-2*ma*aux_21*aux_18+aux_20+aux_19+2*aux_3*aux_17*aux_18+aux_16+aux_14+aux_13-aux_3*aux_9*aux_11*aux_12)*aux_22*aux_26;
00369   mat[3][4] = 2*(aux_37-ma*mb*aux_21*aux_18/2-2*aux_5*mc*aux_17*aux_18+aux_5*mc*aux_9*aux_11*aux_12-ma*mb*aux_9*aux_11*aux_12+ma*mb*aux_21*aux_23/2)*aux_25*aux_26-2*(aux_36+aux_35+aux_34+2*aux_4*mb*aux_17*aux_18+aux_33+aux_32+aux_31-aux_4*mb*aux_9*aux_11*aux_12)*aux_22*aux_26;
00370   mat[3][5] = 2*(aux_45+aux_44+aux_35+aux_32+aux_43+aux_42)*aux_25*aux_26-2*(2*mc*aux_21*aux_18-2*aux_39*aux_17*aux_18+aux_41+2*aux_4*mc*aux_17*aux_18+aux_40+aux_39*aux_9*aux_11*aux_12+aux_38-aux_4*mc*aux_9*aux_11*aux_12)*aux_22*aux_26;
00371   mat[4][0] = aux_2;
00372   mat[4][1] = aux_2;
00373   mat[4][2] = aux_2;
00374   mat[4][3] = -2*(aux_37+ma*mb*aux_21*aux_18/2+aux_47+aux_46+ma*mb*aux_9*aux_11*aux_12-ma*mb*aux_21*aux_23/2)*aux_48;
00375   mat[4][4] = -2*(aux_5*aux_21*aux_18/2+aux_29-aux_15*aux_11*aux_12+aux_27+aux_5*aux_9*aux_11*aux_12-aux_5*aux_21*aux_23/2)*aux_48;
00376   mat[4][5] = -2*(mb*mc*aux_21*aux_18/2+aux_49+aux_20+aux_14+mb*mc*aux_9*aux_11*aux_12-mb*mc*aux_21*aux_23/2)*aux_48;
00377   mat[5][0] = aux_2;
00378   mat[5][1] = aux_2;
00379   mat[5][2] = aux_2;
00380   mat[5][3] = 2*(aux_45+aux_44+aux_54+aux_43+aux_53+aux_42)*aux_51*aux_52-2*(2*ma*aux_21*aux_18+2*ma*aux_6*aux_17*aux_18+aux_19-2*aux_3*aux_17*aux_18+aux_16-ma*aux_6*aux_9*aux_11*aux_12+aux_13+aux_3*aux_9*aux_11*aux_12)*aux_50*aux_52;
00381   mat[5][4] = 2*(-mb*mc*aux_21*aux_18/2+aux_49-2*ma*aux_5*aux_17*aux_18-mb*mc*aux_9*aux_11*aux_12+ma*aux_5*aux_9*aux_11*aux_12+mb*mc*aux_21*aux_23/2)*aux_51*aux_52-2*(aux_36+2*mb*aux_6*aux_17*aux_18+aux_34+aux_54+aux_33-mb*aux_6*aux_9*aux_11*aux_12+aux_31+aux_53)*aux_50*aux_52;
00382   mat[5][5] = 2*aux_51*(-aux_6*aux_21*aux_18/2+aux_29+aux_28-aux_6*aux_9*aux_11*aux_12+aux_27+aux_6*aux_21*aux_23/2)*aux_52-2*aux_50*(-2*mc*aux_21*aux_18+2*aux_39*aux_17*aux_18+aux_41+aux_47+aux_40-aux_39*aux_9*aux_11*aux_12+aux_38+aux_46)*aux_52;
00383 }
00384 
00385 inline void manifoldZero2euler(Matrix6& mat, double manifold[6])
00386 {
00387   //const double& mx = manifold[0];
00388   //const double& my = manifold[1];
00389   //const double& mz = manifold[2];
00390   const double& ma = manifold[3];
00391   const double& mb = manifold[4];
00392   const double& mc = manifold[5];
00393 
00394   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 , aux_12 ;
00395   aux_1 = 1 ;
00396   aux_2 = 0 ;
00397   aux_3 = mb*mc/4+ma/2 ;
00398   aux_4 = std::pow(ma,2) ;
00399   aux_5 = -std::pow(mb,2)/4 ;
00400   aux_6 = std::pow(mc,2) ;
00401   aux_7 = aux_6/4+aux_5-aux_4/4+1 ;
00402   aux_8 = 1/(std::pow(aux_7,2)+4*std::pow(aux_3,2)) ;
00403   aux_9 = 1/sqrt(1-4*std::pow(ma*mc/4-mb/2,2)) ;
00404   aux_10 = mc/2+ma*mb/4 ;
00405   aux_11 = -aux_6/4+aux_5+aux_4/4+1 ;
00406   aux_12 = 1/(std::pow(aux_11,2)+4*std::pow(aux_10,2)) ;
00407 
00408   mat[0][0] = aux_1;
00409   mat[0][1] = aux_2;
00410   mat[0][2] = aux_2;
00411   mat[0][3] = aux_2;
00412   mat[0][4] = aux_2;
00413   mat[0][5] = aux_2;
00414   mat[1][0] = aux_2;
00415   mat[1][1] = aux_1;
00416   mat[1][2] = aux_2;
00417   mat[1][3] = aux_2;
00418   mat[1][4] = aux_2;
00419   mat[1][5] = aux_2;
00420   mat[2][0] = aux_2;
00421   mat[2][1] = aux_2;
00422   mat[2][2] = aux_1;
00423   mat[2][3] = aux_2;
00424   mat[2][4] = aux_2;
00425   mat[2][5] = aux_2;
00426   mat[3][0] = aux_2;
00427   mat[3][1] = aux_2;
00428   mat[3][2] = aux_2;
00429   mat[3][3] = aux_7*aux_8+ma*aux_3*aux_8;
00430   mat[3][4] = mc*aux_7*aux_8/2+mb*aux_3*aux_8;
00431   mat[3][5] = mb*aux_7*aux_8/2-mc*aux_3*aux_8;
00432   mat[4][0] = aux_2;
00433   mat[4][1] = aux_2;
00434   mat[4][2] = aux_2;
00435   mat[4][3] = -mc*aux_9/2;
00436   mat[4][4] = aux_9;
00437   mat[4][5] = -ma*aux_9/2;
00438   mat[5][0] = aux_2;
00439   mat[5][1] = aux_2;
00440   mat[5][2] = aux_2;
00441   mat[5][3] = mb*aux_11*aux_12/2-ma*aux_10*aux_12;
00442   mat[5][4] = ma*aux_11*aux_12/2+mb*aux_10*aux_12;
00443   mat[5][5] = aux_11*aux_12+aux_10*mc*aux_12;
00444 }
00445 
00446 inline void eulerGradientXi(Matrix6& mat, const Vector6& e, const Vector6& xi, const Vector6& xj)
00447 {
00448   //const double& ex  = e[0];
00449   //const double& ey  = e[1];
00450   //const double& ez  = e[2];
00451   const double& er  = e[3];
00452   const double& ep  = e[4];
00453   const double& eya = e[5];
00454 
00455   const double& x1 = xi[0];
00456   const double& y1 = xi[1];
00457   const double& z1 = xi[2];
00458   const double& a1 = xi[3];
00459   const double& b1 = xi[4];
00460   const double& c1 = xi[5];
00461 
00462   const double& x2 = xj[0];
00463   const double& y2 = xj[1];
00464   const double& z2 = xj[2];
00465   const double& a2 = xj[3];
00466   const double& b2 = xj[4];
00467   const double& c2 = xj[5];
00468 
00469   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 , aux_12 ,
00470          aux_13 , aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 , aux_21 , aux_22 , aux_23 ,
00471          aux_24 , aux_25 , aux_26 , aux_27 , aux_28 , aux_29 , aux_30 , aux_31 , aux_32 , aux_33 , aux_34 ,
00472          aux_35 , aux_36 , aux_37 , aux_38 , aux_39 , aux_40 , aux_41 , aux_42 , aux_43 , aux_44 , aux_45 ,
00473          aux_46 , aux_47 , aux_48 , aux_49 , aux_50 , aux_51 , aux_52 , aux_53 , aux_54 , aux_55 , aux_56 ,
00474          aux_57 , aux_58 , aux_59 , aux_60 , aux_61 , aux_62 , aux_63 , aux_64 , aux_65 , aux_66 , aux_67 ,
00475          aux_68 , aux_69 ;
00476   aux_1 = sin(c1) ;
00477   aux_2 = sin(a1) ;
00478   aux_3 = sin(ep) ;
00479   aux_4 = cos(a1) ;
00480   aux_5 = cos(ep) ;
00481   aux_6 = sin(eya) ;
00482   aux_7 = cos(c1) ;
00483   aux_8 = cos(b1) ;
00484   aux_9 = cos(eya) ;
00485   aux_10 = sin(b1) ;
00486   aux_11 = aux_4*aux_3-aux_2*aux_5*aux_6 ;
00487   aux_12 = aux_10*aux_11-aux_8*aux_5*aux_9 ;
00488   aux_13 = aux_2*aux_1+aux_4*aux_10*aux_7 ;
00489   aux_14 = aux_4*aux_10*aux_1-aux_2*aux_7 ;
00490   aux_15 = aux_4*aux_8*z2-aux_4*aux_8*z1+aux_14*y2-aux_14*y1+aux_13*x2+(-aux_2*aux_1-aux_4*aux_10*aux_7)*x1 ;
00491   aux_16 = aux_2*aux_10*aux_7-aux_4*aux_1 ;
00492   aux_17 = aux_4*aux_1-aux_2*aux_10*aux_7 ;
00493   aux_18 = -aux_2*aux_10*aux_1-aux_4*aux_7 ;
00494   aux_19 = -aux_2*aux_8*z2+aux_2*aux_8*z1+aux_18*y2-aux_18*y1+aux_17*x2+aux_16*x1 ;
00495   aux_20 = -aux_8*z2+aux_8*z1-aux_10*aux_1*y2+aux_10*aux_1*y1-aux_10*aux_7*x2+aux_10*aux_7*x1 ;
00496   aux_21 = -aux_4*aux_10*z2+aux_4*aux_10*z1+aux_4*aux_8*aux_1*y2-aux_4*aux_8*aux_1*y1+aux_4*aux_8*aux_7*x2-aux_4*aux_8*aux_7*x1 ;
00497   aux_22 = -aux_2*aux_10*z2+aux_2*aux_10*z1+aux_2*aux_8*aux_1*y2-aux_2*aux_8*aux_1*y1+aux_2*aux_8*aux_7*x2-aux_2*aux_8*aux_7*x1 ;
00498   aux_23 = aux_8*aux_7*y2-aux_8*aux_7*y1-aux_8*aux_1*x2+aux_8*aux_1*x1 ;
00499   aux_24 = aux_2*aux_10*aux_1+aux_4*aux_7 ;
00500   aux_25 = aux_16*y2-aux_16*y1+aux_18*x2+aux_24*x1 ;
00501   aux_26 = aux_2*aux_7-aux_4*aux_10*aux_1 ;
00502   aux_27 = aux_13*y2-aux_13*y1+aux_26*x2+aux_14*x1 ;
00503   aux_28 = sin(er) ;
00504   aux_29 = cos(er) ;
00505   aux_30 = aux_3*aux_28*aux_6+aux_29*aux_9 ;
00506   aux_31 = -aux_3*aux_28*aux_6-aux_29*aux_9 ;
00507   aux_32 = aux_2*aux_31-aux_4*aux_5*aux_28 ;
00508   aux_33 = aux_10*aux_32+aux_8*(aux_29*aux_6-aux_3*aux_28*aux_9) ;
00509   aux_34 = aux_3*aux_28*aux_9-aux_29*aux_6 ;
00510   aux_35 = aux_3*aux_29*aux_6-aux_28*aux_9 ;
00511   aux_36 = aux_28*aux_9-aux_3*aux_29*aux_6 ;
00512   aux_37 = aux_2*aux_36-aux_4*aux_5*aux_29 ;
00513   aux_38 = aux_10*aux_37+aux_8*(-aux_28*aux_6-aux_3*aux_29*aux_9) ;
00514   aux_39 = aux_28*aux_6+aux_3*aux_29*aux_9 ;
00515   aux_40 = 0 ;
00516   aux_41 = sin(a2) ;
00517   aux_42 = cos(b2) ;
00518   aux_43 = sin(b2) ;
00519   aux_44 = cos(c2) ;
00520   aux_45 = cos(a2) ;
00521   aux_46 = sin(c2) ;
00522   aux_47 = aux_41*aux_43*aux_44-aux_45*aux_46 ;
00523   aux_48 = aux_41*aux_43*aux_46+aux_45*aux_44 ;
00524   aux_49 = aux_14*aux_48+aux_13*aux_47+aux_4*aux_41*aux_8*aux_42 ;
00525   aux_50 = aux_41*aux_46+aux_45*aux_43*aux_44 ;
00526   aux_51 = aux_45*aux_43*aux_46-aux_41*aux_44 ;
00527   aux_52 = aux_14*aux_51+aux_13*aux_50+aux_4*aux_45*aux_8*aux_42 ;
00528   aux_53 = (aux_8*aux_1*aux_51+aux_8*aux_7*aux_50-aux_45*aux_10*aux_42)*aux_39+(aux_24*aux_51+aux_16*aux_50+aux_2*aux_45*aux_8*aux_42)*aux_35+aux_52*aux_5*aux_29 ;
00529   aux_54 = (aux_8*aux_1*aux_48+aux_8*aux_7*aux_47-aux_41*aux_10*aux_42)*aux_39+(aux_24*aux_48+aux_16*aux_47+aux_2*aux_41*aux_8*aux_42)*aux_35+aux_49*aux_5*aux_29 ;
00530   aux_55 = 1/(pow(aux_54,2)+pow(aux_53,2)) ;
00531   aux_56 = aux_42*aux_18*aux_46+aux_42*aux_17*aux_44+aux_2*aux_8*aux_43 ;
00532   aux_57 = aux_42*aux_14*aux_46+aux_42*aux_13*aux_44-aux_4*aux_8*aux_43 ;
00533   aux_58 = aux_42*aux_24*aux_46+aux_42*aux_16*aux_44-aux_2*aux_8*aux_43 ;
00534   aux_59 = aux_8*aux_42*aux_1*aux_46+aux_8*aux_42*aux_7*aux_44+aux_10*aux_43 ;
00535   aux_60 = 1/sqrt(1-pow(aux_59*aux_39+aux_58*aux_35+aux_57*aux_5*aux_29,2)) ;
00536   aux_61 = aux_4*aux_8*aux_42*aux_1*aux_46+aux_4*aux_8*aux_42*aux_7*aux_44+aux_4*aux_10*aux_43 ;
00537   aux_62 = aux_2*aux_8*aux_42*aux_1*aux_46+aux_2*aux_8*aux_42*aux_7*aux_44+aux_2*aux_10*aux_43 ;
00538   aux_63 = -aux_10*aux_42*aux_1*aux_46-aux_10*aux_42*aux_7*aux_44+aux_8*aux_43 ;
00539   aux_64 = aux_42*aux_13*aux_46+aux_42*aux_26*aux_44 ;
00540   aux_65 = aux_42*aux_16*aux_46+aux_42*aux_18*aux_44 ;
00541   aux_66 = aux_8*aux_42*aux_7*aux_46-aux_8*aux_42*aux_1*aux_44 ;
00542   aux_67 = aux_58*aux_5*aux_6+aux_59*aux_5*aux_9-aux_57*aux_3 ;
00543   aux_68 = aux_58*aux_30+aux_59*aux_34+aux_57*aux_5*aux_28 ;
00544   aux_69 = 1/(pow(aux_68,2)+pow(aux_67,2)) ;
00545 
00546   mat[0][0] =  aux_7*aux_12+aux_1*(aux_4*aux_5*aux_6+aux_2*aux_3);
00547   mat[0][1] = aux_1*aux_12+aux_7*(-aux_4*aux_5*aux_6-aux_2*aux_3);
00548   mat[0][2] = aux_8*aux_11+aux_10*aux_5*aux_9;
00549   mat[0][3] = aux_5*aux_6*aux_15-aux_3*aux_19;
00550   mat[0][4] = aux_5*aux_6*aux_22-aux_3*aux_21+aux_5*aux_9*aux_20;
00551   mat[0][5] = -aux_3*aux_27+aux_5*aux_6*aux_25+aux_5*aux_9*aux_23;
00552   mat[1][0] = aux_7*aux_33+aux_1*(aux_4*aux_30-aux_2*aux_5*aux_28);
00553   mat[1][1] = aux_1*aux_33+aux_7*(aux_4*aux_31+aux_2*aux_5*aux_28);
00554   mat[1][2] = aux_8*aux_32+aux_10*aux_34;
00555   mat[1][3] = aux_5*aux_28*aux_19+aux_30*aux_15;
00556   mat[1][4] = aux_30*aux_22+aux_5*aux_28*aux_21+aux_34*aux_20;
00557   mat[1][5] = aux_5*aux_28*aux_27+aux_30*aux_25+aux_34*aux_23;
00558   mat[2][0] = aux_7*aux_38+aux_1*(aux_4*aux_35-aux_2*aux_5*aux_29);
00559   mat[2][1] = aux_1*aux_38+aux_7*(aux_4*aux_36+aux_2*aux_5*aux_29);
00560   mat[2][2] = aux_8*aux_37+aux_10*aux_39;
00561   mat[2][3] = aux_5*aux_29*aux_19+aux_35*aux_15;
00562   mat[2][4] = aux_35*aux_22+aux_5*aux_29*aux_21+aux_39*aux_20;
00563   mat[2][5] = aux_5*aux_29*aux_27+aux_35*aux_25+aux_39*aux_23;
00564   mat[3][0] = aux_40;
00565   mat[3][1] = aux_40;
00566   mat[3][2] = aux_40;
00567   mat[3][3] = (aux_49*aux_35+(aux_18*aux_48+aux_17*aux_47-aux_2*aux_41*aux_8*aux_42)*aux_5*aux_29)*aux_53*aux_55-(aux_52*aux_35+(aux_18*aux_51+aux_17*aux_50-aux_2*aux_45*aux_8*aux_42)*aux_5*aux_29)*aux_54*aux_55;
00568   mat[3][4] = aux_53*((-aux_10*aux_1*aux_48-aux_10*aux_7*aux_47-aux_41*aux_8*aux_42)*aux_39+(aux_2*aux_8*aux_1*aux_48+aux_2*aux_8*aux_7*aux_47-aux_2*aux_41*aux_10*aux_42)*aux_35+(aux_4*aux_8*aux_1*aux_48+aux_4*aux_8*aux_7*aux_47-aux_4*aux_41*aux_10*aux_42)*aux_5*aux_29)*aux_55-((-aux_10*aux_1*aux_51-aux_10*aux_7*aux_50-aux_45*aux_8*aux_42)*aux_39+(aux_2*aux_8*aux_1*aux_51+aux_2*aux_8*aux_7*aux_50-aux_2*aux_45*aux_10*aux_42)*aux_35+(aux_4*aux_8*aux_1*aux_51+aux_4*aux_8*aux_7*aux_50-aux_4*aux_45*aux_10*aux_42)*aux_5*aux_29)*aux_54*aux_55;
00569   mat[3][5] = aux_53*((aux_8*aux_7*aux_48-aux_8*aux_1*aux_47)*aux_39+(aux_16*aux_48+aux_18*aux_47)*aux_35+(aux_13*aux_48+aux_26*aux_47)*aux_5*aux_29)*aux_55-((aux_8*aux_7*aux_51-aux_8*aux_1*aux_50)*aux_39+(aux_16*aux_51+aux_18*aux_50)*aux_35+(aux_13*aux_51+aux_26*aux_50)*aux_5*aux_29)*aux_54*aux_55;
00570   mat[4][0] = aux_40;
00571   mat[4][1] = aux_40;
00572   mat[4][2] = aux_40;
00573   mat[4][3] = -(aux_57*aux_35+aux_56*aux_5*aux_29)*aux_60;
00574   mat[4][4] = -(aux_63*aux_39+aux_62*aux_35+aux_61*aux_5*aux_29)*aux_60;
00575   mat[4][5] = -(aux_66*aux_39+aux_65*aux_35+aux_64*aux_5*aux_29)*aux_60;
00576   mat[5][0] = aux_40;
00577   mat[5][1] = aux_40;
00578   mat[5][2] = aux_40;
00579   mat[5][3] = aux_67*(aux_57*aux_30+aux_56*aux_5*aux_28)*aux_69-(aux_57*aux_5*aux_6-aux_56*aux_3)*aux_68*aux_69;
00580   mat[5][4] = aux_67*(aux_62*aux_30+aux_63*aux_34+aux_61*aux_5*aux_28)*aux_69-(aux_62*aux_5*aux_6+aux_63*aux_5*aux_9-aux_61*aux_3)*aux_68*aux_69;
00581   mat[5][5] = aux_67*(aux_65*aux_30+aux_66*aux_34+aux_64*aux_5*aux_28)*aux_69-(aux_65*aux_5*aux_6+aux_66*aux_5*aux_9-aux_64*aux_3)*aux_68*aux_69;
00582 }
00583 
00584 inline void eulerGradientXj(Matrix6& mat, const Vector6& e, const Vector6& xi, const Vector6& xj)
00585 {
00586   //const double& ex  = e[0];
00587   //const double& ey  = e[1];
00588   //const double& ez  = e[2];
00589   const double& er  = e[3];
00590   const double& ep  = e[4];
00591   const double& eya = e[5];
00592 
00593   //const double& x1 = xi[0];
00594   //const double& y1 = xi[1];
00595   //const double& z1 = xi[2];
00596   const double& a1 = xi[3];
00597   const double& b1 = xi[4];
00598   const double& c1 = xi[5];
00599 
00600   //const double& x2 = xj[0];
00601   //const double& y2 = xj[1];
00602   //const double& z2 = xj[2];
00603   const double& a2 = xj[3];
00604   const double& b2 = xj[4];
00605   const double& c2 = xj[5];
00606 
00607 
00608   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 , aux_12 ,
00609          aux_13 , aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 , aux_21 , aux_22 , aux_23 ,
00610          aux_24 , aux_25 , aux_26 , aux_27 , aux_28 , aux_29 , aux_30 , aux_31 , aux_32 , aux_33 , aux_34 ,
00611          aux_35 , aux_36 , aux_37 , aux_38 , aux_39 , aux_40 , aux_41 , aux_42 , aux_43 , aux_44 , aux_45 ,
00612          aux_46 , aux_47 , aux_48 , aux_49 , aux_50 , aux_51 ;
00613   aux_1 = cos(a1) ;
00614   aux_2 = sin(b1) ;
00615   aux_3 = cos(c1) ;
00616   aux_4 = sin(a1) ;
00617   aux_5 = sin(c1) ;
00618   aux_6 = sin(ep) ;
00619   aux_7 = cos(b1) ;
00620   aux_8 = cos(ep) ;
00621   aux_9 = cos(eya) ;
00622   aux_10 = aux_4*aux_2*aux_3-aux_1*aux_5 ;
00623   aux_11 = sin(eya) ;
00624   aux_12 = aux_4*aux_2*aux_5+aux_1*aux_3 ;
00625   aux_13 = 0 ;
00626   aux_14 = aux_4*aux_5+aux_1*aux_2*aux_3 ;
00627   aux_15 = sin(er) ;
00628   aux_16 = cos(er) ;
00629   aux_17 = aux_1*aux_2*aux_5-aux_4*aux_3 ;
00630   aux_18 = cos(a2) ;
00631   aux_19 = cos(b2) ;
00632   aux_20 = sin(b2) ;
00633   aux_21 = cos(c2) ;
00634   aux_22 = sin(a2) ;
00635   aux_23 = sin(c2) ;
00636   aux_24 = aux_22*aux_23+aux_18*aux_20*aux_21 ;
00637   aux_25 = aux_18*aux_20*aux_23-aux_22*aux_21 ;
00638   aux_26 = aux_6*aux_16*aux_11-aux_15*aux_9 ;
00639   aux_27 = aux_15*aux_11+aux_6*aux_16*aux_9 ;
00640   aux_28 = (aux_7*aux_5*aux_25+aux_7*aux_3*aux_24-aux_18*aux_2*aux_19)*aux_27+(aux_12*aux_25+aux_10*aux_24+aux_4*aux_18*aux_7*aux_19)*aux_26+(aux_17*aux_25+aux_14*aux_24+aux_1*aux_18*aux_7*aux_19)*aux_8*aux_16 ;
00641   aux_29 = pow(aux_28,2) ;
00642   aux_30 = aux_22*aux_20*aux_21-aux_18*aux_23 ;
00643   aux_31 = aux_22*aux_20*aux_23+aux_18*aux_21 ;
00644   aux_32 = (aux_7*aux_5*aux_31+aux_7*aux_3*aux_30-aux_22*aux_2*aux_19)*aux_27+(aux_12*aux_31+aux_10*aux_30+aux_4*aux_22*aux_7*aux_19)*aux_26+(aux_17*aux_31+aux_14*aux_30+aux_1*aux_22*aux_7*aux_19)*aux_8*aux_16 ;
00645   aux_33 = 1/(pow(aux_32,2)+aux_29) ;
00646   aux_34 = aux_18*aux_23-aux_22*aux_20*aux_21 ;
00647   aux_35 = -aux_22*aux_20*aux_23-aux_18*aux_21 ;
00648   aux_36 = aux_22*aux_21-aux_18*aux_20*aux_23 ;
00649   aux_37 = -aux_20*aux_17*aux_23-aux_20*aux_14*aux_21-aux_1*aux_7*aux_19 ;
00650   aux_38 = -aux_20*aux_12*aux_23-aux_20*aux_10*aux_21-aux_4*aux_7*aux_19 ;
00651   aux_39 = -aux_7*aux_20*aux_5*aux_23-aux_7*aux_20*aux_3*aux_21+aux_2*aux_19 ;
00652   aux_40 = aux_19*aux_17*aux_23+aux_19*aux_14*aux_21-aux_1*aux_7*aux_20 ;
00653   aux_41 = aux_19*aux_12*aux_23+aux_19*aux_10*aux_21-aux_4*aux_7*aux_20 ;
00654   aux_42 = aux_7*aux_19*aux_5*aux_23+aux_7*aux_19*aux_3*aux_21+aux_2*aux_20 ;
00655   aux_43 = 1/sqrt(1-pow(aux_42*aux_27+aux_41*aux_26+aux_40*aux_8*aux_16,2)) ;
00656   aux_44 = aux_19*aux_17*aux_21-aux_19*aux_14*aux_23 ;
00657   aux_45 = aux_19*aux_12*aux_21-aux_19*aux_10*aux_23 ;
00658   aux_46 = aux_7*aux_19*aux_5*aux_21-aux_7*aux_19*aux_3*aux_23 ;
00659   aux_47 = aux_6*aux_15*aux_9-aux_16*aux_11 ;
00660   aux_48 = aux_6*aux_15*aux_11+aux_16*aux_9 ;
00661   aux_49 = aux_41*aux_48+aux_42*aux_47+aux_40*aux_8*aux_15 ;
00662   aux_50 = aux_41*aux_8*aux_11+aux_42*aux_8*aux_9-aux_40*aux_6 ;
00663   aux_51 = 1/(pow(aux_50,2)+pow(aux_49,2)) ;
00664 
00665   mat[0][0] =  aux_10*aux_8*aux_11+aux_7*aux_3*aux_8*aux_9+(-aux_4*aux_5-aux_1*aux_2*aux_3)*aux_6;
00666   mat[0][1] = aux_12*aux_8*aux_11+aux_7*aux_5*aux_8*aux_9+(aux_4*aux_3-aux_1*aux_2*aux_5)*aux_6;
00667   mat[0][2] = aux_4*aux_7*aux_8*aux_11-aux_2*aux_8*aux_9-aux_1*aux_7*aux_6;
00668   mat[0][3] = aux_13;
00669   mat[0][4] = aux_13;
00670   mat[0][5] = aux_13;
00671   mat[1][0] = (aux_10*aux_6*aux_15-aux_7*aux_3*aux_16)*aux_11+(aux_7*aux_3*aux_6*aux_15+aux_10*aux_16)*aux_9+aux_14*aux_8*aux_15;
00672   mat[1][1] = (aux_12*aux_6*aux_15-aux_7*aux_5*aux_16)*aux_11+(aux_7*aux_5*aux_6*aux_15+aux_12*aux_16)*aux_9+aux_17*aux_8*aux_15;
00673   mat[1][2] = (aux_4*aux_7*aux_6*aux_15+aux_2*aux_16)*aux_11+(aux_4*aux_7*aux_16-aux_2*aux_6*aux_15)*aux_9+aux_1*aux_7*aux_8*aux_15;
00674   mat[1][3] = aux_13;
00675   mat[1][4] = aux_13;
00676   mat[1][5] = aux_13;
00677   mat[2][0] = (aux_7*aux_3*aux_15+aux_10*aux_6*aux_16)*aux_11+((aux_1*aux_5-aux_4*aux_2*aux_3)*aux_15+aux_7*aux_3*aux_6*aux_16)*aux_9+aux_14*aux_8*aux_16;
00678   mat[2][1] = (aux_7*aux_5*aux_15+aux_12*aux_6*aux_16)*aux_11+((-aux_4*aux_2*aux_5-aux_1*aux_3)*aux_15+aux_7*aux_5*aux_6*aux_16)*aux_9+aux_17*aux_8*aux_16;
00679   mat[2][2] = (aux_4*aux_7*aux_6*aux_16-aux_2*aux_15)*aux_11+(-aux_4*aux_7*aux_15-aux_2*aux_6*aux_16)*aux_9+aux_1*aux_7*aux_8*aux_16;
00680   mat[2][3] = aux_13;
00681   mat[2][4] = aux_13;
00682   mat[2][5] = aux_13;
00683   mat[3][0] = aux_13;
00684   mat[3][1] = aux_13;
00685   mat[3][2] = aux_13;
00686   mat[3][3] = aux_29*aux_33-((aux_7*aux_5*aux_35+aux_7*aux_3*aux_34+aux_22*aux_2*aux_19)*aux_27+(aux_12*aux_35+aux_10*aux_34-aux_4*aux_22*aux_7*aux_19)*aux_26+(aux_17*aux_35+aux_14*aux_34-aux_1*aux_22*aux_7*aux_19)*aux_8*aux_16)*aux_32*aux_33;
00687   mat[3][4] = ((aux_22*aux_7*aux_19*aux_5*aux_23+aux_22*aux_7*aux_19*aux_3*aux_21+aux_22*aux_2*aux_20)*aux_27+(aux_22*aux_19*aux_12*aux_23+aux_22*aux_19*aux_10*aux_21-aux_4*aux_22*aux_7*aux_20)*aux_26+(aux_22*aux_19*aux_17*aux_23+aux_22*aux_19*aux_14*aux_21-aux_1*aux_22*aux_7*aux_20)*aux_8*aux_16)*aux_28*aux_33-((aux_18*aux_7*aux_19*aux_5*aux_23+aux_18*aux_7*aux_19*aux_3*aux_21+aux_18*aux_2*aux_20)*aux_27+(aux_18*aux_19*aux_12*aux_23+aux_18*aux_19*aux_10*aux_21-aux_4*aux_18*aux_7*aux_20)*aux_26+(aux_18*aux_19*aux_17*aux_23+aux_18*aux_19*aux_14*aux_21-aux_1*aux_18*aux_7*aux_20)*aux_8*aux_16)*aux_32*aux_33;
00688   mat[3][5] = aux_28*((aux_7*aux_3*aux_35+aux_7*aux_5*aux_30)*aux_27+(aux_10*aux_35+aux_12*aux_30)*aux_26+(aux_14*aux_35+aux_17*aux_30)*aux_8*aux_16)*aux_33-((aux_7*aux_3*aux_36+aux_7*aux_5*aux_24)*aux_27+(aux_10*aux_36+aux_12*aux_24)*aux_26+(aux_14*aux_36+aux_17*aux_24)*aux_8*aux_16)*aux_32*aux_33;
00689   mat[4][0] = aux_13;
00690   mat[4][1] = aux_13;
00691   mat[4][2] = aux_13;
00692   mat[4][3] = aux_13;
00693   mat[4][4] = -(aux_39*aux_27+aux_38*aux_26+aux_37*aux_8*aux_16)*aux_43;
00694   mat[4][5] = -(aux_46*aux_27+aux_45*aux_26+aux_44*aux_8*aux_16)*aux_43;
00695   mat[5][0] = aux_13;
00696   mat[5][1] = aux_13;
00697   mat[5][2] = aux_13;
00698   mat[5][3] = aux_13;
00699   mat[5][4] = aux_50*(aux_38*aux_48+aux_39*aux_47+aux_37*aux_8*aux_15)*aux_51-(aux_38*aux_8*aux_11+aux_39*aux_8*aux_9-aux_37*aux_6)*aux_49*aux_51;
00700   mat[5][5] = aux_50*(aux_45*aux_48+aux_46*aux_47+aux_44*aux_8*aux_15)*aux_51-(aux_45*aux_8*aux_11+aux_46*aux_8*aux_9-aux_44*aux_6)*aux_49*aux_51;
00701 }
00702 
00703 inline void propagateJacobianManifold(Matrix6& mat, const Vector6& xi, const Vector6& xj)
00704 {
00705   //const double& x1 = xi[0];
00706   //const double& y1 = xi[1];
00707   //const double& z1 = xi[2];
00708   const double& a1 = xi[3];
00709   const double& b1 = xi[4];
00710   const double& c1 = xi[5];
00711 
00712   //const double& x2 = xj[0];
00713   //const double& y2 = xj[1];
00714   //const double& z2 = xj[2];
00715   const double& a2 = xj[3];
00716   const double& b2 = xj[4];
00717   const double& c2 = xj[5];
00718 
00719   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 , aux_12 , aux_13 ,
00720          aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 , aux_21 , aux_22 , aux_23 , aux_24 , aux_25 ,
00721          aux_26 , aux_27 , aux_28 , aux_29 , aux_30 ;
00722   aux_1 = cos(b1) ;
00723   aux_2 = cos(c1) ;
00724   aux_3 = sin(c1) ;
00725   aux_4 = sin(b1) ;
00726   aux_5 = 0 ;
00727   aux_6 = sin(a1) ;
00728   aux_7 = cos(a1) ;
00729   aux_8 = aux_6*aux_4*aux_2-aux_7*aux_3 ;
00730   aux_9 = aux_6*aux_4*aux_3+aux_7*aux_2 ;
00731   aux_10 = aux_6*aux_3+aux_7*aux_4*aux_2 ;
00732   aux_11 = aux_7*aux_4*aux_3-aux_6*aux_2 ;
00733   aux_12 = cos(a2) ;
00734   aux_13 = cos(b2) ;
00735   aux_14 = sin(b2) ;
00736   aux_15 = cos(c2) ;
00737   aux_16 = sin(a2) ;
00738   aux_17 = sin(c2) ;
00739   aux_18 = aux_11*(aux_12*aux_14*aux_17-aux_16*aux_15)+aux_10*(aux_16*aux_17+aux_12*aux_14*aux_15)+aux_7*aux_12*aux_1*aux_13 ;
00740   aux_19 = std::pow(aux_18,2) ;
00741   aux_20 = aux_16*aux_14*aux_15-aux_12*aux_17 ;
00742   aux_21 = aux_16*aux_14*aux_17+aux_12*aux_15 ;
00743   aux_22 = aux_11*aux_21+aux_10*aux_20+aux_7*aux_16*aux_1*aux_13 ;
00744   aux_23 = 1/(std::pow(aux_22,2)+aux_19) ;
00745   aux_24 = aux_13*aux_11*aux_17+aux_13*aux_10*aux_15-aux_7*aux_1*aux_14 ;
00746   aux_25 = -aux_16*aux_17-aux_12*aux_14*aux_15 ;
00747   aux_26 = aux_16*aux_15-aux_12*aux_14*aux_17 ;
00748   aux_27 = 1/sqrt(1-std::pow(aux_24,2)) ;
00749   aux_28 = aux_13*aux_9*aux_17+aux_13*aux_8*aux_15-aux_6*aux_1*aux_14 ;
00750   aux_29 = aux_1*aux_13*aux_3*aux_17+aux_1*aux_13*aux_2*aux_15+aux_4*aux_14 ;
00751   aux_30 = 1/(std::pow(aux_29,2)+std::pow(aux_28,2)) ;
00752 
00753   mat[0][0] = aux_1*aux_2;
00754   mat[0][1] = aux_1*aux_3;
00755   mat[0][2] = -aux_4;
00756   mat[0][3] = aux_5;
00757   mat[0][4] = aux_5;
00758   mat[0][5] = aux_5;
00759   mat[1][0] = aux_8;
00760   mat[1][1] = aux_9;
00761   mat[1][2] = aux_6*aux_1;
00762   mat[1][3] = aux_5;
00763   mat[1][4] = aux_5;
00764   mat[1][5] = aux_5;
00765   mat[2][0] = aux_10;
00766   mat[2][1] = aux_11;
00767   mat[2][2] = aux_7*aux_1;
00768   mat[2][3] = aux_5;
00769   mat[2][4] = aux_5;
00770   mat[2][5] = aux_5;
00771   mat[3][0] = aux_5;
00772   mat[3][1] = aux_5;
00773   mat[3][2] = aux_5;
00774   mat[3][3] = aux_19*aux_23-(aux_11*(-aux_16*aux_14*aux_17-aux_12*aux_15)+aux_10*(aux_12*aux_17-aux_16*aux_14*aux_15)-aux_7*aux_16*aux_1*aux_13)*aux_22*aux_23;
00775   mat[3][4] = -aux_24*aux_22*aux_23;
00776   mat[3][5] = (-aux_13*aux_11*aux_17-aux_13*aux_10*aux_15+aux_7*aux_1*aux_14)*aux_18*aux_23;
00777   mat[4][0] = aux_5;
00778   mat[4][1] = aux_5;
00779   mat[4][2] = aux_5;
00780   mat[4][3] = aux_5;
00781   mat[4][4] = -(aux_11*aux_26+aux_10*aux_25-aux_7*aux_12*aux_1*aux_13)*aux_27;
00782   mat[4][5] = -aux_22*aux_27;
00783   mat[5][0] = aux_5;
00784   mat[5][1] = aux_5;
00785   mat[5][2] = aux_5;
00786   mat[5][3] = aux_5;
00787   mat[5][4] = aux_29*(aux_9*aux_26+aux_8*aux_25-aux_6*aux_12*aux_1*aux_13)*aux_30-aux_28*(aux_1*aux_3*aux_26+aux_1*aux_2*aux_25+aux_12*aux_4*aux_13)*aux_30;
00788   mat[5][5] = aux_29*(aux_9*aux_21+aux_8*aux_20+aux_6*aux_16*aux_1*aux_13)*aux_30-aux_28*(aux_1*aux_3*aux_21+aux_1*aux_2*aux_20-aux_16*aux_4*aux_13)*aux_30;
00789 }
00790 
00791 inline void motionJacobianState(Matrix6& mat, const Vector6& xi, const Vector6& e)
00792 {
00793   const double& ex  = e[0];
00794   const double& ey  = e[1];
00795   const double& ez  = e[2];
00796   //const double& er  = e[3];
00797   const double& ep  = e[4];
00798   const double& eya = e[5];
00799 
00800   //const double& x1 = xi[0];
00801   //const double& y1 = xi[1];
00802   //const double& z1 = xi[2];
00803   const double& a1 = xi[3];
00804   const double& b1 = xi[4];
00805   const double& c1 = xi[5];
00806 
00807   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 ,
00808          aux_12 , aux_13 , aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 , aux_21 ,
00809          aux_22 , aux_23 , aux_24 , aux_25 , aux_26;
00810 
00811   aux_1 = 1 ;
00812   aux_2 = 0 ;
00813   aux_3 = cos(a1) ;
00814   aux_4 = sin(b1) ;
00815   aux_5 = cos(c1) ;
00816   aux_6 = sin(a1) ;
00817   aux_7 = sin(c1) ;
00818   aux_8 = aux_6*aux_7+aux_3*aux_4*aux_5 ;
00819   aux_9 = cos(b1) ;
00820   aux_10 = -aux_6*aux_4*aux_7-aux_3*aux_5 ;
00821   aux_11 = sin(ep) ;
00822   aux_12 = std::pow(aux_9,2) ;
00823   aux_13 = cos(ep) ;
00824   aux_14 = cos(eya) ;
00825   aux_15 = sin(eya) ;
00826   aux_16 = std::pow(aux_6,2) ;
00827   aux_17 = std::pow(aux_13,2) ;
00828   aux_18 = -2*aux_3*aux_9*aux_4*aux_13*aux_11*aux_14 ;
00829   aux_19 = std::pow(aux_14,2) ;
00830   aux_20 = (2*aux_6*aux_9*aux_4*aux_17*aux_14+2*aux_3*aux_6*aux_12*aux_13*aux_11)*aux_15 ;
00831   aux_21 = aux_20+((aux_16+1)*aux_12-1)*aux_17*aux_19+aux_18+(1-2*aux_16)*aux_12*aux_17+(aux_16-1)*aux_12+1 ;
00832   aux_22 = 1/aux_21 ;
00833   aux_23 = std::pow(aux_3,2) ;
00834   aux_24 = std::pow(aux_15,2) ;
00835   aux_25 = ((aux_23-2)*aux_12+1)*aux_17*aux_24+aux_20+aux_18+((aux_23+1)*aux_12-1)*aux_17-aux_23*aux_12+1 ;
00836   aux_26 = 1/aux_25 ;
00837 
00838   mat[0][0] = aux_1;
00839   mat[0][1] = aux_2;
00840   mat[0][2] = aux_2;
00841   mat[0][3] = (aux_3*aux_7-aux_6*aux_4*aux_5)*ez+aux_8*ey;
00842   mat[0][4] = aux_3*aux_9*aux_5*ez+aux_6*aux_9*aux_5*ey-aux_4*aux_5*ex;
00843   mat[0][5] = (aux_6*aux_5-aux_3*aux_4*aux_7)*ez+aux_10*ey-aux_9*aux_7*ex;
00844   mat[1][0] = aux_2;
00845   mat[1][1] = aux_1;
00846   mat[1][2] = aux_2;
00847   mat[1][3] = aux_10*ez+(aux_3*aux_4*aux_7-aux_6*aux_5)*ey;
00848   mat[1][4] = aux_3*aux_9*aux_7*ez+aux_6*aux_9*aux_7*ey-aux_4*aux_7*ex;
00849   mat[1][5] = aux_8*ez+(aux_6*aux_4*aux_5-aux_3*aux_7)*ey+aux_9*aux_5*ex;
00850   mat[2][0] = aux_2;
00851   mat[2][1] = aux_2;
00852   mat[2][2] = aux_1;
00853   mat[2][3] = aux_3*aux_9*ey-aux_6*aux_9*ez;
00854   mat[2][4] = -aux_3*aux_4*ez-aux_6*aux_4*ey-aux_9*ex;
00855   mat[2][5] = aux_2;
00856   mat[3][0] = aux_2;
00857   mat[3][1] = aux_2;
00858   mat[3][2] = aux_2;
00859   mat[3][3] = (aux_6*aux_9*aux_4*aux_13*aux_15+aux_12*aux_13*aux_14-aux_3*aux_9*aux_4*aux_11)*aux_22;
00860   mat[3][4] = (aux_3*aux_13*aux_15+aux_6*aux_11)*aux_26;
00861   mat[3][5] = aux_2;
00862   mat[4][0] = aux_2;
00863   mat[4][1] = aux_2;
00864   mat[4][2] = aux_2;
00865   mat[4][3] = -(aux_3*aux_9*aux_13*aux_15+aux_6*aux_9*aux_11)/sqrt(aux_21);
00866   mat[4][4] = (aux_6*aux_4*aux_13*aux_15+aux_9*aux_13*aux_14-aux_3*aux_4*aux_11)/sqrt(aux_25);
00867   mat[4][5] = aux_2;
00868   mat[5][0] = aux_2;
00869   mat[5][1] = aux_2;
00870   mat[5][2] = aux_2;
00871   mat[5][3] = -(aux_6*aux_9*aux_17*aux_14*aux_15-aux_4*aux_17*aux_19-aux_3*aux_9*aux_13*aux_11*aux_14+aux_4)*aux_22;
00872   mat[5][4] = -(aux_3*aux_6*aux_9*aux_17*aux_24+((1-2*aux_23)*aux_9*aux_13*aux_11-aux_3*aux_4*aux_17*aux_14)*aux_15-aux_6*aux_4*aux_13*aux_11*aux_14+aux_3*aux_6*aux_9*aux_17-aux_3*aux_6*aux_9)*aux_26;
00873   mat[5][5] = aux_1;
00874 }
00875 
00876 inline void motionJacobianMeasurement(Matrix6& mat, const Vector6& xi, const Vector6& e)
00877 {
00878   //const double& ex  = e[0];
00879   //const double& ey  = e[1];
00880   //const double& ez  = e[2];
00881   //const double& er  = e[3];
00882   const double& ep  = e[4];
00883   const double& eya = e[5];
00884 
00885   //const double& x1 = xi[0];
00886   //const double& y1 = xi[1];
00887   //const double& z1 = xi[2];
00888   const double& a1 = xi[3];
00889   const double& b1 = xi[4];
00890   const double& c1 = xi[5];
00891 
00892   double aux_1 , aux_2 , aux_3 , aux_4 , aux_5 , aux_6 , aux_7 , aux_8 , aux_9 , aux_10 , aux_11 ,
00893          aux_12 , aux_13 , aux_14 , aux_15 , aux_16 , aux_17 , aux_18 , aux_19 , aux_20 ;
00894 
00895   aux_1 = cos(b1) ;
00896   aux_2 = cos(c1) ;
00897   aux_3 = sin(a1) ;
00898   aux_4 = sin(b1) ;
00899   aux_5 = cos(a1) ;
00900   aux_6 = sin(c1) ;
00901   aux_7 = 0 ;
00902   aux_8 = cos(ep) ;
00903   aux_9 = std::pow(aux_1,2) ;
00904   aux_10 = sin(ep) ;
00905   aux_11 = cos(eya) ;
00906   aux_12 = std::pow(aux_11,2) ;
00907   aux_13 = std::pow(aux_3,2) ;
00908   aux_14 = (aux_13+1)*aux_9-1 ;
00909   aux_15 = sin(eya) ;
00910   aux_16 = (aux_13-1)*aux_9 ;
00911   aux_17 = std::pow(aux_8,2) ;
00912   aux_18 = (2*aux_3*aux_1*aux_4*aux_17*aux_11+2*aux_5*aux_3*aux_9*aux_8*aux_10)*aux_15+aux_14*aux_17*aux_12-2*aux_5*aux_1*aux_4*aux_8*aux_10*aux_11+(1-2*aux_13)*aux_9*aux_17+aux_16+1 ;
00913   aux_19 = 1/aux_18 ;
00914   aux_20 = 1/sqrt(aux_18) ;
00915 
00916   mat[0][0] = aux_1*aux_2;
00917   mat[0][1] = aux_3*aux_4*aux_2-aux_5*aux_6;
00918   mat[0][2] = aux_3*aux_6+aux_5*aux_4*aux_2;
00919   mat[0][3] = aux_7;
00920   mat[0][4] = aux_7;
00921   mat[0][5] = aux_7;
00922   mat[1][0] = aux_1*aux_6;
00923   mat[1][1] = aux_3*aux_4*aux_6+aux_5*aux_2;
00924   mat[1][2] = aux_5*aux_4*aux_6-aux_3*aux_2;
00925   mat[1][3] = aux_7;
00926   mat[1][4] = aux_7;
00927   mat[1][5] = aux_7;
00928   mat[2][0] = -aux_4;
00929   mat[2][1] = aux_3*aux_1;
00930   mat[2][2] = aux_5*aux_1;
00931   mat[2][3] = aux_7;
00932   mat[2][4] = aux_7;
00933   mat[2][5] = aux_7;
00934   mat[3][0] = aux_7;
00935   mat[3][1] = aux_7;
00936   mat[3][2] = aux_7;
00937   mat[3][3] = 1;
00938   mat[3][4] = -((aux_14*aux_8*aux_11-aux_5*aux_1*aux_4*aux_10)*aux_15-2*aux_3*aux_1*aux_4*aux_8*aux_12-aux_5*aux_3*aux_9*aux_10*aux_11+aux_3*aux_1*aux_4*aux_8)*aux_19;
00939   mat[3][5] = -(aux_5*aux_3*aux_9*aux_8*aux_15-aux_5*aux_1*aux_4*aux_8*aux_11+(aux_16+1)*aux_10)*aux_19;
00940   mat[4][0] = aux_7;
00941   mat[4][1] = aux_7;
00942   mat[4][2] = aux_7;
00943   mat[4][3] = aux_7;
00944   mat[4][4] = (aux_3*aux_1*aux_10*aux_15-aux_4*aux_10*aux_11+aux_5*aux_1*aux_8)*aux_20;
00945   mat[4][5] = -(aux_4*aux_8*aux_15+aux_3*aux_1*aux_8*aux_11)*aux_20;
00946   mat[5][0] = aux_7;
00947   mat[5][1] = aux_7;
00948   mat[5][2] = aux_7;
00949   mat[5][3] = aux_7;
00950   mat[5][4] = (aux_4*aux_15+aux_3*aux_1*aux_11)*aux_19;
00951   mat[5][5] = (aux_3*aux_1*aux_8*aux_10*aux_15-aux_4*aux_8*aux_10*aux_11+aux_5*aux_1*aux_17)*aux_19;
00952 }
00953 
00954 } // end namespace
00955 
00956 #endif
posegraph3d_gradient.h
