I have this ground-truth trajectory, which is an array with 30 homogeneous coordinates transformation matrix that are 30 positions of a car.
[[[ 1.000000e+00 9.043680e-12 2.326809e-11 5.551115e-17]
[ 9.043683e-12 1.000000e+00 2.392370e-10 3.330669e-16]
[ 2.326810e-11 2.392370e-10 9.999999e-01 -4.440892e-16]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.999978e-01 5.272628e-04 -2.066935e-03 -4.690294e-02]
[-5.296506e-04 9.999992e-01 -1.154865e-03 -2.839928e-02]
[ 2.066324e-03 1.155958e-03 9.999971e-01 8.586941e-01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.999910e-01 1.048972e-03 -4.131348e-03 -9.374345e-02]
[-1.058514e-03 9.999968e-01 -2.308104e-03 -5.676064e-02]
[ 4.128913e-03 2.312456e-03 9.999887e-01 1.716275e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.999796e-01 1.566466e-03 -6.198571e-03 -1.406429e-01]
[-1.587952e-03 9.999927e-01 -3.462706e-03 -8.515762e-02]
[ 6.193102e-03 3.472479e-03 9.999747e-01 2.574964e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.999637e-01 2.078471e-03 -8.263498e-03 -1.874858e-01]
[-2.116664e-03 9.999871e-01 -4.615826e-03 -1.135202e-01]
[ 8.253797e-03 4.633149e-03 9.999551e-01 3.432648e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.999433e-01 2.586172e-03 -1.033094e-02 -2.343818e-01]
[-2.645881e-03 9.999798e-01 -5.770163e-03 -1.419150e-01]
[ 1.031581e-02 5.797170e-03 9.999299e-01 4.291335e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.999184e-01 3.088363e-03 -1.239599e-02 -2.812195e-01]
[-3.174350e-03 9.999710e-01 -6.922975e-03 -1.702743e-01]
[ 1.237425e-02 6.961759e-03 9.998991e-01 5.148987e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.998890e-01 3.586305e-03 -1.446384e-02 -3.281178e-01]
[-3.703403e-03 9.999605e-01 -8.077186e-03 -1.986703e-01]
[ 1.443430e-02 8.129853e-03 9.998627e-01 6.007777e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.998551e-01 4.078705e-03 -1.652913e-02 -3.749547e-01]
[-4.231669e-03 9.999484e-01 -9.229794e-03 -2.270290e-01]
[ 1.649063e-02 9.298401e-03 9.998207e-01 6.865477e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.998167e-01 4.566671e-03 -1.859652e-02 -4.218367e-01]
[-4.760342e-03 9.999347e-01 -1.038342e-02 -2.554151e-01]
[ 1.854788e-02 1.047004e-02 9.997731e-01 7.724036e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.997738e-01 5.049868e-03 -2.066463e-02 -4.687329e-01]
[-5.289072e-03 9.999194e-01 -1.153730e-02 -2.838096e-01]
[ 2.060470e-02 1.164399e-02 9.997198e-01 8.582886e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.997264e-01 5.527315e-03 -2.272922e-02 -5.155474e-01]
[-5.816781e-03 9.999025e-01 -1.268908e-02 -3.121547e-01]
[ 2.265686e-02 1.281782e-02 9.996611e-01 9.440275e+00]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.996745e-01 6.000540e-03 -2.479692e-02 -5.624310e-01]
[-6.345160e-03 9.998840e-01 -1.384246e-02 -3.405416e-01]
[ 2.471098e-02 1.399530e-02 9.995966e-01 1.029896e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.996182e-01 6.468772e-03 -2.686440e-02 -6.093087e-01]
[-6.873365e-03 9.998639e-01 -1.499561e-02 -3.689250e-01]
[ 2.676374e-02 1.517453e-02 9.995266e-01 1.115757e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.995562e-01 7.058450e-03 -2.894213e-02 -6.562052e-01]
[-7.530449e-03 9.998399e-01 -1.623192e-02 -3.973964e-01]
[ 2.882292e-02 1.644266e-02 9.994492e-01 1.201541e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.995095e-01 5.595311e-03 -3.081450e-02 -7.018788e-01]
[-6.093682e-03 9.998517e-01 -1.610315e-02 -4.239119e-01]
[ 3.071983e-02 1.628303e-02 9.993953e-01 1.286965e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.994226e-01 7.842768e-03 -3.306167e-02 -7.498241e-01]
[-8.456089e-03 9.997940e-01 -1.845198e-02 -4.540039e-01]
[ 3.291014e-02 1.872089e-02 9.992829e-01 1.373146e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.993587e-01 8.704386e-03 -3.473400e-02 -7.992511e-01]
[-9.439421e-03 9.997338e-01 -2.105422e-02 -4.840770e-01]
[ 3.454149e-02 2.136859e-02 9.991747e-01 1.460026e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.992482e-01 1.235688e-02 -3.674849e-02 -8.546642e-01]
[-1.320676e-02 9.996488e-01 -2.297477e-02 -5.155070e-01]
[ 3.645169e-02 2.344282e-02 9.990604e-01 1.547957e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.991826e-01 1.352279e-02 -3.809783e-02 -9.072868e-01]
[-1.439161e-02 9.996404e-01 -2.262373e-02 -5.464705e-01]
[ 3.777819e-02 2.315352e-02 9.990178e-01 1.636940e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.991241e-01 1.328777e-02 -3.967996e-02 -9.609163e-01]
[-1.412604e-02 9.996813e-01 -2.092052e-02 -5.783595e-01]
[ 3.938933e-02 2.146271e-02 9.989934e-01 1.726896e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.990604e-01 1.351433e-02 -4.118029e-02 -1.011589e+00]
[-1.432436e-02 9.997084e-01 -1.943894e-02 -6.092403e-01]
[ 4.090558e-02 2.001055e-02 9.989626e-01 1.817318e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.990174e-01 1.304822e-02 -4.235654e-02 -1.066256e+00]
[-1.386115e-02 9.997242e-01 -1.895579e-02 -6.387363e-01]
[ 4.209752e-02 1.952427e-02 9.989227e-01 1.908411e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.989383e-01 1.325999e-02 -4.411963e-02 -1.118651e+00]
[-1.413494e-02 9.997084e-01 -1.957870e-02 -6.686219e-01]
[ 4.384715e-02 2.018154e-02 9.988343e-01 1.999722e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.989120e-01 1.152857e-02 -4.518863e-02 -1.171840e+00]
[-1.247777e-02 9.997062e-01 -2.077977e-02 -6.990778e-01]
[ 4.493579e-02 2.132101e-02 9.987623e-01 2.091368e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.988800e-01 1.128892e-02 -4.595003e-02 -1.224279e+00]
[-1.218132e-02 9.997417e-01 -1.918760e-02 -7.287421e-01]
[ 4.572155e-02 1.972584e-02 9.987594e-01 2.184042e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.988593e-01 1.299421e-02 -4.594914e-02 -1.280807e+00]
[-1.370812e-02 9.997896e-01 -1.525610e-02 -7.604717e-01]
[ 4.574123e-02 1.586857e-02 9.988272e-01 2.277432e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.988876e-01 1.379030e-02 -4.509495e-02 -1.334841e+00]
[-1.432558e-02 9.998304e-01 -1.156850e-02 -7.929673e-01]
[ 4.492776e-02 1.220164e-02 9.989157e-01 2.370953e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.989649e-01 1.285173e-02 -4.363580e-02 -1.385747e+00]
[-1.325687e-02 9.998715e-01 -9.007757e-03 -8.209244e-01]
[ 4.351442e-02 9.576906e-03 9.990068e-01 2.465175e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]
[[ 9.990298e-01 1.265253e-02 -4.218321e-02 -1.436633e+00]
[-1.299403e-02 9.998849e-01 -7.831284e-03 -8.456208e-01]
[ 4.207926e-02 8.371815e-03 9.990791e-01 2.559694e+01]
[ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]]
I am doing an ICP in each point cloud I have and then I am taking the transformation matrix of each source and target, and making my array trajectory. but I see that the ground truth has only the first matrix equaling 1 and the rest all starts with 9 and mine is jumping from 1 to 9 all the time
[[[ 9.99990294e-01 -9.79876725e-04 4.74431620e-04 -6.63781000e-03]
[ 9.79861234e-04 9.99990406e-01 3.28838706e-05 5.59895312e-03]
[-4.74463614e-04 -3.24189697e-05 9.99990774e-01 -1.14888461e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00018268e+00 8.14803903e-04 -4.53000232e-04 -1.68058379e-02]
[-8.14816032e-04 1.00018278e+00 -2.65962366e-05 -4.41518765e-04]
[ 4.52978415e-04 2.69652693e-05 1.00018301e+00 -4.50599907e-04]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 9.99845226e-01 -1.98113875e-05 2.74619394e-04 -8.50417373e-03]
[ 1.96509057e-05 9.99845093e-01 5.84279054e-04 -8.79387604e-04]
[-2.74630925e-04 -5.84273635e-04 9.99845056e-01 1.79846462e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 9.99844965e-01 2.57145746e-04 4.18682639e-04 -5.36429085e-03]
[-2.57644879e-04 9.99844341e-01 1.19234806e-03 -1.11984018e-03]
[-4.18375673e-04 -1.19245580e-03 9.99844287e-01 3.66727746e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00038202e+00 2.76087779e-04 3.37802144e-04 -6.13155960e-03]
[-2.76256937e-04 1.00038195e+00 5.01007154e-04 -2.43701146e-03]
[-3.37663819e-04 -5.01100391e-04 1.00038193e+00 1.72724107e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00051256e+00 1.30503950e-04 -5.23983383e-04 -5.60173718e-03]
[-1.31065989e-04 1.00051212e+00 -1.07328758e-03 -8.35482833e-03]
[ 5.23843081e-04 1.07335606e-03 1.00051200e+00 -2.18030897e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00031491e+00 3.05967105e-04 -1.13032591e-04 -2.91990007e-03]
[-3.05962652e-04 1.00031491e+00 3.94262340e-05 -2.74101180e-03]
[ 1.13044645e-04 -3.93916590e-05 1.00031495e+00 4.71574420e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00038083e+00 3.12874119e-04 2.04488306e-04 2.03103468e-03]
[-3.12926920e-04 1.00038082e+00 2.58326907e-04 -1.32311207e-03]
[-2.04407496e-04 -2.58390855e-04 1.00038085e+00 9.50509109e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 9.99996307e-01 3.16230221e-04 3.55686815e-04 1.56106875e-03]
[-3.15987809e-04 9.99996139e-01 -6.81380268e-04 -7.12932823e-03]
[-3.55902189e-04 6.81267797e-04 9.99996125e-01 4.87799679e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00030792e+00 -1.46242291e-04 3.49406989e-04 9.69184039e-04]
[ 1.46417012e-04 1.00030785e+00 -5.00232419e-04 -3.23758223e-03]
[-3.49333809e-04 5.00283527e-04 1.00030780e+00 1.19268204e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00069025e+00 9.45843059e-05 -1.91183889e-04 2.31457942e-03]
[-9.46790008e-05 1.00069015e+00 -4.95701874e-04 -1.87684101e-03]
[ 1.91137011e-04 4.95719951e-04 1.00069014e+00 -2.58417438e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00086013e+00 8.82705138e-05 7.90657503e-04 2.97173553e-03]
[-8.90159236e-05 1.00085999e+00 9.43597733e-04 8.45910549e-04]
[-7.90573928e-04 -9.43667756e-04 1.00085968e+00 1.96582634e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00076542e+00 -9.02622141e-05 1.41725256e-03 4.94193135e-03]
[ 9.01544875e-05 1.00076642e+00 7.61327929e-05 9.95342927e-04]
[-1.41725941e-03 -7.60050425e-05 1.00076542e+00 -5.16527873e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00126338e+00 1.14192275e-04 6.66747484e-04 3.31178934e-03]
[-1.14418718e-04 1.00126354e+00 3.40024696e-04 1.40844775e-03]
[-6.66708662e-04 -3.40100811e-04 1.00126333e+00 -9.41134558e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00069822e+00 -1.73396713e-04 -1.15113528e-04 3.91673690e-03]
[ 1.73260997e-04 1.00069753e+00 -1.17876314e-03 -2.32975542e-03]
[ 1.15317698e-04 1.17874318e-03 1.00069754e+00 -3.15845440e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00096536e+00 2.05189631e-05 9.53534802e-05 1.49264147e-03]
[-2.05062720e-05 1.00096536e+00 -1.33221933e-04 -2.56480487e-03]
[-9.53562103e-05 1.33219979e-04 1.00096536e+00 -3.24515831e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00127178e+00 -2.55537143e-05 5.35495618e-04 1.03115070e-03]
[ 2.61570837e-05 1.00127129e+00 -1.12820602e-03 -2.32347878e-03]
[-5.35466485e-04 1.12821985e-03 1.00127115e+00 -4.01131776e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00038527e+00 3.83049348e-04 -4.13311775e-04 2.34035456e-03]
[-3.83805707e-04 1.00038367e+00 -1.83217834e-03 -6.41354056e-03]
[ 4.12609507e-04 1.83233662e-03 1.00038366e+00 -3.48101996e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00057952e+00 2.91010353e-04 -9.78183854e-04 4.34165320e-03]
[-2.90557322e-04 1.00057989e+00 4.63514314e-04 6.86065524e-04]
[ 9.78318517e-04 -4.63230019e-04 1.00057946e+00 -1.38228691e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00048248e+00 -1.45027908e-04 -1.73677969e-03 1.32524672e-03]
[ 1.47115248e-04 1.00048326e+00 1.20235915e-03 1.09147387e-03]
[ 1.73660412e-03 -1.20261271e-03 1.00048177e+00 -1.29471686e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00032461e+00 1.80959529e-04 -1.87998946e-03 -1.91349808e-03]
[-1.78325431e-04 1.00032539e+00 1.40165445e-03 2.24994895e-04]
[ 1.88024115e-03 -1.40131681e-03 1.00032364e+00 -2.52663491e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00113679e+00 -1.78816598e-04 -1.86672384e-03 -8.09504432e-03]
[ 1.83125725e-04 1.00113586e+00 2.31110326e-03 -9.76751278e-04]
[ 1.86630604e-03 -2.31144066e-03 1.00113414e+00 -3.33325648e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 9.99165338e-01 -2.53744146e-04 -2.34676360e-03 -4.83436625e-03]
[ 2.73346882e-04 9.99133201e-01 8.34959634e-03 1.25203668e-02]
[ 2.34456115e-03 -8.35021506e-03 9.99130483e-01 -9.87975894e-04]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 9.99698005e-01 -2.16002642e-04 -1.63954977e-03 -7.91466922e-03]
[ 2.21958657e-04 9.99692749e-01 3.63230921e-03 4.09689059e-03]
[ 1.63875408e-03 -3.63266827e-03 9.99691429e-01 -6.86969604e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 9.99156645e-01 -3.14720080e-04 -1.28916127e-03 -1.88186353e-03]
[ 3.19916225e-04 9.99149351e-01 4.02902138e-03 5.14401403e-03]
[ 1.28788164e-03 -4.02943060e-03 9.99148571e-01 -8.60493541e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00023543e+00 -6.24236501e-04 -1.26204086e-03 4.49581779e-03]
[ 6.31451535e-04 1.00021983e+00 5.72601936e-03 6.96306821e-03]
[ 1.25844639e-03 -5.72681042e-03 1.00021924e+00 8.21701639e-04]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 9.99924665e-01 1.35111793e-04 -8.15275284e-04 4.49949924e-03]
[-1.31882380e-04 9.99917158e-01 3.95958893e-03 5.75866825e-03]
[ 8.15803913e-04 -3.95948005e-03 9.99916834e-01 -4.43610552e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00001794e+00 4.96205589e-04 -5.98112845e-05 -1.23280619e-03]
[-4.96172402e-04 1.00001779e+00 5.53613659e-04 -2.56423656e-04]
[ 6.00859692e-05 -5.53583914e-04 1.00001791e+00 1.49347691e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 1.00083213e+00 3.53418101e-04 2.19645127e-05 8.16955085e-04]
[-3.53417764e-04 1.00083213e+00 -1.53434743e-05 -3.27813758e-03]
[-2.19699295e-05 1.53357172e-05 1.00083219e+00 1.74476531e-03]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]]
I think my iterations are wrong... I don't know what is doing this to become wrong. If I pass the ICP in source0, source1 I get first matrix. Source1 and source2, I get second matrix. That's what I am doing.
# carrega os cloud_points presentes em um arquivo
def load_points(path):
point_clouds = []
for filename in os.listdir(path):
filepath = os.path.join(path, filename)
points = trimesh.load(filepath).vertices
point_clouds.append(points)
# retorna todos os pontos do espaço 3d em um array
all_points = np.concatenate(point_clouds, axis=0)
return all_points
def correspondence(source, target, limit):
# Cria uma árvore KD a partir da origem
# para encontrar o ponto mais próximo na source ate prox ponto
kd_source = cKDTree(source)
correspondences = []
for i, points in enumerate(target):# itera sobre os pontos
d, source_index = kd_source.query(points, k=1) # procura o ponto mais proximo da origem ate esse
if(d < limit):
correspondences.append((source_index, i))
return correspondences
def transform(X, Y):
X_ = X - np.mean(X, axis=0)
Y_ = Y - np.mean(Y, axis=0)
covariance = np.dot(X_.T, Y_)
U, _, Vt = np.linalg.svd(covariance)
R = np.dot(U, Vt)
scale = np.trace(np.dot(np.dot(R.T, covariance), R)) / np.trace(np.dot(X_.T, X_))
translation = np.mean(Y, axis=0) - scale * np.dot(R, np.mean(X, axis=0))
transformation_matrix = np.eye(4)
# parte superior esquerda (rotação e escala)
transformation_matrix[:3, :3] = scale * R
# coluna da direita (translação)
transformation_matrix[:3, 3] = translation
return transformation_matrix
# (Iterative Closest-Points (ICP))
def estimate_transform(source, target, correspondences, limit):
arr = np.array(correspondences) # pega todas as correspondencias de todos os pontos
length = arr.shape[0]
source_p = source[arr[:, 0]]
target_p = target[arr[:, 1]]
iterations = 1000
limit = 0.1
best_inliers = None
best_transform = None
for i in range(iterations):
# escolhe entre 3 para estimar a matriz de transformação
index = np.random.choice(length, size=3, replace=False)
src_sample = source_p[index]
trg_sample = target_p[index]
# matriz de transformação em cada amostras
matrix = transform(src_sample, trg_sample)
# Modern Robotics, Chapter 3.3.1: Homogeneous Transformation Matrices
# transformacao dos pontos
h_p = np.column_stack((source_p, np.ones(len(source_p)))) #homogenous points
t_p = np.dot(h_p, matrix.T) #transform points multiplicado pela matriz trasnposta
d = np.linalg.norm(target_p - t_p[:, :3], axis=1)
inliers = np.where(d < limit)[0]
if best_inliers is None or len(inliers) > len(best_inliers):
best_inliers = inliers
best_transform = matrix #transform
best_src_points = source_p[best_inliers]
best_dst_points = target_p[best_inliers]
best_transform = transform(best_src_points, best_dst_points) #matrix
return best_transform
trajectory = []
#carrega os dados do arquivo
DIR = '/content/drive/MyDrive/LIDAR/KITTI-Sequence'
subDIR = sorted(os.listdir(DIR))
for i, DIRs in enumerate(subDIR[:-1]): # iterar as subpastas target
source = os.path.join(DIR, DIRs) #começa a partir do primeiro
target = os.path.join(DIR, subDIR[i + 1])
X = load_points(source)
Y = load_points(target)
correspondences = correspondence(X, Y, 0.1)
transform_matrix = estimate_transform(X, Y, correspondences, 0.1)
Y_aligned = np.dot(np.column_stack((Y, np.ones(len(Y)))), transform_matrix.T)[:, :3]
# final trajectory
trajectory.append(transform_matrix)
final_trajectory = np.array(trajectory)