search

hku-mars / BALM

An efficient and consistent bundle adjustment for lidar mapping
GNU General Public License v2.0
700 stars 151 forks source link

issue in fix T0 #38

Open huanglilong opened 1 year ago

huanglilong commented 1 year ago

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 commented 1 year ago

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

Zale-Liu commented 1 year ago

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