We read every piece of feedback, and take your input very seriously.
To see all available qualifiers, see our documentation.
There was an error while loading. Please reload this page.
1 parent d91afa9 commit c28c66fCopy full SHA for c28c66f
src/benchmark/benchmark_virtual.cpp
@@ -462,10 +462,16 @@ class BALM2
462
delete plvecVoxels[i];
463
plvecVoxels.clear();
464
465
+ // for(int j=1; j<winSize; j++)
466
+ // {
467
+ // x_stats[j].p = x_stats[j].p - x_stats[j].R * x_stats[0].R.transpose() * x_stats[0].p;
468
+ // x_stats[j].R = x_stats[j].R * x_stats[0].R.transpose();
469
+ // }
470
+
471
for(int j=1; j<winSize; j++)
472
{
- x_stats[j].p = x_stats[j].p - x_stats[j].R * x_stats[0].R.transpose() * x_stats[0].p;
- x_stats[j].R = x_stats[j].R * x_stats[0].R.transpose();
473
+ x_stats[j].p = x_stats[0].R.transpose() * (x_stats[j].p - x_stats[0].p);
474
+ x_stats[j].R = x_stats[0].R.transpose() * x_stats[j].R;
475
}
476
477
x_stats[0].R.setIdentity();
0 commit comments