-
Notifications
You must be signed in to change notification settings - Fork 17
/
Copy pathP4pf.cpp
1628 lines (1382 loc) · 74.2 KB
/
P4pf.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
* P4pf.cpp
*
* Created on: Mar 15, 2015
* Author: Marco Zorzi
*/
#include "P4pf.hpp" // this includes the header. All other includes are included there.
#define DBG 0 // debug flag for P4pf function
#define DBG_M 0 // debug flag for P4pf_m function
#define DBG_PWP 0 // debug flag for pointWisePower function
#define DBG_FILL 0 // debug flag for fillMatrix function
#define DBG_P4PFCODE 0 // debug flag for p4pfcode function
#define DBG_TR 0 // debug flag for getRigidTransform2 function
P4pf::P4pf() {
}
P4pf::~P4pf() {
}
/* INLINE FUNCTION FOR CALCULATING THE SIGN OF A NUMBER
this was made in order to manipulate matrices, since it needs to
return ONLY 1, -1 or 0.
INPUT: a double number
OUTPUT: 1 if input number is >0
-1 if input number is <0
0 if input number is =0
*/
double inline P4pf::signOf(double aNumber)
{
if (aNumber > 0)
return 1;
else if (aNumber <0)
return -1;
else
return 0;
}
/* FUNCTION THAT COMPUTES THE 2ND POINTWISE POWER OF A MATRIX OF ANY SIZE
This was not found in the standard library for Dynamic Matrices.
INPUT: a Eigen Matrix of generic size
OUTPUT: the given matrix with its element squared
*/
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> P4pf::pointWisePower(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat)
{ {
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> result = (Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(mat.rows(),mat.cols()));
if (DBG_PWP) std::cout << "mat,rows()" << mat.rows() << "mat cols() " << mat.cols() <<" mat size() " <<mat.size() <<"\n";
for(int i=0; i<mat.rows(); i++)
{
if (DBG_PWP) std::cout << "i= " << i <<"\n";
for(int j=0; j<mat.cols(); j++)
{
if (DBG_PWP) std::cout << "j= " << j <<"\n";
double temp = mat(i,j);
if (DBG_PWP) std::cout << "temp= " << temp <<"\n";
result(i,j)= temp*temp;
if (DBG_PWP) std::cout << "temp*temp= " << temp*temp <<"\n";
}
}
return result;
} }
/*********************************************/
/*********** BEGIN first funciton ************/
/*********************************************/
/* COMMENTS ON THE GIVEN MATLAB CODE
% find rigid transformation (rotation translation) from p1->p2 given
% 3 or more points in the 3D space
%
% based on : K.Arun, T.Huangs, D.Blostein. Least-squares
% fitting of two 3D point sets IEEE PAMI 1987
%
% by Martin Bujnak, nov2007
%
%function [R t] = GetRigidTransform(p1, p2, bLeftHandSystem)
% p1 - 3xN matrix with reference 3D points
% p2 - 3xN matrix with target 3D points
% bLeftHandSystem - camera coordinate system
*/
/*
INPUT: - p1 and p2, two matrices of 3XN size.
- boolean for the camera coordiante system
- a vector of matrices for the output
OUTPUT: - Outputs a vector of Eigen 3x3 matrices with these elements: 1)the first element is the rotation matrix
2) the second element is the translation vector
- the int return is used as error check: if 0 correct, if -1 an error was found
NOTE: In order to increase efficiency it was preferred to fit a 3X1 vector in a 3X3 matrix in order to allow Eigen library
to pre-allocate the memory. This way, the array doesn't need to be of Eigen 3XDynamic size. Nevertheless,
the second element of the vector is just a 3x1 vector and the other elements are zeros.
*/
int P4pf::getRigidTransform2(Eigen::Matrix<double, 3, Eigen::Dynamic> p1,Eigen::Matrix<double, 3, Eigen::Dynamic> p2,bool bLeftHandSystem, std::vector<Eigen::Matrix<double, 3,3>>* solutions)
{
//prints out the element received
if (DBG_TR) std::cout << "p1\n" <<p1 << "\n\n";
if (DBG_TR) std::cout << "p2\n"<< p2 << "\n\n";
//creation of means matrices
Eigen::Matrix<double, 3, 1> p1mean;
Eigen::Matrix<double, 3, 1> p2mean ;
//Means calculations
p1mean = p1.rowwise().sum();
p1mean = p1mean/ 4;
p2mean = p2.rowwise().sum();
p2mean = p2mean/ 4;
// Prints the means after the calculations. Used to verify rowwise sum of Eigen Library
if (DBG_TR) std::cout << "p1mean: \n" << p1mean << "\n\n";
if (DBG_TR) std::cout << "p2mean:\n" << p2mean << "\n\n";
//Replicates the mean in 4 columns and subtracts from the received matrix
p1 = p1 - p1mean.replicate(1, 4);
p2 = p2 - p2mean.replicate(1, 4);
// Print updated matrices to verify replicate function of Eigen Library
if (DBG_TR) std::cout << "p1\n" <<p1 << "\n\n";
if (DBG_TR) std::cout << "p2\n"<< p2 << "\n\n";
//normalize to unit size
// Used very spacy coding for better understanding of the operations order.
// Saved in a temporary matrix to reduce complexity.
Eigen::Matrix<double, 1, Eigen::Dynamic> temp1 =
(
(
(
(
(
this->pointWisePower(p1) // matrix gets squared pointwise
)
).colwise()
).sum() // colwise sum
).cwiseSqrt() // pointwise square root
).cwiseInverse(); // Pointwise inversion
// final operations and then save in u1
Eigen::Matrix<double, 3, Eigen::Dynamic> u1 = p1.cwiseProduct(temp1.replicate(3,1)); //replicate horizontal vector 3 times
//same operations for u2
Eigen::Matrix<double, 1, Eigen::Dynamic> temp2 =
(
(
(
(
(
this->pointWisePower(p2)
)
).colwise()
).sum()
).cwiseSqrt()
).cwiseInverse();
Eigen::Matrix<double, 3, Eigen::Dynamic> u2 = p2.cwiseProduct(temp2.replicate(3,1));
// Prints u1 and u2 after the calculations
if (DBG_TR) std::cout << "u1\n" <<u1 << "\n\n";
if (DBG_TR) std::cout << "u2\n"<< u2 << "\n\n";
// calcolations for rotation
Eigen::Matrix<double, 3, 3> C = u2 * u1.transpose();
// prints C for verifications
if (DBG_TR) std::cout << "C created. Here it is\n" << C << "\n";
// calculations of singular value decomposition using EigenSolver JacboiSVD
// Std outputs used to verify the progression of the process
if (DBG_TR) std::cout << "Ready for SVD\n";
// The two flags "ComputeFullU and ComputeFullV tells the solver to calculate the full squared matrices.
Eigen::JacobiSVD<Eigen::Matrix<double, 3, 3>> svd(C, Eigen::ComputeFullU | Eigen::ComputeFullV);
if (DBG_TR) std::cout << "Made SVD\n";
// Jacobi SVD doesn't have flags for giving the singular values as a matrix.
// this needs to be done
Eigen::Matrix<double, 3, 1> S_diag = svd.singularValues();
Eigen::Matrix<double, 3, 3> S = Eigen::Matrix<double, 3, 3>::Zero(3,3);
// diagonal matrix creation
if (DBG_TR) std::cout << "Got S\n";
for (int i = 0; i < 3; i++)
{
S(i,i) = S_diag(i);
}
// Printed S to verify if it's correct
if (DBG_TR) std::cout << "S\n" << S << "\n\n";
// Calculated U and printed to verify if it's correct
Eigen::Matrix<double, 3, 3> U = svd.matrixU();
if (DBG_TR) std::cout << "U\n" << U << "\n\n";
// Calculated V and printed to verify if it's correct
Eigen::Matrix<double, 3, 3> V = svd.matrixV();
if (DBG_TR) std::cout << "V\n" << V << "\n\n";
// Cast to double is required here because the access to a Matrix element somehow
// returns a matrix subset.
S(0,0) = (double) this->signOf((double) S(0,0));
S(1,1) = (double) this->signOf((double) S(1,1));
if (bLeftHandSystem)
S(2,2) = -(double) this->signOf((double) (U*V.transpose()).determinant());
else
S(2,2) = (double) this->signOf((double) (U*V.transpose()).determinant());
//Check of S
if (DBG_TR) std::cout << "filled S\n";
if (DBG_TR) std::cout << "S\n" << S << "\n\n";
// Final calculations of R and T, than verified by printing them
Eigen::Matrix<double, 3, 3> R = U*S*(V.transpose());
Eigen::Matrix<double, 3, 1> t = -R*p1mean + p2mean;
if (DBG_TR) std::cout << "R and t\n";
if (DBG_TR) std::cout << "R\n" << R << "\n\n";
if (DBG_TR) std::cout << "t\n" << t << "\n\n";
// as per function introduction, here the translation vector (3X1) gets stuffed
// in the first column of a 3x3 matrix and then pushed back on the solution vector
Eigen::Matrix<double, 3, 3> t_out = Eigen::Matrix<double, 3, 3>::Zero(3,3);
for(int i = 0; i<3;i++)
{
t_out(i,0)=t(i);
}
//Verification of the output
if (DBG_TR) std::cout << "T out ready\n";
if (DBG_TR) std::cout << "t_out\n" << t_out << "\n\n";
//solution given back
solutions->push_back(R);
solutions->push_back(t_out);
return 0;
}
/*********************************************/
/************ END first funciton *************/
/*********************************************/
/*********************************************/
/*********** BEGIN second funciton ***********/
/*********************************************/
/* Comments on the original Matlab code
% P4P + unknown focal length
% given a set of 4x 2D<->3D correspondences, calculate camera pose and the
% camera focal length.
%
% by Martin Bujnak, (c)apr2008
%
%
% Please refer to the following paper, when using this code :
%
% Bujnak, M., Kukelova, Z., and Pajdla, T. A general solution to the p4p
% problem for camera with unknown focal length. CVPR 2008, Anchorage,
% Alaska, USA, June 2008
%
%
% function [f R t] = P4Pf_m(m2D, M3D)
%
% input:
%
% m2D - 2x4 matrix with 4x2D measuremets
% M3D - 3x4 matrix with corresponding 3D points
%
% output:
%
% f - vector with N focal lengths
% R - 3x3xN matrix with N rotation matrices
% t - 3xN matrix with N translation matrices
%
% following equation holds for each solution
%
% lambda*m2D = diag([f(i) f(i) 1])*[R(:,:,i) t(:,i)] * M3D
*/
/*
INPUT: - m2D, 2x4 matrix
- M3D, 3x4 matrix
- a vector of double for returning focal lenghts
- a vector of matrices 3x3 for the rotation matrices
- a vector of Eigen vectors 3x1 for the translation vectors
OUTPUT: - The given vectors filled
- the int return is used as error check: if 0 correct, if -1 an error was found
*/
int P4pf::P4Pf_m(Eigen::Matrix<double, 2, 4> m2D, Eigen::Matrix<double, 3, 4> M3D, std::vector<double>* focalLengths_mResults, std::vector<Eigen::Matrix<double, 3,3>>* rotationMatrices, std::vector<Eigen::Matrix<double, 3,1>>* translationResult )
{
//check data arrived
if (DBG_M) std::cout << "\n**************** ENTERING P4PF_M ****************\n";
if (DBG_M) std::cout << "\nThis is M3D\n" << M3D << "\n";
if (DBG_M) std::cout << "\nThis is m2D\n" << m2D << "\n";
// shift 3D data so that variance = sqrt(2), mean = 0
Eigen::Matrix<double, 3, 1> mean3d = M3D.rowwise().sum() /4;
M3D = M3D - mean3d.replicate(1, 4);
// Created this matrix for calculation of var. M3d needs to stay as it is.
Eigen::Matrix<double, 3, 4> M3Dbis = Eigen::Matrix<double, 3, 4>::Zero(3,4);
// Check if all the calculations give appropriate results.
// Several bugs solved here
if (DBG_M) std::cout << "\nThis is M3D\n" << M3D << "\n";
if (DBG_M) std::cout << "\nThis is M3D^2\n" << M3Dbis << "\n";
if (DBG_M) std::cout << "\nThis is M3D colwise \n" << M3Dbis.colwise().sum() << "\n";
if (DBG_M) std::cout << "\nThis is M3D colwise + sqrt \n" << M3Dbis.colwise().sum().cwiseSqrt() << "\n";
if (DBG_M) std::cout << "\nThis is M3D colwise + sqrt + rowise sum\n" << M3Dbis.colwise().sum().cwiseSqrt().rowwise().sum() << "\n";
if (DBG_M) std::cout << "\nThis is M3D colwise + sqrt + rowise sum at 0\n" << M3Dbis.colwise().sum().cwiseSqrt().rowwise().sum()(0) << "\n";
// Calculation of var in several step to reduce errors.
M3Dbis = this->pointWisePower(M3D);
double var = M3Dbis.colwise().sum().cwiseSqrt().rowwise().sum()(0);
var = var/4;
if (DBG_M) std::cout << "\nThis is var = " << var << "\n";
M3D = (1/var)*M3D;
// Check of var and of M3D modified
if (DBG_M) std::cout << "\nThis is var = " << var << "\n";
if (DBG_M) std::cout << "\nThis is M3D final, after it was elaborated with var\n" << M3D << "\n";
// scale 2D data
double var2d = this->pointWisePower(m2D).colwise().sum().cwiseSqrt().rowwise().sum()(0);
var2d = var2d/4;
if (DBG_M) std::cout << "\n var2d extracted from m2D = " << var2d;
m2D = (1/var2d)*m2D;
// Checking of the results
if (DBG_M) std::cout << "\n var2d extracted from m2D = " << var2d <<"\n";
if (DBG_M) std::cout << "\nThis is m2D final, after it was elaborated with var2D\n" << m2D << "\n";
//coefficients of 5 reduced polynomials in these monomials mon
double glab = this->pointWisePower( (M3D.col(0) - M3D.col(1)) ).colwise().sum()(0);
double glac = this->pointWisePower( (M3D.col(0) - M3D.col(2)) ).colwise().sum()(0);
double glad = this->pointWisePower( (M3D.col(0) - M3D.col(3)) ).colwise().sum()(0);
double glbc = this->pointWisePower( (M3D.col(1) - M3D.col(2)) ).colwise().sum()(0);
double glbd = this->pointWisePower( (M3D.col(1) - M3D.col(3)) ).colwise().sum()(0);
double glcd = this->pointWisePower( (M3D.col(2) - M3D.col(3)) ).colwise().sum()(0);
// Consistecy check of the coefficients
if (glbc*glbd*glcd*glab*glac*glad < 1e-15)
{
return -1;
}
if (DBG_M) std::cout << "\n Passed the coefficient check, the are not zero\n";
// Creating array for solutions and calling the function
std::vector<Eigen::Matrix<double, 1, Eigen::Dynamic>> solutions;
if (DBG_M) std::cout << "solutions.size()= " << solutions.size() << "\n";
if (DBG_M) std::cout << "\n**************** LEAVING P4PF_M ****************\n";
int errorCheck= this->P4pf::p4pfcode(glab, glac, glad, glbc, glbd, glcd, (double)m2D(0,0),(double) m2D(1,0),
(double)m2D(0,1),(double) m2D(1,1),(double) m2D(0,2), (double)m2D(1,2),
(double) m2D(0,3),(double) m2D(1,3), &solutions);
if (DBG_M) std::cout << "\n**************** RE-ENTERED P4PF_M after P4PFCODE ****************\n";
//Checking if there are errors
if (DBG_M) std::cout << "\n called p4pfcode. errorCheck = " << errorCheck <<".\n";
if (errorCheck == -1)
{
return -1;
}
// Check of solution consistency
if (DBG_M) std::cout << "solutions.size()= " << solutions.size() << "\n";
// Unpacking of the solutions
// This was tricky because in matlab they are created in an opposite way.
Eigen::Matrix<double, 1, Eigen::Dynamic> focalLengths_m = solutions.at(0);
Eigen::Matrix<double, 1, Eigen::Dynamic> zb = solutions.at(1);
Eigen::Matrix<double, 1, Eigen::Dynamic> zc = solutions.at(2);
Eigen::Matrix<double, 1, Eigen::Dynamic> zd = solutions.at(3);
// Check of the solutions
if (DBG_M) std::cout << "This is focalLenghts:" << focalLengths_m << "\n";
if (DBG_M) std::cout << "This is zb:" << zb << "\n";
if (DBG_M) std::cout << "This is zc:" << zc << "\n";
if (DBG_M) std::cout << "This is zd:" << zd << "\n";
// number of focal lenghts possible found with check
double lcnt = focalLengths_m.size();
if (DBG_M) std::cout << "\nlcnt = " << lcnt <<".\n";
//Vector for the translation vectors to be used in every loop
std::vector<Eigen::Matrix<double, 3, 1>> translationVectors;
// now we loop and go through every possible solution
for (int i=0; i<lcnt; i++)//i=1:lcnt
{
// print the loop cycle
if (DBG_M) std::cout << "i=" << i <<"\n";
// create p3d points in a camera coordinate system (using depths)
Eigen::Matrix<double,3,4> p3dc = Eigen::Matrix<double,3,4>::Zero(3,4);
Eigen::Matrix<double,3,1> temp = Eigen::Matrix<double,3,1>::Zero(3,1);
// Check current points
if (DBG_M) std::cout << "This is zb(i)= "<< zb(i) <<"\n\n";
if (DBG_M) std::cout << "This is zc(i)= "<< zc(i) <<"\n\n";
if (DBG_M) std::cout << "This is zd(i)= "<< zd(i) <<"\n\n";
if (DBG_M) std::cout << "This is focalLengths_m(i)= "<< focalLengths_m(i) <<"\n\n";
if (DBG_M) std::cout << "This is m2d\n"<< m2D <<"\n\n";
// p3dc gets filled and verified at each step
temp.col(0) << m2D.col(0),
focalLengths_m(i);
p3dc.col(0) = 1* temp;
if (DBG_M) std::cout << "This is p3dc 0\n"<<p3dc <<"\n\n";
temp.col(0) << m2D.col(1),
focalLengths_m(i);
p3dc.col(1) = zb(i)* temp;
if (DBG_M) std::cout << "This is p3dc 1 \n"<<p3dc <<"\n\n";
temp.col(0) << m2D.col(2),
focalLengths_m(i);
p3dc.col(2) = zc(i)* temp;
if (DBG_M) std::cout << "This is p3dc 2 \n"<<p3dc <<"\n\n";
temp.col(0) << m2D.col(3),
focalLengths_m(i);
p3dc.col(3) = zd(i)* temp;
if (DBG_M) std::cout << "This is p3dc 3 \n"<<p3dc <<"\n\n";
// d matrix prepared and then calculated
Eigen::Matrix<double,6,1> d_matrix = Eigen::Matrix<double,6,1>::Zero(6,1);
// fix scale (recover 'za')
double temp_double = 0;
temp_double = this->pointWisePower( (p3dc.col(0) - p3dc.col(1)) ).colwise().sum()(0);
d_matrix(0) = sqrt(glab / temp_double);
temp_double = this->pointWisePower( (p3dc.col(0) - p3dc.col(2)) ).colwise().sum()(0);
d_matrix(1) = sqrt(glac / temp_double);
temp_double = this->pointWisePower( (p3dc.col(0) - p3dc.col(3)) ).colwise().sum()(0);
d_matrix(2) = sqrt(glad / temp_double);
temp_double = this->pointWisePower( (p3dc.col(1) - p3dc.col(2)) ).colwise().sum()(0);
d_matrix(3) = sqrt(glbc / temp_double);
temp_double = this->pointWisePower( (p3dc.col(1) - p3dc.col(3)) ).colwise().sum()(0);
d_matrix(4) = sqrt(glbd / temp_double);
temp_double = this->pointWisePower( (p3dc.col(2) - p3dc.col(3)) ).colwise().sum()(0);
d_matrix(5) = sqrt(glcd / temp_double);
// d matrix checked
if (DBG_M) std::cout << "This is d:\n"<< d_matrix<<"\n\n";
//Check of d matrix
double gta = d_matrix.colwise().sum()(0) / 6;
if (DBG_M) std::cout << "This is gta= "<< gta<<"\n";
p3dc = gta * p3dc;
if (DBG_M) std::cout << "This is p3dc FINAL\n\n"<< p3dc <<"\n\n";
//Vector for solution output
std::vector<Eigen::Matrix<double, 3,3>> rigidTrasform_results;
// function is called and checked
if (DBG_M) std::cout << "\n********************************************** BEGIN GETRIGIDTRANSFORM2**********************************************\n";
int errorCheck = getRigidTransform2(M3D, p3dc, false, &rigidTrasform_results);
if(errorCheck == -1)
{
return -1;
}
if (DBG_M) std::cout << "\n********************************************** END GETRIGIDTRANSFORM2 **********************************************\n";
if (DBG_M) std::cout << "errorCheck is= "<< errorCheck << "\n\n";
// Create temporary matrices to stuff them into the result vector
Eigen::Matrix<double,3,3> Rr;
Eigen::Matrix<double,3,3> tt_big;
Eigen::Matrix<double,3,1> tt;
Rr = rigidTrasform_results.at(0);
tt_big = rigidTrasform_results.at(1);
// Unpacking of translation vector
for (int i=0; i<3;i++)
{
tt(i) = tt_big(i,0);
}
if (DBG_M) std::cout << "RR is:\n"<< Rr << "\n\n";
if (DBG_M) std::cout << "tt is:\n"<< tt << "\n\n";
if (DBG_M) std::cout << "mean3d is:\n"<< mean3d << "\n\n";
if (DBG_M) std::cout << "var is:\n"<< var << "\n\n";
if (DBG_M) std::cout << "var*tt:\n"<< var*tt << "\n\n";
if (DBG_M) std::cout << "Rr*mean3d:\n"<< Rr*mean3d << "\n\n";
// Pushing out solutions
rotationMatrices->push_back(Rr);
temp = var*tt-Rr*mean3d;
translationResult->push_back(temp);
double focalToReturn = var2d*focalLengths_m(i);
focalLengths_mResults->push_back(focalToReturn);
// Print the solutions before sending them back
if (DBG_M) std::cout << "focalToReturn:\n"<< focalToReturn << "\n";
if (DBG_M) std::cout << "RR is:\n"<< Rr << "\n\n";
if (DBG_M) std::cout << "tt is:\n"<< tt << "\n\n";
}
// Saying we are returning
if (DBG_M) std::cout << "\n**************** END OF P4PF_M ****************\n";
return 0;
}
/*********************************************/
/************ END second funciton ************/
/*********************************************/
/*********************************************/
/*********** BEGIN third funciton ************/
/*********************************************/
/* COMMENTS FROM THE MATLAB CODE
% P4P + unknown focal length
% given a set of 4x 2D<->3D correspondences, calculate camera pose and the
% camera focal length.
%
% by Martin Bujnak, (c)apr2008
%
%
% Please refer to the following paper, when using this code :
%
% Bujnak, M., Kukelova, Z., and Pajdla, T. A general solution to the p4p
% problem for camera with unknown focal length. CVPR 2008, Anchorage,
% Alaska, USA, June 2008
%
%
% function [f R t] = P4Pf(m2D, M3D)
%
% input:
%
% m2D - 2x4 matrix with 4x2D measuremets
% M3D - 3x4 matrix with corresponding 3D points
%
% output:
%
% f - vector with N focal lengths
% R - 3x3xN matrix with N rotation matrices
% t - 3xN matrix with N translation matrices
%
% following equation holds for each solution
%
% lambda*m2D = diag([f(i) f(i) 1])*[R(:,:,i) t(:,i)] * M3D
*/
/*
INPUT: - m2D, 2x4 matrix
- M3D, 3x4 matrix
- a vector of double for returning focal lenghts
- a vector of matrices 3x3 for the rotation matrices
- a vector of Eigen vectors 3x1 for the translation vectors
OUTPUT: - The given vectors filled
- the int return is used as error check: if 0 correct, if -1 an error was found
NOTE: This is similar to P4pf_M but with improved efficiency.
There are comments only on parts which differs from previous code.
*/
int P4pf::P4Pf(Eigen::Matrix<double, 2, 4> m2D, Eigen::Matrix<double, 3, 4> M3D, std::vector<double>* focalLengthsResults, std::vector<Eigen::Matrix<double, 3,3>>* rotationMatrices, std::vector<Eigen::Matrix<double, 3,1>>* translationResult )
{
/* SAME CODE OF P4PF_M*/
if (DBG) std::cout << "\n**************** ENTERING P4PF ****************\n";
if (DBG) std::cout << "this is M3D:\n" << M3D << "\n\n";
if (DBG) std::cout << "this is M3D.transpose():\n" << M3D.transpose() << "\n\n"; //matrix trasposed but it's the same as before
if (DBG) std::cout << "this is M3D.transpose().colwise().sum():\n" << M3D.transpose().colwise().sum() << "\n\n";
Eigen::Matrix<double, 1, 3> mean3d = M3D.transpose().colwise().sum() /4;
if (DBG) std::cout << "this is mean3d:\n" << mean3d << "\n\n";
if (DBG) std::cout << "this is mean3d.replicate(1, 4):\n" << mean3d.replicate(1, 4) << "\n\n";
if (DBG) std::cout << "\nmean3d created\n";
if (DBG) std::cout << "this is mean3d:\n" << mean3d << "\n\n";
if (DBG) std::cout << "this is mean3d.transpose():\n" << mean3d.transpose() << "\n\n";
M3D = M3D - mean3d.transpose().replicate(1, 4);
Eigen::Matrix<double, 3, 4> M3Dbis = Eigen::Matrix<double, 3, 4>::Zero(3,4);
M3Dbis = this->pointWisePower(M3D);
if (DBG) std::cout << "\nThis is M3D\n" << M3D << "\n";
if (DBG) std::cout << "\nThis is M3D^2\n" << M3Dbis << "\n";
if (DBG) std::cout << "\nThis is M3D colwise \n" << M3Dbis.colwise().sum() << "\n";
if (DBG) std::cout << "\nThis is M3D colwise + sqrt \n" << M3Dbis.colwise().sum().cwiseSqrt() << "\n";
if (DBG) std::cout << "\nThis is M3D colwise + sqrt + rowise sum\n" << M3Dbis.colwise().sum().cwiseSqrt().rowwise().sum() << "\n";
if (DBG) std::cout << "\nThis is M3D colwise + sqrt + rowise sum at 0\n" << M3Dbis.colwise().sum().cwiseSqrt().rowwise().sum()(0) << "\n";
double var = M3Dbis.colwise().sum().cwiseSqrt().rowwise().sum()(0);
var = var/4;
M3D = (1/var)*M3D;
if (DBG) std::cout << "\nThis is var = " << var << "\n";
if (DBG) std::cout << "\nThis is M3D final, after it was elaborated with var\n" << M3D << "\n";
// scale 2D data
double var2d = this->pointWisePower(m2D).colwise().sum().cwiseSqrt().rowwise().sum()(0);
var2d = var2d/4;
m2D = (1/var2d)*m2D;
if (DBG) std::cout << "\n var2d extracted from m2D = " << var2d;
if (DBG) std::cout << "\nThis is m2D final, after it was elaborated with var2D\n" << m2D << "\n";
//coefficients of 5 reduced polynomials in these monomials mon
double glab = this->pointWisePower( (M3D.col(0) - M3D.col(1)) ).colwise().sum()(0);
double glac = this->pointWisePower( (M3D.col(0) - M3D.col(2)) ).colwise().sum()(0);
double glad = this->pointWisePower( (M3D.col(0) - M3D.col(3)) ).colwise().sum()(0);
double glbc = this->pointWisePower( (M3D.col(1) - M3D.col(2)) ).colwise().sum()(0);
double glbd = this->pointWisePower( (M3D.col(1) - M3D.col(3)) ).colwise().sum()(0);
double glcd = this->pointWisePower( (M3D.col(2) - M3D.col(3)) ).colwise().sum()(0);
double tol = 2.2204e-10;
if (glbc*glbd*glcd*glab*glac*glad < tol)
{
return -1;
}
if (DBG) std::cout << "\n Passed the coefficient check, no zeros here\n";
// Here the code icludes the C file from the paper.
// Data needs to be prepared accordingly
// Creation of a array of gl coefficients
double gl_coef[6];
gl_coef[0] = glab;
gl_coef[1] = glac;
gl_coef[2] = glad;
gl_coef[3] = glbc;
gl_coef[4] = glbd;
gl_coef[5] = glcd;
//other arrays of coefficients
double a_coef[] = {(double) m2D(0,0),(double) m2D(1,0)};
double b_coef[] = {(double) m2D(0,1),(double) m2D(1,1)};
double c_coef[] = {(double) m2D(0,2), (double)m2D(1,2)};
double d_coef[] = {(double) m2D(0,3),(double) m2D(1,3)};
//Code of p4pfcode is used so we prepare matrices and solutions before.
Eigen::Matrix<std::complex<double>, 10, 10> A = (Eigen::Matrix<std::complex<double>, 10, 10>::Zero(10,10));
std::vector<Eigen::Matrix<double, 1, Eigen::Dynamic>> solutions;
if (DBG_M) std::cout << "solutions.size()= " << solutions.size() << "\n";
//additional code is called and checked.
this->P4pf::p4pfmex(gl_coef, a_coef, b_coef, c_coef, d_coef, &A);
if (DBG) std::cout << "This is A\n\n" << A << "\n\n";
//matrix comes out transposed, so we traspose it back and use the code of p4pfcode
Eigen::Matrix<std::complex<double>, 10, 10> temporaryA;
temporaryA = A.transpose();
A = temporaryA;
// result check.
if (DBG) std::cout << "This is A transposed\n\n" << A << "\n\n";
/****************************** BEGIN CODE OF P4PCODE FUNCTION ************************/
Eigen::Matrix<std::complex<double>, 1, 10> D_diag;
Eigen::Matrix<std::complex<double>, 10, 10> D = (Eigen::Matrix<std::complex<double>, 10, 10>::Zero(10,10));
Eigen::Matrix<std::complex<double>, 10, 10> V;
Eigen::Matrix<std::complex<double>, 4, 10> sol;
if (DBG) std::cout << "Ready for eigensolver\n";
Eigen::ComplexEigenSolver<Eigen::Matrix<std::complex<double>, 10, 10>> es(A);
D_diag =es.eigenvalues();
if (DBG) std::cout << "This is D_diag\n\n" << D_diag << "\n\n";
for (int i = 0; i < 10; i++)
{
D(i,i) = D_diag(i);
}
if (DBG) std::cout << "This is D\n\n" << D << "\n\n";
V = es.eigenvectors();
if (DBG) std::cout << "This is V\n" << V<< "\n\n";
if (DBG) std::cout << "V.rows()" << V.rows() << "V.cols() " << V.cols() <<" V.size() " <<V.size() <<"\n";
for(int i = 1; i<5; i++)
{
for(int j=0; j<10; j++)
{
sol(i-1,j) = V(i,j)/V(0,j);
if (!(sol.real()(i-1,j) >= 0 || sol.real()(i-1,j) <0))
{
return -1;
}
}
}
std::vector<int> indexes;
if (DBG) std::cout << "Now Entering the index search loop\n";
for(int j = 0; j<10; j++)
{
if (DBG) std::cout << ". j = " << j <<"\n";
if (DBG) std::cout << "real = " << sol.real()(3,j) <<"\n";
if (DBG) std::cout << "imag = " << sol.imag()(3,j) <<"\n";
if (DBG) std::cout << "imaginary check = " << (sol.imag()(3,j)<0.00001 && sol.imag()(3,j) > -0.00001) <<"\n";
if ((sol.imag()(3,j)<0.00001 && sol.imag()(3,j) > -0.00001) ) //solution is non imaginary
{
if (DBG) std::cout << "positive check = " << (sol.real()(3,j)>0.00001) <<"\n";
if (sol.real()(3,j)>0.00001)
{
if (DBG) std::cout << "I got this\n";
indexes.push_back(j);
}
}
}
if (DBG) std::cout << "This is sol\n" << sol<< "\n";
if (DBG) std::cout << "Found focal lengths, now saving solutions\n";
Eigen::Matrix<double, 1, Eigen::Dynamic> f;
Eigen::Matrix<double, 1, Eigen::Dynamic> zd;
Eigen::Matrix<double, 1, Eigen::Dynamic> zc;
Eigen::Matrix<double, 1, Eigen::Dynamic> zb;
f.resize(1,indexes.size());
zd.resize(1,indexes.size());
zc.resize(1,indexes.size());
zb.resize(1,indexes.size());
if (DBG) std::cout << "sol.rows() = " << sol.rows() << ". sol.cols() = " << sol.cols() <<". sol.size() = " <<sol.size() <<".\n";
if (DBG) std::cout << "indexes.size() = " << indexes.size() << "\n\n";
//go to positions marked and take results
for (int i = 0; i < indexes.size(); i++)
{
if (DBG) std::cout << "i = " << i<< "\n\n";
f(i) = sqrt(sol.real()(3, indexes.at(i)));
zd(i) = sol.real()(0, indexes.at(i));
zc(i) = sol.real()(1, indexes.at(i));
zb(i) = sol.real()(2, indexes.at(i));
if (DBG) std::cout << "f(" <<i<< ")= " << f(i) <<"\n";
if (DBG) std::cout << "zd(" <<i<< ")= " << zd(i) <<"\n";
if (DBG) std::cout << "zc(" <<i<< ")= " << zc(i) <<"\n";
if (DBG) std::cout << "zb(" <<i<< ")= " << zb(i) <<"\n";
}
if (DBG) std::cout << "Done. Now Onto the next part\n";
/****************************** END CODE OF P4PCODE FUNCTION ************************/
// compatibility with previous code, we change name
Eigen::Matrix<double, 1, Eigen::Dynamic> focalLengths = f;
if (DBG) std::cout << "This is focalLenghts:\n" << focalLengths <<"\n";
double lcnt = focalLengths.size();
std::vector<Eigen::Matrix<double, 3, 1>> translationVectors;
if (DBG) std::cout << "\nlcnt = " << lcnt <<".\n";
for (int i=0; i<lcnt; i++)
{
if (DBG) std::cout << "i=" << i <<"\n";
Eigen::Matrix<double,3,4> p3dc = Eigen::Matrix<double,3,4>::Zero(3,4);
Eigen::Matrix<double,3,1> temp = Eigen::Matrix<double,3,1>::Zero(3,1);
if (DBG) std::cout << "This is zb(i)= "<< zb(i) <<"\n\n";
if (DBG) std::cout << "This is zc(i)= "<< zc(i) <<"\n\n";
if (DBG) std::cout << "This is zd(i)= "<< zd(i) <<"\n\n";
if (DBG) std::cout << "This is focalLengths(i)= "<< focalLengths(i) <<"\n\n";
if (DBG) std::cout << "This is m2d\n"<< m2D <<"\n\n";
temp.col(0) << m2D.col(0),
focalLengths(i);
p3dc.col(0) = 1* temp;
if (DBG) std::cout << "This is p3dc 0\n"<<p3dc <<"\n\n";
temp.col(0) << m2D.col(1),
focalLengths(i);
p3dc.col(1) = zb(i)* temp;
if (DBG) std::cout << "This is p3dc 1 \n"<<p3dc <<"\n\n";
temp.col(0) << m2D.col(2),
focalLengths(i);
p3dc.col(2) = zc(i)* temp;
if (DBG) std::cout << "This is p3dc 2 \n"<<p3dc <<"\n\n";
temp.col(0) << m2D.col(3),
focalLengths(i);
p3dc.col(3) = zd(i)* temp;
if (DBG) std::cout << "This is p3dc 3 \n"<<p3dc <<"\n\n";
if (DBG) std::cout << "temp and p3dc full\n\n";
Eigen::Matrix<double,6,1> d_matrix = Eigen::Matrix<double,6,1>::Zero(6,1);
if (DBG) std::cout << "This is temp\n"<< temp <<"\n\n";
double temp_double = 0;
temp_double = this->pointWisePower( (p3dc.col(0) - p3dc.col(1)) ).colwise().sum()(0);
d_matrix(0) = sqrt(glab / temp_double);
temp_double = this->pointWisePower( (p3dc.col(0) - p3dc.col(2)) ).colwise().sum()(0);
d_matrix(1) = sqrt(glac / temp_double);
temp_double = this->pointWisePower( (p3dc.col(0) - p3dc.col(3)) ).colwise().sum()(0);
d_matrix(2) = sqrt(glad / temp_double);
temp_double = this->pointWisePower( (p3dc.col(1) - p3dc.col(2)) ).colwise().sum()(0);
d_matrix(3) = sqrt(glbc / temp_double);
temp_double = this->pointWisePower( (p3dc.col(1) - p3dc.col(3)) ).colwise().sum()(0);
d_matrix(4) = sqrt(glbd / temp_double);
temp_double = this->pointWisePower( (p3dc.col(2) - p3dc.col(3)) ).colwise().sum()(0);
d_matrix(5) = sqrt(glcd / temp_double);
double gta = d_matrix.colwise().sum()(0) / 6;
p3dc = gta * p3dc;
if (DBG) std::cout << "This is d:\n"<< d_matrix<<"\n\n";
if (DBG) std::cout << "This is gta= "<< gta<<"\n";
if (DBG) std::cout << "This is p3dc FINAL\n\n"<< p3dc <<"\n\n";
std::vector<Eigen::Matrix<double, 3,3>> rigidTrasform_results;
if (DBG) std::cout << "\n********************************************** BEGIN GETRIGIDTRANSFORM2**********************************************\n";
int errorCheck = getRigidTransform2(M3D, p3dc, false, &rigidTrasform_results);
if(errorCheck == -1)
{
return -1;
}
if (DBG) std::cout << "\n********************************************** END GETRIGIDTRANSFORM2 **********************************************\n";
if (DBG) std::cout << "errorCheck is= "<< errorCheck << "\n\n";
Eigen::Matrix<double,3,3> Rr;
Eigen::Matrix<double,3,3> tt_big;
Eigen::Matrix<double,3,1> tt;
Rr = rigidTrasform_results.at(0);
tt_big = rigidTrasform_results.at(1);
for (int i=0; i<3;i++)
{
tt(i) = tt_big(i,0);
}
if (DBG) std::cout << "RR is:\n"<< Rr << "\n\n";
if (DBG) std::cout << "tt is:\n"<< tt << "\n\n";
if (DBG) std::cout << "mean3d is:\n"<< mean3d << "\n\n";
if (DBG) std::cout << "var is:\n"<< var << "\n\n";
if (DBG) std::cout << "var*tt:\n"<< var*tt << "\n\n";
if (DBG) std::cout << "Rr*mean3d:\n"<< Rr*(mean3d.transpose()) << "\n\n";
rotationMatrices->push_back(Rr);
temp = var*tt-Rr*(mean3d.transpose());
translationResult->push_back(temp);
double focalToReturn = var2d*focalLengths(i);
focalLengthsResults->push_back(focalToReturn);
if (DBG) std::cout << "focalToReturn:\n"<< focalToReturn << "\n";
if (DBG) std::cout << "RR is:\n"<< Rr << "\n\n";
if (DBG) std::cout << "tt is:\n"<< tt << "\n\n";
}
if (DBG) std::cout << "\n**************** END OF P4PF_M ****************\n";
return 0;
}
/*********************************************/
/************* END third funciton ************/
/*********************************************/
/* FUNCTION THAT FILLS A GIVEN MATRIX IN PARTICULAR SPARSE COEFFICIENTS
INPUT: - the 78x88 matrix to fill, as a pointer
- the array of coefficients
- the value to store inside the matrix cells
- the size of the array passed
OUTPUT: 0 when done
*/
int P4pf::fillMatrix(Eigen::Matrix<double,78,88> *matrix, int *arr, double value, int size)
{
// check which size was received
if (DBG_FILL) std::cout << "size received = " << size << "\n";
for ( int i = 0; i < size; i++)
{
// analyse every cycle
if (DBG_FILL) std::cout << "i = " << i << "\n";
if (DBG_FILL) std::cout << "arr[i] = " << arr[i] << "\n";
if (DBG_FILL) std::cout << "value = " << value << "\n";
//column and rows calculated since indexes inside the arrays are in matlab format
// so they start from 1.
int col = (arr[i]-1) / 78;
int row = (arr[i] -1 ) % 78;
(*matrix)(row,col)= value;
// Double check if it was inserted correctly, you never know
if (DBG_FILL) std::cout << "Matrix value inserted = " << (*matrix)(row,col) << "\n\n";
}
return 0;
}
/*********************************************/
/*********** BEGIN fourth funciton ************/
/*********************************************/
/* MATLAB COMMENTS
% P4P + unknown focal length helper
% given a set of 4x 2D<->3D correspondences, calculate focal length and 3 unknown depths
%
% by Martin Bujnak, (c)june2011
%
%
% Please refer to the following paper, when using this code :
%
% Bujnak, M., Kukelova, Z., and Pajdla, T. A general solution to the p4p
% problem for camera with unknown focal length. CVPR 2008, Anchorage,
% Alaska, USA, June 2008
%
% [f zb zc zd] = p4pfcode(glab, glac, glad, glbc, glbd, glcd, a1, a2, b1, b2, c1, c2, d1, d2)
%
% input:
% glXY - ||X-Y||^2 - quadratic distances between 3D points X and Y
% a1 (a2) = x (resp y) measurement of the first 2D point
% b1 (b2) = x (resp y) measurement of the second 2D point
% c1 (c2) = x (resp y) measurement of the third 2D point
% d1 (d2) = x (resp y) measurement of the fourth 2D point
%
% output:
% f - estimated focal lengths
% zb, zc, zd - depths of points b, c, d (depth of 'a' is fixed to 1)
%
*/
/* FUNCTION THAT FILLS A GIVEN MATRIX IN PARTICULAR SPARSE COEFFICIENTS
INPUT: - values needed for the calculations
- vector for the solutions
OUTPUT: - 0 if executed correctly
- -1 if errors
*/
int P4pf::p4pfcode(double glab, double glac, double glad,
double glbc, double glbd, double glcd,
double a1, double a2, double b1, double b2,
double c1, double c2, double d1, double d2, std::vector<Eigen::Matrix<double, 1, Eigen::Dynamic>>* solutions)
{
//Check if variables were received correctly
if (DBG_P4PFCODE) std::cout << "\n**************** ENTERED P4PFCODE ****************\n";
if (DBG_P4PFCODE) std::cout << "\n**************** PARAMETERS:\n";
if (DBG_P4PFCODE) std::cout << "glab = " << glab << "\n";
if (DBG_P4PFCODE) std::cout << "glac = " << glac << "\n";
if (DBG_P4PFCODE) std::cout << "glad = " << glad << "\n";
if (DBG_P4PFCODE) std::cout << "glbc = " << glbc << "\n";
if (DBG_P4PFCODE) std::cout << "glbd = " << glbd << "\n";
if (DBG_P4PFCODE) std::cout << "glcd = " << glcd << "\n";
if (DBG_P4PFCODE) std::cout << "a1 = " << a1 << "\n";
if (DBG_P4PFCODE) std::cout << "a2 = " << a2 << "\n";
if (DBG_P4PFCODE) std::cout << "b1 = " << b1 << "\n";
if (DBG_P4PFCODE) std::cout << "b2 = " << b2 << "\n";
if (DBG_P4PFCODE) std::cout << "c1 = " << c1 << "\n";
if (DBG_P4PFCODE) std::cout << "c2 = " << c2 << "\n";
if (DBG_P4PFCODE) std::cout << "d1 = " << d1 << "\n";
if (DBG_P4PFCODE) std::cout << "d2 = " << d2 << "\n";
if (DBG_P4PFCODE) std::cout << "\n**************** NOW THE CALCULATIONS ****************\n";
// precalculate polynomial equations coefficients
Eigen::Matrix<double, 78,88> M = Eigen::Matrix<double,78,88>::Zero(78,88);
if (DBG_P4PFCODE) std::cout << "Start filling matrix\n";
//The matrix gets filled
int id_01[] = {72, 149, 520, 597, 752, 829, 1062, 1217, 1528, 1895, 2050, 2127, 2360, 2515, 2904, 3439, 3594, 3983, 4993}; //size 19
fillMatrix(&M, id_01, 1, sizeof(id_01)/sizeof(id_01[0]));
int id_02[] = {384, 461, 988, 1299, 1454, 1609, 1686, 1841, 2830, 2985, 3140, 3295, 3372, 4219, 4374, 4607, 5539}; //17
fillMatrix(&M, id_02,0.5/glad*glbc-0.5*glab/glad-0.5*glac/glad, sizeof(id_02 )/sizeof(id_01[0]));
int id_03[] = {618, 929, 1924, 2235, 2390, 2545, 2778, 2933, 3244, 3454, 3609, 3842, 3997, 4308, 4843, 4998, 5231, 5851 }; //18
fillMatrix(&M, id_03,-1, sizeof(id_03 )/sizeof(id_01[0]));
int id_04[] = {696, 1007, 2002, 2313, 2468, 2623, 2856, 3011, 3322, 3532, 3687, 3920, 4075, 4386, 4921, 5076, 5309, 5929 };//18
fillMatrix(&M, id_04, -1, sizeof(id_04 )/sizeof(id_01[0]));
int id_05[] = {774, 1085, 2080, 2391, 2546, 2701, 2934, 3089, 3455, 3610, 3765, 3998, 4153, 4464, 4999, 5154, 5387, 6007 }; //18
fillMatrix(&M, id_05,c2*b2+c1*b1, sizeof(id_05 )/sizeof(id_01[0]));
int id_06[] = {1008, 1319, 2314, 2858, 3013, 3168, 3323, 3400, 3922, 4077, 4232, 4387, 4620, 5310, 5543, 6163 }; //18
fillMatrix(&M, id_06, glac/glad-1/glad*glbc+glab/glad, sizeof(id_06 )/sizeof(id_01[0]));
int id_07[] = {1476, 1709, 2860, 3171, 3326, 3402, 4235, 4390, 4545, 4622, 4777, 5545, 5700, 5777, 6397 }; //15
fillMatrix(&M, id_07, 0.5/glad*glbc*pow(d2,2)-0.5*glab/glad*pow(d2,2)-0.5*glac/glad*pow(d2,2)-0.5*glac/glad*pow(d1,2)+0.5/glad*glbc*pow(d1,2)-0.5*glab/glad*pow(d1,2), sizeof(id_07 )/sizeof(id_01[0]));
int id_08[] = {2334, 3950, 4105, 4260, 4415, 4648, 4936, 5091, 5323, 5556, 5934, 6167, 6475}; //13
fillMatrix(&M, id_08, 1-0.5*glac/glad-0.5*glab/glad+0.5/glad*glbc, sizeof(id_08 )/sizeof(id_01[0]));
int id_09[] = {2412, 2801, 3484, 3873, 4028, 4183, 4338, 4493, 4726, 4859, 5014, 5169, 5246, 5401, 5634, 5857, 6012, 6245, 6553 }; //19
fillMatrix(&M, id_09, -b1*a1-a2*b2, sizeof(id_09 )/sizeof(id_01[0]));
int id_10[] = {2490, 2879, 3562, 3951, 4106, 4416, 4571, 4804, 4937, 5092, 5324, 5479, 5712, 5935, 6090, 6323, 6631 }; //17
fillMatrix(&M, id_10, -c2*a2-c1*a1, sizeof(id_10 )/sizeof(id_01[0]));
int id_11[] = {2880, 3191, 3952, 4263, 4418, 4573, 4650, 4805, 5326, 5481, 5558, 5713, 5790, 6169, 6324, 6401, 6709 }; //17
fillMatrix(&M, id_11, -a1/glad*glbc*d1+a1*glac/glad*d1+glac/glad*a2*d2+a1*glab/glad*d1-1/glad*glbc*a2*d2+glab/glad*a2*d2, sizeof(id_11 )/sizeof(id_01[0]));
int id_12[] = {3972, 4283, 4966, 5354, 5509, 5586, 5741, 5818, 5950, 6105, 6182, 6337, 6414, 6481, 6636, 6713, 6787 }; //17
fillMatrix(&M, id_12, pow(a2,2)+pow(a1,2)-0.5*glac/glad*pow(a2,2)-0.5*pow(a1,2)*glac/glad+0.5/glad*glbc*pow(a2,2)-0.5*pow(a1,2)*glab/glad+0.5*pow(a1,2)/glad*glbc-0.5*glab/glad*pow(a2,2), sizeof(id_12 )/sizeof(id_01[0]));
int id_13[] = {74, 229, 527, 682, 759, 836, 1147, 1224, 1613, 1980, 2057, 2134, 2445, 2522, 2599, 2988, 3520, 3597, 4064, 5072 }; //20
fillMatrix(&M, id_13, 1, sizeof(id_13 )/sizeof(id_01[0]));
int id_14[] = {308, 463, 917, 1306, 1383, 1538, 1693, 1770, 2759, 2914, 3147, 3224, 3301, 3378, 4222, 4299, 4610, 5540 }; //18
fillMatrix(&M, id_14, -glac/glad, sizeof(id_14 )/sizeof(id_01[0]));
int id_15[] = {620, 1009, 1931, 2320, 2397, 2552, 2863, 2940, 3329, 3461, 3616, 3927, 4004, 4081, 4392, 4924, 5001, 5312, 5930 };//19
fillMatrix(&M, id_15, -2, sizeof(id_15 )/sizeof(id_01[0]));
int id_16[] = {776, 1165, 2087, 2476, 2553, 2708, 3019, 3096, 3540, 3617, 3772, 4083, 4160, 4548, 5080, 5157, 5468, 6086 }; //18
fillMatrix(&M, id_16, pow(c1,2)+pow(c2,2), sizeof(id_16 )/sizeof(id_01[0]));
int id_17[] = {932, 1321, 2243, 2787, 2942, 3175, 3252, 3407, 3851, 4006, 4239, 4316, 4393, 4626, 5235, 5546, 6164 }; //17
fillMatrix(&M, id_17, 2*glac/glad, sizeof(id_17 )/sizeof(id_01[0]));
int id_18[] = {1400, 1711, 2789, 3178, 3255, 3409, 4242, 4319, 4474, 4629, 4706, 4783, 5548, 5625, 5780, 6398 };
fillMatrix(&M, id_18, -glac/glad*pow(d1,2)-glac/glad*pow(d2,2), sizeof(id_18 )/sizeof(id_01[0]));
int id_19[] = {2258, 3879, 4034, 4267, 4344, 4655, 4865, 5020, 5252, 5329, 5562, 5859, 6170, 6476 };
fillMatrix(&M, id_19,-glac/glad+1, sizeof(id_19 )/sizeof(id_01[0]));
int id_20[] = {2414, 2881, 3491, 3958, 4035, 4190, 4423, 4500, 4811, 4944, 5021, 5176, 5331, 5408, 5485, 5718, 5938, 6015, 6326, 6632 };
fillMatrix(&M, id_20,-2*c2*a2-2*c1*a1, sizeof(id_20 )/sizeof(id_01[0]));
int id_21[] = {2804, 3193, 3881, 4270, 4347, 4502, 4657, 4734, 5255, 5410, 5565, 5642, 5719, 5796, 6172, 6249, 6404, 6710 };
fillMatrix(&M, id_21,2*a1*glac/glad*d1+2*glac/glad*a2*d2, sizeof(id_21)/sizeof(id_01[0]));