asr_flock_of_birds: transform_coords.cc Source File

Go to the documentation of this file.
00001 
00018 /* system includes */
00019 /* (none) */
00020 
00021 /* my includes */
00022 #include "transform_coords.h"
00023 #include "bird_track_impl.h"
00024 #include "math.h"
00025 #include "complex.h"
00026 #include "polynomial.h"
00027 #include "quaternion.h"
00028 
00029 // Emitter unter dem Tisch auf nem Wasserkasten - Wird nur verwendet falls nix anderes zur Verfgung steht
00030 #define POLHEMUS_POS_X 1284.6
00031 #define POLHEMUS_POS_Y 1319.2
00032 #define POLHEMUS_POS_Z -553.0
00033 #define KOORD_OFFSET_X -16.0
00034 #define KOORD_OFFSET_Y 65.0
00035 #define KOORD_OFFSET_Z 0.0
00036 
00037 bool calibrated;
00038 
00039 double fobCoordList[256][6];
00040 double worldCoordList[256][6];
00041 int countCoord;
00042 
00043 void
00044 transformCoords::resetCalibration()
00045 {
00046   calibrated=false;
00047   countCoord=0;
00048 }
00049 
00050 transformCoords::transformCoords()
00051 {
00052   calibrated=false;
00053   countCoord=0;
00054 }
00055 
00056 transformCoords::~transformCoords()
00057 {
00058 }
00059 
00060 /*
00061 bool
00062 transformCoords::loadCalibFile(const char *srcFileName)
00063 { //Load and Save is not implemented yet, since the Calibration GUI is able to load and save the
00064   //Coord lists anyway
00065   return true;
00066 }
00067 
00068 bool
00069 transformCoords::saveCalibFile(const char *srcFileName)
00070 { //Same here
00071   return true;
00072 }
00073 */
00074 double S[3][3];
00075 double N[4][4];
00076 double Cm[3];
00077 double Cs[3];
00078 double c2,c1,c0;
00079 double l;
00080 int ERR=0;
00081 
00082 quaternion solution;//Rotation
00083 double scale;//Scale
00084 double trans[3];//
00085 quaternion transq;//Translation
00086 
00087 void calcC(){
00088         for (int x=0;x<3;x++)
00089                 Cs[x]=Cm[x]=0;
00090         for (int i=0;i<countCoord;i++)
00091         {
00092                 for (int x=0;x<3;x++)
00093                 {
00094                         Cs[x]+=worldCoordList[i][x];
00095                         Cm[x]+=fobCoordList[i][x];
00096                 }
00097         }
00098         for (int x=0;x<3;x++)
00099         {
00100                 Cs[x]=Cs[x]/countCoord;
00101                 Cm[x]=Cm[x]/countCoord;
00102         }
00103 
00104 }
00105 
00106 double sqr(double a){
00107         return a*a;
00108 }
00109 
00110 void calcS(){
00111   double distf=0,distw=0;
00112 
00113   for (int x=0;x<3;x++)
00114                 for (int y=0;y<3;y++)
00115                         S[x][y]=0;
00116         
00117         for (int i=0;i<countCoord;i++)
00118           {distw+=sqrt(sqr(worldCoordList[i][0]-Cs[0])+sqr(worldCoordList[i][1]-Cs[1])+sqr(worldCoordList[i][2]-Cs[2]));
00119            distf+=sqrt(sqr(fobCoordList[i][0]-Cs[0])+sqr(fobCoordList[i][1]-Cs[1])+sqr(fobCoordList[i][2]-Cs[2]));
00120           }
00121         distf=distf*distw/countCoord/countCoord/10; //??? Was ist dieser distf-Faktor?
00122         
00123         for (int i=0;i<countCoord;i++)
00124                 for (int x=0;x<3;x++)
00125                         for (int y=0;y<3;y++)
00126                                 S[x][y]+=(worldCoordList[i][x]-Cs[x])*(fobCoordList[i][y]-Cm[y])/distf;
00127 }
00128 
00129 void calcN(){
00130         N[0][0]=S[0][0]+S[1][1]+S[2][2]; N[0][1]=S[1][2]-S[2][1];         N[0][2]=S[2][0]-S[0][2];          N[0][3]=S[0][1]-S[1][0]; 
00131         N[1][0]=S[1][2]-S[2][1];         N[1][1]=S[0][0]-S[1][1]-S[2][2]; N[1][2]=S[0][1]+S[1][0];          N[1][3]=S[2][0]+S[0][2]; 
00132         N[2][0]=S[2][0]-S[0][2];         N[2][1]=S[0][1]+S[1][0];         N[2][2]=-S[0][0]+S[1][1]-S[2][2]; N[2][3]=S[1][2]+S[2][1]; 
00133         N[3][0]=S[0][1]-S[1][0];         N[3][1]=S[2][0]+S[0][2];         N[3][2]=S[1][2]+S[2][1];          N[3][3]=-S[0][0]-S[1][1]+S[2][2]; 
00134 }
00135 
00136 void calc_c(){
00137 c2= (N[0][0]*N[2][2] - sqr(N[0][2])) + (N[1][1]*N[2][2] - sqr(N[1][2])) + 
00138                 (N[0][0]*N[3][3] - sqr(N[0][3])) + (N[1][1]*N[3][3] - sqr(N[1][3])) +
00139             (N[2][2]*N[3][3] + sqr(N[2][3])) + (N[0][0]*N[1][1] - sqr(N[0][1]));
00140 
00141 c1 = -N[1][1]*(N[2][2]*N[3][3] - sqr(N[2][3])) + N[1][2]*(N[3][3]*N[1][2] - N[2][3]*N[1][3]) - N[1][3]*(N[1][2]*N[2][3] - N[2][2]*N[1][3])
00142          -N[0][0]*(N[2][2]*N[3][3] - sqr(N[2][3])) + N[0][2]*(N[3][3]*N[0][2] - N[2][3]*N[0][3]) - N[0][3]*(N[2][3]*N[0][2] - N[2][2]*N[0][3])
00143          -N[0][0]*(N[1][1]*N[3][3] - sqr(N[1][3])) + N[0][1]*(N[3][3]*N[0][1] - N[1][3]*N[0][3]) - N[0][3]*(N[0][1]*N[1][3] - N[1][1]*N[0][3])
00144          -N[0][0]*(N[1][1]*N[2][2] - sqr(N[1][2])) + N[0][1]*(N[2][2]*N[0][1] - N[1][2]*N[0][2]) - N[0][2]*(N[0][1]*N[1][2] - N[1][1]*N[0][2]);
00145 
00146 c0 =  (N[0][0]*N[1][1] - sqr(N[0][1]))*(N[2][2]*N[3][3] - sqr(N[2][3])) + (N[0][1]*N[0][2] - N[0][0]*N[1][2])*(N[1][2]*N[3][3] - N[2][3]*N[1][3])
00147         + (N[0][0]*N[1][3] - N[0][1]*N[0][3])   *   (N[1][2]*N[2][3] - N[2][2]*N[1][3])   + (N[0][1]*N[1][2] - N[1][1]*N[0][2])*(N[0][2]*N[3][3] - N[2][3]*N[0][3])
00148         + (N[1][1]*N[0][3] - N[0][1]*N[1][3])   *   (N[0][2]*N[2][3] - N[2][2]*N[0][3])   +   sqr(N[0][2]*N[1][3] - N[1][2]*N[0][3]);
00149 //printf("x^4+%fx^2+%fx+%f\n",c2,c1,c0);
00150 }
00151 
00152 void find_max_lambda(){
00153         double a[4];
00154         double x1,x2,x3,x4;
00155         int c;
00156         int i;
00157         int b=0;
00158         c=solve_biquadratic(1,0,c2,c1,c0,&x1,&x2,&x3,&x4);
00159         a[0]=x1;a[1]=x2;a[2]=x3;a[3]=x4;
00160         printf("Solutions for Lamda (%d): ",c);
00161         for (i=0;i<c;i++) printf("%f ",a[i]);
00162         printf("\n");
00163         if (c!=0) l=a[0]; else l=-HUGE_VAL;
00164         if (c>1)
00165                 for (i=1;i<c;i++)
00166                         if (a[i]>l) l=a[i];
00167                 for (i=1;i<c;i++)
00168                         if (abs(a[i]-l)<epsilon) b++;
00169         if (b>1) ERR=1; //Zwei gleiche maximale Eigenwerte -> Der dazugeh�ige Eigenraum ist nicht eindimensional
00170     printf ("Error after find_max_lambda: %d\n", ERR);
00171 }
00172 
00173 void flipLines(int a,int b){//Flips two lines within the matrix
00174         for (int i=0;i<4;i++)
00175         {
00176                 double c=N[a][i];
00177                 N[a][i]=N[b][i];
00178                 N[b][i]=c;
00179         }
00180 }
00181 
00182 void mAddLines(int to,int from,double value){//Adds line "from" to line "to muliplied with "value"
00183                 for (int i=0;i<4;i++)
00184                         N[to][i]+=N[from][i]*value;
00185 }
00186 
00187 void setQuat(int pos,quaternion* q,double value)//If you ever need to access a quaternion like an array
00188 {
00189         switch (pos)
00190         {
00191         case 0:q->r=value;break;
00192         case 1:q->i=value;break;
00193         case 2:q->j=value;break;
00194         case 3:q->k=value;break;
00195         }
00196 }
00197 
00198 double NB[4][4];
00199 
00200 void calc_eigenVec(){
00201 //Calculation of N-Lambda*I
00202         solution.r=0;
00203         solution.i=0;
00204         solution.j=0;
00205         solution.k=0;
00206         for (int i=0;i<4;i++)
00207                 N[i][i]-=l;
00208         for (int i=0;i<4;i++)
00209                 for (int j=0;j<4;j++)
00210                         NB[i][j]=N[i][j];
00211         {
00212         int j=-1;
00213         for (int i=0;i<4;i++)
00214         {int l=0;
00215                 for (int k=0;k<4;k++)
00216                 {
00217                         if (abs(N[k][i])>epsilon) l++;
00218                 }
00219                 if (l==1) j=i;
00220         }
00221         if (j>=0) {setQuat(j,&solution,1);return;}      //One row is zero, so there is a solution with one value not equal zero
00222         }
00223         for (int i1=0;i1<4;i1++)        //there is a solution with two value not equal zero
00224                 for (int j=0;j<3;j++)
00225                         if ((i1!=j)&&(abs(N[0][i1])>epsilon))
00226                         {       int s=0;
00227                                 for (int k=1;k<4;k++)
00228                                         if ((abs(N[k][i1])<epsilon)&&(abs(N[k][j])<epsilon)) s++;
00229                                         else if (abs(N[k][i1])>epsilon)
00230                                                 if (abs(N[k][j]/N[k][i1]-N[0][j]/N[0][i1])<epsilon) s++;
00231                                 if (s==3)
00232                                 {
00233                                         setQuat(j,&solution,N[0][i1]);
00234                                         setQuat(i1,&solution,-N[0][j]);
00235                                         return;
00236                                 }
00237                         }
00238         //try to find a solution with (a,b,1,0) and a,b!=0
00239         {       
00240                 int m=-1;
00241                 int n,p;
00242                 for (int i2=1;i2<4;i2++)
00243                         if (abs(N[0][0]*N[i2][1]-N[i2][0]*N[0][1])>=epsilon)    //Sind Zeile 1 und m linear abh�gig?
00244                         {m=i2;break;}
00245                 double a=-(N[m][2]*N[0][0]-N[0][2]*N[m][0])/(N[0][0]*N[m][1]-N[m][0]*N[0][1]);
00246                 double b= (N[m][2]*N[0][1]-N[0][2]*N[m][1])/(N[0][0]*N[m][1]-N[m][0]*N[0][1]);
00247                 if (m>0)                        //Es gibt eine zu m linear unabh�gige Zeile
00248                 {
00249                         switch (m) //Bestimmung der beiden bisher nichtbenutzten Zeilen
00250                         {
00251                                 case 1:n=2;p=3;break;
00252                                 case 2:n=1;p=3;break;
00253                                 case 3:n=1;p=2;break;
00254                         }
00255                         if ((abs(a*N[n][0]+b*N[n][1]+N[n][2])<epsilon)&&(abs(a*N[p][0]+b*N[p][1]+N[p][2])<epsilon))
00256                         {
00257                                         setQuat(0,&solution,a);
00258                                         setQuat(1,&solution,b);
00259                                         setQuat(2,&solution,1);
00260                                         return;
00261                         }
00262                 }
00263                 
00264         }
00265 //Gibt es wenigstens eine Lösung mit (a,b,c,1)
00266         solution.k=1;
00267         for (int i=0;i<4;i++)   //
00268                 N[i][3]*=-1;    
00269         if (abs(N[0][0])<epsilon) flipLines(0,1);
00270         if (abs(N[0][0])<epsilon) flipLines(0,2);
00271         if (abs(N[0][0])<epsilon) flipLines(0,3);
00272         if (abs(N[0][0])<epsilon) {ERR=2;return;}//Darf eigentlich nicht passieren
00273         
00274         for (int i=1;i<4;i++)
00275                 mAddLines(i,0,-N[i][0]/N[0][0]);//Erste Spalte auf Null setzen  
00276         if (abs(N[1][1])<epsilon) flipLines(1,2);
00277         if (abs(N[1][1])<epsilon) flipLines(1,3);
00278         if (abs(N[1][1])<epsilon) {ERR=2;return;}//Darf eigentlich auch nicht passieren
00279         
00280         for (int i=2;i<4;i++)
00281                 mAddLines(i,1,-N[i][1]/N[1][1]);//Zweite Spalte auf Null setzen
00282                 if (abs(N[2][2])<epsilon) flipLines(2,3);
00283         if (abs(N[2][2])<epsilon) {ERR=2;return;}//Darf auch nicht passieren
00284         if ((abs(N[2][2])<epsilon)&&(abs(N[2][3])>=epsilon)) {ERR=3;return;}    //Es gibt keine Lösung
00285         if ((abs(N[3][2])<epsilon)&&(abs(N[3][3])>=epsilon)) {ERR=3;return;}    //Es gibt keine Lösung
00286         for (int i=0;i<2;i++)
00287                 if (abs(N[i][i])<epsilon) {ERR=4;return;}       //Der Lösungsraum ist nicht eindimensional
00288                 
00289         if ((abs(N[3][3])<epsilon)&&(abs(N[4][4])<epsilon)) {ERR=4;return;}     //Der Lösungsraum ist nicht eindimensional
00290         
00291         solution.k=1;
00292         solution.j=(0.5*N[2][3]/N[2][2]+0.5*N[3][3]/N[3][2]);
00293         solution.i=((N[1][3]-solution.j*N[1][2])/N[1][1]);
00294         solution.r=((N[0][3]-solution.j*N[0][2]-solution.i*N[0][1])/N[0][0]);
00295 }
00296 
00297 void calc_scale()
00298 {
00299 double a=0;
00300 double b=0;
00301 for (int i=0;i<countCoord;i++)
00302 {
00303         a+=sqr(worldCoordList[i][0]-Cs[0])+sqr(worldCoordList[i][1]-Cs[1])+sqr(worldCoordList[i][2]-Cs[2]);
00304         b+=sqr(fobCoordList[i][0]-Cm[0])+sqr(fobCoordList[i][1]-Cm[1])+sqr(fobCoordList[i][2]-Cm[2]);
00305 }
00306 scale=sqrt(a/b);
00307  printf("Scale is %f\n",scale);
00308 }
00309 
00310 void calc_trans()
00311 {quaternion tr,cm,cs;
00312 cm.r=0;
00313 cm.i=Cm[0];
00314 cm.j=Cm[1];
00315 cm.k=Cm[2];
00316 cs.r=0;
00317 cs.i=Cs[0];
00318 cs.j=Cs[1];
00319 cs.k=Cs[2];
00320 tr=qsub(cs,qscale(scale,qmul(qmul(solution,cm),qinv(solution))));
00321 trans[0]=tr.i;
00322 trans[1]=tr.j;
00323 trans[2]=tr.k;
00324 transq=tr;
00325  printf("Transformation Vector is:%f %f %f\n",trans[0],trans[1],trans[2]);
00326 }
00327 
00328 void transform_point(double xi, double yi, double zi,double* xo, double* yo, double* zo)
00329 {
00330 quaternion q;
00331 q.r=0;
00332 q.i=xi;
00333 q.j=yi;
00334 q.k=zi;
00335 q=qadd(transq,qscale(scale,qmul(qmul(solution,q),qinv(solution))));
00336 *xo=q.i;
00337 *yo=q.j;
00338 *zo=q.k;
00339 }
00340 
00341 bool
00342 transformCoords::addCalibrationData(double *fobCoords,double *worldCoords)
00343 {
00344   ERR=0;
00345   //try to compute transformation from all available data and return true if calibration is possible with available data
00346   if (countCoord<256)
00347         {
00348         for (int i=0;i<6;i++)
00349                 {
00350                 worldCoordList[countCoord][i]=worldCoords[i];
00351                 fobCoordList[countCoord][i]=fobCoords[i];
00352                 }
00353         countCoord++;
00354         }
00355   if (countCoord>=3)
00356         {
00357                 calcC();
00358             calc_scale();
00359                 calcS();
00360                 calcN();
00361                 calc_c();
00362                 find_max_lambda();
00363                 if (ERR==0)
00364                 {
00365                     calc_eigenVec();
00366                         solution.i=-solution.i;
00367                         solution.j=-solution.j;
00368                         solution.k=-solution.k;
00369                         printf("Rotation Quaternion:");
00370                         qprint(solution);
00371                 }
00372                 if (ERR==0)
00373                         calc_trans();
00374                 calibrated=(ERR==0);
00375         }
00376   //Calibrate from available data and set calibrated=false if there is some unsolveable trouble
00377   cout << "Calibrated: " << calibrated << "(error: " << ERR << ")\n";
00378   return calibrated;
00379 }
00380 
00381 void
00382 transformCoords::transform(double *v)
00383 { double x,y,z;
00384   if (calibrated)
00385         { 
00386                 x=v[0];
00387                 y=v[1];
00388                 z=v[2];
00389                 transform_point(x,y,z,&x,&y,&z);
00390                 v[0]=x;
00391                 v[1]=y;
00392                 v[2]=z;
00393         }
00394   else
00395         {
00396           printf("Not calibrated call of transform\n");
00397                 //Actually nothing to do, except you want to implement some generic calibration
00398         }
00399 }
00400 
00401 
00402 
00403 bool
00404 transformCoords::isCalibrated()
00405 {
00406   return calibrated;
00407 }
00408 
00409 #if transformCoords_test
00410 #include <stdio.h>
00411 int main(int argc, char **argv)
00412 {
00413   // This is a module-test block. You can put code here that tests
00414   // just the contents of this C file, and build it by saying
00415   //             make transformCoords_test
00416   // Then, run the resulting executable (transformCoords_test).
00417   // If it works as expected, the module is probably correct. ;-)
00418 
00419   fprintf(stderr, "Testing transformCoords\n");
00420 
00421   return 0;
00422 }
00423 #endif /* transformCoords_test */