issue in fix T0

[huanglilong]

https://github.com/hku-mars/BALM/blob/df9a376db9f573b4faf0562c11dcc176b685d18d/src/benchmark/benchmark_virtual.cpp#L467 @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.