issue in fix T0

[huanglilong]

BALM/src/benchmark/benchmark_virtual.cpp

Line 467 in df9a376

x_stats[j].p = x_stats[j].p - x_stats[j].R * x_stats[0].R.transpose() * x_stats[0].p;

@Zale-Liu actually, x_stats[0].p and x_stats[j].p in global frame, x_stats[j].R is from body frame to global frame
origin code:

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();

fixed code:

x_stats[j].p = x_stats[0].R.transpose() *(x_stats[j].p - x_stats[0].p);
x_stats[j].R = x_stats[0].R.transpose() * x_stats[j].R;

[huanglilong]

thanks for great works! @Zale-Liu
initial pointcloud in global frame with noise poses:

optimized pointcloud in global frame with optimized poses:

[Zale-Liu]

Yes, you are right. It is a bug I forget to fix.