Performance Degradation in 'vins_estimator/odometry' Due to 'yaw_diff' Usage in 'double2vector()'

[zhangbb-john]

Description:

Hi, I am a PhD student specializing in robotics. I've observed a performance degradation in the vins_estimator/odometry result, specifically related to the use of yaw_diff in the double2vector() function, although I understand the reasons for use of yaw_diff (https://github.com/HKUST-Aerial-Robotics/VINS-Mono/issues/18). This observation holds true for both publicly available datasets and my own collected data.

Reproduction Steps:

• Maintain the original codebase.
• Evaluate the error of vins_estimator/odometry versus the benchmark using evo_ape.

• Observe and record the degradation in performance. The result for MH05_difficult dataset is

  Aligning using Umeyama's method...
  Rotation of alignment:
  [[  6.74308813e-01  -7.38439821e-01   3.77549211e-03]
  [  7.38449376e-01   6.74302535e-01  -2.93443252e-03]
  [ -3.78922075e-04   4.76672350e-03   9.99988567e-01]]
  Translation of alignment:
  [ 4.22520618 -1.8737507   0.56427183]
  Scale correction: 1.0
  --------------------------------------------------------------------------------
  Compared 1073 absolute pose pairs.
  Calculating APE for translation part pose relation...
  --------------------------------------------------------------------------------
  APE w.r.t. translation part (m)
  (with SE(3) Umeyama alignment)
  
     max      0.461365
    mean      0.327153
  median      0.332055
     min      0.164967
    rmse      0.331708
     sse      118.062321
     std      0.054781

• Modify the original codebase and evaluate the error of vins_estimator/odometry versus the benchmark using evo_ape.

If I change part of the code in estimator.cpp to

    Vector3d origin_R00 = Utility::R2ypr(Quaterniond(para_Pose[0][6],
                                                      para_Pose[0][3],
                                                      para_Pose[0][4],
                                                      para_Pose[0][5]).toRotationMatrix());
    // double y_diff = origin_R0.x() - origin_R00.x();
    double y_diff = 0;
    std::cout << "y_diff is " << y_diff << std::endl;
    //TODO
    Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));
    if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0)
    {
        ROS_DEBUG("euler singular point!");
        rot_diff = Rs[0] * Quaterniond(para_Pose[0][6],
                                       para_Pose[0][3],
                                       para_Pose[0][4],
                                       para_Pose[0][5]).toRotationMatrix().transpose();
    }

the result is

Aligning using Umeyama's method...
Rotation of alignment:
[[  3.57686148e-01  -9.33822824e-01   5.96258294e-03]
 [  9.33840352e-01   3.57689753e-01  -4.86746397e-04]
 [ -1.67821993e-03   5.74220300e-03   9.99982105e-01]]
Translation of alignment:
[ 4.26574792 -1.84399577  0.57479525]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 1075 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)

       max      0.414560
      mean      0.246269
    median      0.238913
       min      0.073168
      rmse      0.255384
       sse      70.112555
       std      0.067619

--------------------------------------------------------------------------------

Discussion:

Upon investigation, it has been noted that excluding the usage of yaw_diff results in improved performance. This observation is consistent not only with publicly available datasets but also holds true for my proprietary data.

Offer for Collaboration:

If anyone is interested in delving deeper into this issue or has insights into the underlying reasons, please feel free to reach out. I welcome collaboration and discussion on this matter. You can contact me at [zhangbb.john@outlook.com].

I am seeking clarification or insights from the community on why the exclusion of yaw_diff leads to enhanced performance. Any explanations or discussions on this topic would be greatly appreciated.