00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
00028
00029
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
00161
00162
00163 const double& er = e[3];
00164 const double& ep = e[4];
00165 const double& eya = e[5];
00166
00167
00168
00169
00170 const double& a1 = xi[3];
00171 const double& b1 = xi[4];
00172 const double& c1 = xi[5];
00173
00174
00175
00176
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
00281
00282
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
00388
00389
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
00449
00450
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
00587
00588
00589 const double& er = e[3];
00590 const double& ep = e[4];
00591 const double& eya = e[5];
00592
00593
00594
00595
00596 const double& a1 = xi[3];
00597 const double& b1 = xi[4];
00598 const double& c1 = xi[5];
00599
00600
00601
00602
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
00706
00707
00708 const double& a1 = xi[3];
00709 const double& b1 = xi[4];
00710 const double& c1 = xi[5];
00711
00712
00713
00714
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
00797 const double& ep = e[4];
00798 const double& eya = e[5];
00799
00800
00801
00802
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
00879
00880
00881
00882 const double& ep = e[4];
00883 const double& eya = e[5];
00884
00885
00886
00887
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 }
00955
00956 #endif