Localization mode result worse than VIO mode result

[amichayfeldman]

I have tried with EuroC dataset to compare between localization mode and VIO mode results.
I export trajectory by load map on maplab console and execute export_trajectory_to_csv command. Than compare trajectory with rpg_traj_evaluation with EuroC ground truth.

For MH3 (and I tried also with different MHs), with VIO mode I got:

and with localization mode:

I can also upload results of others MH, but the bottom line is that according to results, I don't see the improvement of localization mode versus the VIO's results (even got worse results with loc mode).

I followed this issue, but neither I found --export_estimated_pose_to_csv command (I see that export_trajectory_to_csv command just copy "vertirces" column from CSV) nor I got improvement like the whoever opend there the issue.

EDIT:
When I optimized VI maps with lc and optvi (but OFFLINE), I got better results:

So I assume that something is not configured\calibrated with ONLINE localization mode.

EDIT2:
Even when I use VI map from VIO mode and execute lc and optvi, I get good resutls.
So, how it can be explained??? how any vertex position or something else got improve with lc WITHOUT reference map?!?!

[araujokth]

Are you using the map saved after running VIO mode in your LOC mode? One thing you could try to do as well is to load and optimized the full map from your VIO run (unoptimized), and then save it and use the new optimized one when you run in LOC mode afterwards.

I export trajectory by load map on maplab console and execute export_trajectory_to_csv command. Than compare trajectory with rpg_traj_evaluation with EuroC ground truth.

Instead of doing this, you could save the poses to csv during runtime by creating the csv file in the folder specified here https://github.com/ethz-asl/maplab/blob/master/applications/rovioli/src/data-publisher-flow.cc#L19 as it was suggested in the issue you referenced. I could not understand if you have tried this.

[amichayfeldman]

OK.
I tried what @araujokth wrote above, run trajecories comparison but got a huge error.
I assume that it caused by taking the wrong columns from --export_estimated_poses_to_csv output CSV.

Which columns represent the position coordinates?

t_G_M x [m] | t_G_I M [m] | t_G_M z [m] | q_G_M x | q_G_M y | q_G_M z | q_G_M w | p_M_I x [m] | t_G_I M [m] | p_M_I z [m] | q_M_I x | q_M_I y | q_M_I z | q_M_I w ??

Thanks.

`

[araujokth]

Hi, to understand the output you should look into the line where they actually save the data into csv as I mentioned here #250 (comment).

So in https://github.com/ethz-asl/maplab/blob/master/applications/rovioli/src/data-publisher-flow.cc#L134 you have

file_logger->writeDataWithDelimiterAndNewLine(
              kDelimiter,
              aslam::time::nanoSecondsToSeconds(vio_update->timestamp_ns),
              latest_T_G_M_.getPosition(), latest_T_G_M_.getEigenQuaternion(),
              T_M_I.getPosition(), T_M_I.getEigenQuaternion(), has_T_G_M);

so you see that timestamp is followed by the latest_T_G_M_.getPosition() (x, y, z) and then latest_T_G_M_.getEigenQuaternion() (orientation) and so on.

Actually a thing I just noticed is that the T_G_I, which is what you want when you run in loc mode, is not being saved. So what you have to do is to create T_G_I and define it as follows aslam::Transformation T_G_I = latest_T_G_M_ * T_M_I and then save it to CSV by swapping it with the latest_T_G_M for example (so T_G_I.getPosition()). Sorry for missing this but I had made this change a long time ago and had forgotten about it.

[amichayfeldman]

I tried to use as you described, and took the 3 first columns for x,y,z and the next 4 columns for quaternion, but got really bad results.

What is the different between your way and using export_trajectory_to_csv in maplab_console?

[araujokth]

You mean you did the following and got bad results?

           aslam::Transformation T_G_I = latest_T_G_M_ * T_M_I;
           file_logger_ros->writeDataWithDelimiterAndNewLine(
               kDelimiter,
               aslam::time::nanoSecondsToSeconds(vio_update->timestamp_ns),
               T_G_I.getPosition(), T_G_I.getEigenQuaternion());  

So export_trajectory_to_csv outputs T_G_I directly

maplab/console-plugins/vi-map-data-import-export-plugin/src/export-vertex-data.cc

Line 65 in 88d8587

csv_file << timestamp_nanoseconds << kDelimiter << vertex_id.hexString()

. The results should then be the same as with the example I showed you above, if you dont further optimize the map and poses

[araujokth]

Yes for any change you make in the files you always have to build maplab again

[amichayfeldman]

@araujokth What did you change in the export-vertex-data.cc file?

[araujokth]

No, you have to make this change inside this script https://github.com/ethz-asl/maplab/blob/master/applications/rovioli/src/data-publisher-flow.cc#L134 where instead of having this

file_logger->writeDataWithDelimiterAndNewLine(
              kDelimiter,
              aslam::time::nanoSecondsToSeconds(vio_update->timestamp_ns),
              latest_T_G_M_.getPosition(), latest_T_G_M_.getEigenQuaternion(),
              T_M_I.getPosition(), T_M_I.getEigenQuaternion(), has_T_G_M);

you want to have this so you can save to csv the T_G_I:

aslam::Transformation T_G_I = latest_T_G_M_ * T_M_I;
          file_logger->writeDataWithDelimiterAndNewLine(
              kDelimiter,
              aslam::time::nanoSecondsToSeconds(vio_update->timestamp_ns),
              T_G_I.getPosition(), T_G_I.getEigenQuaternion());  

Then the CSV file will be written during runtime as you are running the experiment, to the path you define here https://github.com/ethz-asl/maplab/blob/master/applications/rovioli/src/data-publisher-flow.cc#L19

[amichayfeldman]

@araujokth So I did as you said:

1. I change and write the T_G_I to the csv instead the current T_G_M, as you explained above.

2. I rebuilt maplab.

3. I run the command rosrun rovioli tutorial_euroc_localization ~/maplab_ws/runnings/MH2_VIO_loc_localization runnings_copy2/MH1_with_ref_MH2 dataset/MH_01_easy.bag --export_estimated_poses_to_csv runnings_copy2/MH1_with_ref_MH2_poses.csv,
   while ~/maplab_ws/runnings/MH2_VIO_loc_localization is the loc folder,
   runnings_copy2/MH1_with_ref_MH2 is the target folder
   and dataset/MH_01_easy.bag is the EuroC data bag.

   From the output CSV ( the last argument in the command runnings_copy2/MH1_with_ref_MH2_poses.csv) I got this :
   

4. I also try to run maplab_console, load the map folder and run export_trajectory_to_csv.
   The results are much better, but RMSE is 0.14 [m] (while the paper has reported 0.082 [m]).
   The trajectory looks like:

5. In addition - I tried to restore paper's result on VIO mode (ROVIO). The paper's RMSE is 0.17m, and I got 0.13m. Something is strange here...

[araujokth]

The results you get are very strange. Did you take a look at both CSV files and try to see if in one you maybe have the coordinate system swapped (like the x column has z values, or something like that) or that you are using the right column values?

On the results being different from the paper, one thing you have to consider is that the RMSE or ATE results are obtained after the alignment transformation from the tool you are using, which could totally give you different results even if the trajectory is the same.

Also, it could be that the maplab authors used a previous or older version of the version you are testing which has a slight modification, or the parameters of the config files are slighty different.