util.cpp
Go to the documentation of this file.
1 #include "util.h"
2 
3 
4 #include <unistd.h>
5 #include <iostream>
6 #include <stdlib.h>
7 #include <iomanip>
8 #include <chrono>
9 #include <fstream>
10 
20 {
21  bool ok=false;
22 
23  while (!ok)
24  {
25  // create four random numbers between -1 and 1
26  e=Vector4::Random();
27 
28  // normalize the randomly generated quaternion
29  if (((e(0)*e(0)+e(1)*e(1))<1)&&((e(2)*e(2)+e(3)*e(3))<1))
30  {
31  double norma = sqrt((1-e(0)*e(0)-e(1)*e(1))/(e(2)*e(2)+e(3)*e(3)));
32 
33  e(2) = e(2) * norma;
34  e(3) = e(3) * norma;
35 
36  ok=(e.norm()==1.0);
37  }
38  }
39 }
40 
41 void Quat2Mat(Matrix3 &R, Vector4 &e)
42 {
43  real e00,e01,e02,e03,e11,e12,e13,e22,e23,e33;
44 
45  e00=e(0)*e(0);
46  e01=e(0)*e(1);
47  e02=e(0)*e(2);
48  e03=e(0)*e(3);
49 
50  e11=e(1)*e(1);
51  e12=e(1)*e(2);
52  e13=e(1)*e(3);
53 
54  e22=e(2)*e(2);
55  e23=e(2)*e(3);
56 
57  e33=e(3)*e(3);
58 
59  R(0,0) = e00 + e11 - e22 - e33;
60  R(0,1) = 2 * (e12 - e03);
61  R(0,2) = 2 * (e13 + e02);
62 
63  R(1,0) = 2 * (e12 + e03);
64  R(1,1) = e00 - e11 + e22 - e33;
65  R(1,2) = 2 * (e23 - e01);
66 
67  R(2,0) = 2 * (e13 - e02);
68  R(2,1) = 2 * (e23 + e01);
69  R(2,2) = e00 - e11 - e22 + e33;
70 
71  //If e is a unit quaternion, the following instruction can be removed
72 
73  real n=(e00 + e11 + e22+ e33);
74 
75  R=(1/n)*R;
76 }
77 
78 void RandomMatrix(Matrix3 &M,real noiseLevel,bool gaussian,default_random_engine& e)
79 {
80  // We define two static variables, one for normal and another for uniform distributions
81  static std::normal_distribution<real> normal(0,0.5);
82  static std::uniform_real_distribution<real> uniform(-1,1);
83 
84  if (gaussian)
85  {
86  for(unsigned int i=0;i<3;i++)
87  {
88  for(unsigned int j=0;j<3;j++)
89  M(i,j)=normal(e)*noiseLevel; // 95% of samples in [-noiseLevel,noiseLevel]
90  }
91  }
92  else
93  {
94  for(unsigned int i=0;i<3;i++)
95  {
96  for(unsigned int j=0;j<3;j++)
97  M(i,j)=uniform(e)*noiseLevel;
98  }
99  }
100 }
101 
102 void TestMethod(ONMethod *method,unsigned int nRep,Matrix3 *N,double *res)
103 {
104  unsigned int i;
105  Matrix3 X;
106  Matrix3d Xd,E1,E2,I=Matrix3d::Identity();
107  Array<double,Dynamic,Dynamic> error(nRep,2);
108 
109  /* First we perform the orthnomalization with the only purpose of obtaining execution time */
110  auto start = chrono::high_resolution_clock::now();
111 
112  for(i=0;i<nRep;i++)
113  method(X,N[i]);
114 
115  auto finish = chrono::high_resolution_clock::now();
116  chrono::duration<double> time = finish - start;
117  res[AverageT]=time.count()/(double)nRep;
118 
119  /* Now we repeat the process to obtain the error metrics */
120  for(i=0;i<nRep;i++)
121  {
122  method(X,N[i]);
123 
124  /* We compute the errors in double precision for accuracy */
125  Xd=X.cast<double>(); // If already using doubles, this does nothing
126 
127  /* Distance to the original random rotation */
128  E1=Xd-N[i].cast<double>();
129  error(i,0)=E1.norm();
130 
131  /* Distance to the identity matrix */
132  E2=(Xd.transpose()*Xd)-I;
133  error(i,1)=E2.norm();
134  }
135 
136  res[MeanDstE]=error.col(0).mean();
137  res[MaxDstE]=error.col(0).maxCoeff();
138  res[MinDstE]=error.col(0).minCoeff();
139 
140  res[MeanOrtoE]=error.col(1).mean();
141  res[MaxOrtoE]=error.col(1).maxCoeff();
142  res[MinOrtoE]=error.col(1).minCoeff();
143 }
144 
145 void PrintResults(unsigned int nMethods,string *methodLabel,
146  unsigned int nResults,string *resultLabel,
147  unsigned int noiseTicks,noise &noiseLevel,double ***results)
148 {
149  unsigned int i,k,l;
150 
151  cout.setf(ios::fixed, ios::floatfield);
152  cout.precision(20);
153 
154  // Print on screen
155  for(l=0;l<nResults;l++)
156  {
157  cout << resultLabel[l] << endl << endl;
158 
159  // Top row with the labels
160  for(k=0;k<nMethods;k++)
161  {
162  cout << methodLabel[k];
163  if (k<nMethods-1)
164  cout << setw(26);
165  else
166  cout << endl;
167  }
168 
169  cout << "-----------------------------------------------------------------------------------------------------------------------" << endl;
170 
171  for(i=0;i<noiseTicks;i++)
172  {
173  for(k=0;k<nMethods;k++)
174  {
175  cout << results[k][i][l];
176  if (k<nMethods-1)
177  cout << setw(25);
178  else
179  cout << endl;
180  }
181  }
182  cout << endl << endl << endl;
183  }
184 }
185 
186 void SaveResults(unsigned int nMethods,string *methodLabel,
187  unsigned int nResults,string *resultLabel,
188  bool gaussian,unsigned int noiseTicks,noise &noiseLevel,double ***results)
189 {
190  string fName;
191  ofstream file;
192  unsigned i,k,l;
193  string suffix;
194 
195  #if (USE_DOUBLE)
196  string precision="_double";
197  #else
198  string precision="_float";
199  #endif
200 
201  if (gaussian)
202  suffix="_gaussian"+precision;
203  else
204  suffix="_uniform"+precision;
205 
206  // Save on files: one file per method with all the result (one row for each error level
207  for(k=0;k<nMethods;k++)
208  {
209  string fname="results/"+methodLabel[k]+suffix+".csv";
210  cout << "Generating file : " << fname << endl;
211 
212  file.open(fname);
213 
214  file.precision(20);
215  file << fixed;
216 
217  // Top row with the labels of each result
218  file << "N, " << "Noise, ";
219  for(l=0;l<nResults;l++)
220  {
221  file << resultLabel[l] ;
222  if (l<nResults-1)
223  file << ", ";
224  else
225  file << endl;
226  }
227 
228  for(i=0;i<noiseTicks;i++)
229  {
230  file << i << ", " << noiseLevel(i) << ", ";
231  for(l=0;l<nResults;l++)
232  {
233  file << results[k][i][l];
234  if (l<nResults-1)
235  file << ", ";
236  else
237  file << endl;
238  }
239  }
240  file.close();
241  }
242 
243  // Save in Matlab format
244  string fname="results/results_cpp"+suffix+".m";
245 
246  cout << "Generating file : " << fname << endl;
247 
248  file.open(fname);
249  file << "function [errorbound_cpp"<< suffix << ",error_cpp"<< suffix << ",time_cpp"<< suffix << "]=results_cpp"<< suffix << "()"<< endl;
250  file << " %% Error levels" << endl;
251  file << " errorbound_cpp"<< suffix << "=[";
252  for(i=0;i<noiseTicks;i++)
253  file << noiseLevel(i) << " ";
254  file << "];" << endl<< endl;
255  file << " error_cpp"<< suffix << "=zeros("<< nResults-1 << "," << nMethods << "," << noiseTicks << ");" << endl;
256  for(l=0;l<nResults;l++)
257  {
258  if (l!=AverageT)
259  {
260  file << " %% " << resultLabel[l] << " (rows: method columns: noise level)" << endl;
261  file << " error_cpp" << suffix << "("<< (l<AverageT?l+1:l) <<",:,:)=[";
262  for(k=0;k<nMethods;k++)
263  {
264  for(i=0;i<noiseTicks;i++)
265  file << results[k][i][l] << " ";
266  file << endl << " ";
267  }
268  file << "];"<< endl;
269  }
270  }
271  file << " %% Execution time (rows: method columns: noise level)" << endl;
272  file << " time_cpp" << suffix << "=[";
273  for(k=0;k<nMethods;k++)
274  {
275  for(i=0;i<noiseTicks;i++)
276  file << results[k][i][AverageT] << " ";
277  file << endl << " ";
278  }
279  file << "];"<< endl;
280 
281  file.close();
282 }
283 
284